diff --git a/common/interpolation/include/interpolation/spline_interpolation_points_2d.hpp b/common/interpolation/include/interpolation/spline_interpolation_points_2d.hpp index 567a1873aaa5c..653d6c61d05b6 100644 --- a/common/interpolation/include/interpolation/spline_interpolation_points_2d.hpp +++ b/common/interpolation/include/interpolation/spline_interpolation_points_2d.hpp @@ -84,6 +84,7 @@ class SplineInterpolationPoints2d void calcSplineCoefficientsInner(const std::vector & points); SplineInterpolation spline_x_; SplineInterpolation spline_y_; + SplineInterpolation spline_z_; std::vector base_s_vec_; }; diff --git a/common/interpolation/src/spline_interpolation_points_2d.cpp b/common/interpolation/src/spline_interpolation_points_2d.cpp index 82c3841c424c5..848d65dfae575 100644 --- a/common/interpolation/src/spline_interpolation_points_2d.cpp +++ b/common/interpolation/src/spline_interpolation_points_2d.cpp @@ -35,12 +35,13 @@ std::vector calcEuclidDist(const std::vector & x, const std::vec return dist_v; } -std::array, 3> getBaseValues( +std::array, 4> getBaseValues( const std::vector & points) { // calculate x, y std::vector base_x; std::vector base_y; + std::vector base_z; for (size_t i = 0; i < points.size(); i++) { const auto & current_pos = points.at(i); if (i > 0) { @@ -53,16 +54,17 @@ std::array, 3> getBaseValues( } base_x.push_back(current_pos.x); base_y.push_back(current_pos.y); + base_z.push_back(current_pos.z); } // calculate base_keys, base_values - if (base_x.size() < 2 || base_y.size() < 2) { + if (base_x.size() < 2 || base_y.size() < 2 || base_z.size() < 2) { throw std::logic_error("The numbef of unique points is not enough."); } const std::vector base_s = calcEuclidDist(base_x, base_y); - return {base_s, base_x, base_y}; + return {base_s, base_x, base_y, base_z}; } } // namespace @@ -137,10 +139,12 @@ geometry_msgs::msg::Point SplineInterpolationPoints2d::getSplineInterpolatedPoin const double x = spline_x_.getSplineInterpolatedValues({whole_s}).at(0); const double y = spline_y_.getSplineInterpolatedValues({whole_s}).at(0); + const double z = spline_z_.getSplineInterpolatedValues({whole_s}).at(0); geometry_msgs::msg::Point geom_point; geom_point.x = x; geom_point.y = y; + geom_point.z = z; return geom_point; } @@ -226,8 +230,10 @@ void SplineInterpolationPoints2d::calcSplineCoefficientsInner( base_s_vec_ = base.at(0); const auto & base_x_vec = base.at(1); const auto & base_y_vec = base.at(2); + const auto & base_z_vec = base.at(3); // calculate spline coefficients spline_x_ = SplineInterpolation(base_s_vec_, base_x_vec); spline_y_ = SplineInterpolation(base_s_vec_, base_y_vec); + spline_z_ = SplineInterpolation(base_s_vec_, base_z_vec); } diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.py b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.py index de8104af44576..03bf4479c8139 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.py +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.py @@ -48,12 +48,12 @@ def launch_setup(context, *args, **kwargs): obstacle_avoidance_planner_param = yaml.safe_load(f)["/**"]["ros__parameters"] obstacle_avoidance_planner_component = ComposableNode( package="obstacle_avoidance_planner", - plugin="ObstacleAvoidancePlanner", + plugin="obstacle_avoidance_planner::ObstacleAvoidancePlanner", name="obstacle_avoidance_planner", namespace="", remappings=[ - ("~/input/objects", "/perception/object_recognition/objects"), ("~/input/path", LaunchConfiguration("input_path_topic")), + ("~/input/odometry", "/localization/kinematic_state"), ("~/output/path", "obstacle_avoidance_planner/trajectory"), ], parameters=[ diff --git a/planning/obstacle_avoidance_planner/CMakeLists.txt b/planning/obstacle_avoidance_planner/CMakeLists.txt index b1b29a1a5f389..eb7c017af33d6 100644 --- a/planning/obstacle_avoidance_planner/CMakeLists.txt +++ b/planning/obstacle_avoidance_planner/CMakeLists.txt @@ -1,40 +1,52 @@ cmake_minimum_required(VERSION 3.14) project(obstacle_avoidance_planner) +include(CheckCXXCompilerFlag) + +# For Eigen vectorization. +check_cxx_compiler_flag("-march=native" COMPILER_SUPPORTS_MARCH_NATIVE) +if(COMPILER_SUPPORTS_MARCH_NATIVE) + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -march=native") + message(STATUS "Enabling MARCH NATIVE ") +endif() + find_package(autoware_cmake REQUIRED) autoware_package() find_package(Eigen3 REQUIRED) -find_package(OpenCV REQUIRED) ament_auto_add_library(obstacle_avoidance_planner SHARED + # node + src/node.cpp + # core algorithms + src/replan_checker.cpp + src/eb_path_smoother.cpp + src/mpt_optimizer.cpp + src/state_equation_generator.cpp + # debug marker + src/debug_marker.cpp + # vehicle model src/vehicle_model/vehicle_model_interface.cpp src/vehicle_model/vehicle_model_bicycle_kinematics.cpp - src/eb_path_optimizer.cpp - src/mpt_optimizer.cpp - src/utils/utils.cpp - src/utils/debug_utils.cpp - src/node.cpp + # utils + src/utils/trajectory_utils.cpp + src/utils/geometry_utils.cpp ) target_include_directories(obstacle_avoidance_planner SYSTEM PUBLIC - ${OpenCV_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIR} ) -target_link_libraries(obstacle_avoidance_planner - ${OpenCV_LIBRARIES} -) - +# register node rclcpp_components_register_node(obstacle_avoidance_planner - PLUGIN "ObstacleAvoidancePlanner" + PLUGIN "obstacle_avoidance_planner::ObstacleAvoidancePlanner" EXECUTABLE obstacle_avoidance_planner_node ) ament_auto_package( INSTALL_TO_SHARE launch - config scripts + config ) diff --git a/planning/obstacle_avoidance_planner/README.md b/planning/obstacle_avoidance_planner/README.md index 461a657d14a8d..1a03b695aa699 100644 --- a/planning/obstacle_avoidance_planner/README.md +++ b/planning/obstacle_avoidance_planner/README.md @@ -1,32 +1,33 @@ ## Purpose -This package generates a trajectory that is feasible to drive and collision free based on a reference path, drivable area, and static/dynamic obstacles. -Only position and orientation of trajectory are calculated in this module (velocity is just aligned from the one in the path), and velocity or acceleration will be updated in the latter modules. +This package generates a trajectory that is kinematically-feasible to drive and collision-free based on the input path, drivable area. +Only position and orientation of trajectory are updated in this module, and velocity is just taken over from the one in the input path. ## Feature This package is able to -- follow the behavior path smoothly +- make the trajectory smooth - make the trajectory inside the drivable area as much as possible -- insert stop point if its trajectory point is outside the drivable area + - NOTE: Static obstacles to avoid can be removed from the drivable area. +- insert stop point before the planned footprint will be outside the drivable area + +Note that the velocity is just taken over from the input path. ## Inputs / Outputs ### input -| Name | Type | Description | -| ---------------------------------------------------------------------- | ---------------------------------------------- | -------------------------------------------------- | -| `~/input/path` | autoware_auto_planning_msgs/Path | Reference path and the corresponding drivable area | -| `~/input/objects` | autoware_auto_perception_msgs/PredictedObjects | Recognized objects around the vehicle | -| `/localization/kinematic_kinematics` | nav_msgs/Odometry | Current Velocity of ego vehicle | -| `/planning/scenario_planning/lane_driving/obstacle_avoidance_approval` | tier4_planning_msgs/EnableAvoidance | Approval to execute obstacle avoidance | +| Name | Type | Description | +| ------------------ | ------------------------------------ | -------------------------------------------------- | +| `~/input/path` | autoware_auto_planning_msgs/msg/Path | Reference path and the corresponding drivable area | +| `~/input/odometry` | nav_msgs/msg/Odometry | Current Velocity of ego vehicle | ### output -| Name | Type | Description | -| --------------------- | -------------------------------------- | ----------------------------------------------------------------- | -| `~/output/trajectory` | autoware_auto_planning_msgs/Trajectory | Optimized trajectory that is feasible to drive and collision-free | +| Name | Type | Description | +| --------------------- | ------------------------------------------ | ----------------------------------------------------------------- | +| `~/output/trajectory` | autoware_auto_planning_msgs/msg/Trajectory | Optimized trajectory that is feasible to drive and collision-free | ## Flowchart @@ -37,88 +38,81 @@ Flowchart of functions is explained here. title pathCallback start +:isDataReady; + +:createPlannerData; + group generateOptimizedTrajectory - :checkReplan; - if (replanning required?) then (yes) - group getMaps - :getDrivableArea; - :getRoadClearanceMap; - :drawObstacleOnImage; - :getObstacleClearanceMap; - end group - - group optimizeTrajectory + group optimizeTrajectory + :check replan; + if (replanning required?) then (yes) :getEBTrajectory; :getModelPredictiveTrajectory; - if (optimization failed?) then (no) else (yes) :send previous\n trajectory; endif - end group - - :insertZeroVelocityOutsideDrivableArea; - - :publishDebugDataInOptimization; - else (no) - :send previous\n trajectory; - endif + else (no) + :send previous\n trajectory; + endif + end group + + :applyInputVelocity; + :insertZeroVelocityOutsideDrivableArea; + :publishDebugMarkerOfOptimization; end group -group generatePostProcessedTrajectory - :getExtendedOptimizedTrajectory; - :concatTrajectory; - :generateFineTrajectoryPoints; - :alignVelocity; -end group +:extendTrajectory; -:convertToTrajectory; +:setZeroVelocityAfterStopPoint; -:publishDebugDataInMain; +:publishDebugData; stop @enduml ``` -### checkReplan +### createPlannerData -When one of the following conditions are realized, callback function to generate a trajectory is called and publish the trajectory. -Otherwise, previously generated (optimized) trajectory is published just with aligning the velocity from the latest behavior path. +The following data for planning is created. -- Ego moves a certain distance compared to the previous ego pose (default: 3.0 [m]) -- Time passes (default: 1.0 [s]) -- Ego is far from the previously generated trajectory +```cpp +struct PlannerData +{ + // input + Header header; + std::vector traj_points; // converted from the input path + std::vector left_bound; + std::vector right_bound; -### getRoadClearanceMap - -Cost map is generated according to the distance to the road boundaries. - -These cost maps are used in the optimization to generate a collision-free trajectory. - -### drawObstacleOnImage - -Only obstacles that are static and locate in a shoulder lane is decided to avoid. -In detail, this equals to the following three conditions at the same time, and the red obstacles in the figure (id: 3, 4, 5) is to be avoided. + // ego + geometry_msgs::msg::Pose ego_pose; + double ego_vel; +}; +``` -- Velocity is under a certain value (default: 0.1 [m/s]) -- CoG of the obstacles is not on the center line - - so that the ego will not avoid the car in front of the ego in the same lane. -- At least one point of the obstacle polygon is outside the drivable area. +### check replan -![obstacle_to_avoid](./media/obstacle_to_avoid.drawio.svg) +When one of the following conditions are met, trajectory optimization will be executed. +Otherwise, previously optimized trajectory is used with updating the velocity from the latest input path. -### getObstacleClearanceMap +max_path_shape_around_ego_lat_dist -Cost map is generated according to the distance to the target obstacles to be avoided. +- Ego moves longer than `replan.max_ego_moving_dist` in one cycle. (default: 3.0 [m]) + - This is for when the ego pose is set again in the simulation. +- Trajectory's end, which is considered as the goal pose, moves longer than `replan.max_goal_moving_dist` in one cycle. (default: 15.0 [ms]) + - When the goal pose is set again, the planning should be reset. +- Time passes. (default: 1.0 [s]) + - The optimization is skipped for a while sine the optimization is sometimes heavy. +- The input path changes laterally longer than `replan.max_path_shape_around_ego_lat_dist` in one cycle. (default: 2.0) ### getEBTrajectory -The latter optimization (MPT) assumes that the reference path is smooth enough. -Therefore the path from behavior is made smooth here, and send to the optimization as a format of trajectory. -Obstacles are ignored in this function. +The latter optimization (model predictive trajectory) assumes that the reference path is smooth enough. +This function makes the input path smooth by elastic band. -More details can be seen in the Elastic Band section. +More details about elastic band can be seen [here](docs/eb.md). ### getModelPredictiveTrajectory @@ -136,7 +130,7 @@ Some examples are shown in the following figure, and it is shown that the trajec ![mpt_optimization_offset](./media/mpt_optimization_offset.svg) -More details can be seen in the Model Predictive Trajectory section. +More details can be seen [here](docs/mpt.md). ### insertZeroVelocityOutsideDrivableArea @@ -166,561 +160,35 @@ Therefore, we have to make sure that the optimized trajectory is inside the driv Velocity is assigned in the result trajectory from the velocity in the behavior path. The shapes of the trajectory and the path are different, therefore the each nearest trajectory point to the path is searched and interpolated linearly. -## Algorithms - -In this section, Elastic band (to make the path smooth) and Model Predictive Trajectory (to make the trajectory kinematically feasible and collision-free) will be explained in detail. - -### Elastic band - -#### Abstract - -Elastic band smooths the path generated in the behavior. -Since the latter process of optimization uses the curvature and normal vector of the reference path, smoothing should be applied here so that the optimization will be stable. - -This smoothing process does not consider collision. -Therefore the output path may have a collision with road boundaries or obstacles. - -#### Formulation - -We formulate a QP problem minimizing the distance between the previous point and the next point for each point. -Conditions that each point can move to a certain extent are used so that the path will not changed a lot but will be smoother. - -For $k$'th point ($\boldsymbol{p}_k = (x_k, y_k)$), the objective function is as follows. -The beginning and end point are fixed during the optimization. - -$$ -\begin{align} -\ J & = \min \sum_{k=1}^{n-1} ||(\boldsymbol{p}_{k+1} - \boldsymbol{p}_{k}) - (\boldsymbol{p}_{k} - \boldsymbol{p}_{k-1})||^2 \\ -\ & = \min \sum_{k=1}^{n-1} ||\boldsymbol{p}_{k+1} - 2 \boldsymbol{p}_{k} + \boldsymbol{p}_{k-1}||^2 \\ -\ & = \min \sum_{k=1}^{n-1} \{(x_{k+1} - x_k + x_{k-1})^2 + (y_{k+1} - y_k + y_{k-1})^2\} \\ -\ & = \min - \begin{pmatrix} - \ x_0 \\ - \ x_1 \\ - \ x_2 \\ - \vdots \\ - \ x_{n-2}\\ - \ x_{n-1} \\ - \ x_{n} \\ - \ y_0 \\ - \ y_1 \\ - \ y_2 \\ - \vdots \\ - \ y_{n-2}\\ - \ y_{n-1} \\ - \ y_{n} \\ - \end{pmatrix}^T - \begin{pmatrix} - 1 & -2 & 1 & 0 & \dots& \\ - -2 & 5 & -4 & 1 & 0 &\dots \\ - 1 & -4 & 6 & -4 & 1 & \\ - 0 & 1 & -4 & 6 & -4 & \\ - \vdots & 0 & \ddots&\ddots& \ddots \\ - & \vdots & & & \\ - & & & 1 & -4 & 6 & -4 & 1 \\ - & & & & 1 & -4 & 5 & -2 \\ - & & & & & 1 & -2 & 1& \\ - & & & & & & & &1 & -2 & 1 & 0 & \dots& \\ - & & & & & & & &-2 & 5 & -4 & 1 & 0 &\dots \\ - & & & & & & & &1 & -4 & 6 & -4 & 1 & \\ - & & & & & & & &0 & 1 & -4 & 6 & -4 & \\ - & & & & & & & &\vdots & 0 & \ddots&\ddots& \ddots \\ - & & & & & & & & & \vdots & & & \\ - & & & & & & & & & & & 1 & -4 & 6 & -4 & 1 \\ - & & & & & & & & & & & & 1 & -4 & 5 & -2 \\ - & & & & & & & & & & & & & 1 & -2 & 1& \\ - \end{pmatrix} - \begin{pmatrix} - \ x_0 \\ - \ x_1 \\ - \ x_2 \\ - \vdots \\ - \ x_{n-2}\\ - \ x_{n-1} \\ - \ x_{n} \\ - \ y_0 \\ - \ y_1 \\ - \ y_2 \\ - \vdots \\ - \ y_{n-2}\\ - \ y_{n-1} \\ - \ y_{n} \\ - \end{pmatrix} -\end{align} -$$ - -### Model predictive trajectory - -#### Abstract - -Model Predictive Trajectory (MPT) calculates the trajectory that realizes the following conditions. - -- Kinematically feasible for linear vehicle kinematics model -- Collision free with obstacles and road boundaries - -Conditions for collision free is considered to be not hard constraints but soft constraints. -When the optimization failed or the optimized trajectory is not collision free, the output trajectory will be previously generated trajectory. - -Trajectory near the ego must be stable, therefore the condition where trajectory points near the ego are the same as previously generated trajectory is considered, and this is the only hard constraints in MPT. - -#### Vehicle kinematics - -As the following figure, we consider the bicycle kinematics model in the frenet frame to track the reference path. -At time step $k$, we define lateral distance to the reference path, heading angle against the reference path, and steer angle as $y_k$, $\theta_k$, and $\delta_k$ respectively. - -![vehicle_error_kinematics](./media/vehicle_error_kinematics.png) - -Assuming that the commanded steer angle is $\delta_{des, k}$, the kinematics model in the frenet frame is formulated as follows. -We also assume that the steer angle $\delta_k$ is first-order lag to the commanded one. - -$$ -\begin{align} -y_{k+1} & = y_{k} + v \sin \theta_k dt \\ -\theta_{k+1} & = \theta_k + \frac{v \tan \delta_k}{L}dt - \kappa_k v \cos \theta_k dt \\ -\delta_{k+1} & = \delta_k - \frac{\delta_k - \delta_{des,k}}{\tau}dt -\end{align} -$$ - -##### Linearization - -Then we linearize these equations. -$y_k$ and $\theta_k$ are tracking errors, so we assume that those are small enough. -Therefore $\sin \theta_k \approx \theta_k$. - -Since $\delta_k$ is a steer angle, it is not always small. -By using a reference steer angle $\delta_{\mathrm{ref}, k}$ calculated by the reference path curvature $\kappa_k$, we express $\delta_k$ with a small value $\Delta \delta_k$. - -Note that the steer angle $\delta_k$ is within the steer angle limitation $\delta_{\max}$. -When the reference steer angle $\delta_{\mathrm{ref}, k}$ is larger than the steer angle limitation $\delta_{\max}$, and $\delta_{\mathrm{ref}, k}$ is used to linearize the steer angle, $\Delta \delta_k$ is $\Delta \delta_k = \delta - \delta_{\mathrm{ref}, k} = \delta_{\max} - \delta_{\mathrm{ref}, k}$, and the absolute $\Delta \delta_k$ gets larger. -Therefore, we have to apply the steer angle limitation to $\delta_{\mathrm{ref}, k}$ as well. - -$$ -\begin{align} -\delta_{\mathrm{ref}, k} & = \mathrm{clamp}(\arctan(L \kappa_k), -\delta_{\max}, \delta_{\max}) \\ -\delta_k & = \delta_{\mathrm{ref}, k} + \Delta \delta_k, \ \Delta \delta_k \ll 1 \\ -\end{align} -$$ - -$\mathrm{clamp}(v, v_{\min}, v_{\max})$ is a function to convert $v$ to be larger than $v_{\min}$ and smaller than $v_{\max}$. - -Using this $\delta_{\mathrm{ref}, k}$, $\tan \delta_k$ is linearized as follows. - -$$ -\begin{align} -\tan \delta_k & \approx \tan \delta_{\mathrm{ref}, k} + \left.\frac{d \tan \delta}{d \delta}\right|_{\delta = \delta_{\mathrm{ref}, k}} \Delta \delta_k \\ -& = \tan \delta_{\mathrm{ref}, k} + \left.\frac{d \tan \delta}{d \delta}\right|_{\delta = \delta_{\mathrm{ref}, k}} (\delta_{\mathrm{ref}, k} - \delta_k) \\ -& = \tan \delta_{\mathrm{ref}, k} - \frac{\delta_{\mathrm{ref}, k}}{\cos^2 \delta_{\mathrm{ref}, k}} + \frac{1}{\cos^2 \delta_{\mathrm{ref}, k}} \delta_k -\end{align} -$$ - -##### One-step state equation - -Based on the linearization, the error kinematics is formulated with the following linear equations, - -$$ -\begin{align} - \begin{pmatrix} - y_{k+1} \\ - \theta_{k+1} - \end{pmatrix} - = - \begin{pmatrix} - 1 & v dt \\ - 0 & 1 \\ - \end{pmatrix} - \begin{pmatrix} - y_k \\ - \theta_k \\ - \end{pmatrix} - + - \begin{pmatrix} - 0 \\ - \frac{v dt}{L \cos^{2} \delta_{\mathrm{ref}, k}} \\ - \end{pmatrix} - \delta_{k} - + - \begin{pmatrix} - 0 \\ - \frac{v \tan(\delta_{\mathrm{ref}, k}) dt}{L} - \frac{v \delta_{\mathrm{ref}, k} dt}{L \cos^{2} \delta_{\mathrm{ref}, k}} - \kappa_k v dt\\ - \end{pmatrix} -\end{align} -$$ - -which can be formulated as follows with the state $\boldsymbol{x}$, control input $u$ and some matrices, where $\boldsymbol{x} = (y_k, \theta_k)$ - -$$ -\begin{align} - \boldsymbol{x}_{k+1} = A_k \boldsymbol{x}_k + \boldsymbol{b}_k u_k + \boldsymbol{w}_k -\end{align} -$$ - -##### Time-series state equation - -Then, we formulate time-series state equation by concatenating states, control inputs and matrices respectively as - -$$ -\begin{align} - \boldsymbol{x} = A \boldsymbol{x}_0 + B \boldsymbol{u} + \boldsymbol{w} -\end{align} -$$ - -where - -$$ -\begin{align} -\boldsymbol{x} = (\boldsymbol{x}^T_1, \boldsymbol{x}^T_2, \boldsymbol{x}^T_3, \dots, \boldsymbol{x}^T_{n-1})^T \\ -\boldsymbol{u} = (u_0, u_1, u_2, \dots, u_{n-2})^T \\ -\boldsymbol{w} = (\boldsymbol{w}^T_0, \boldsymbol{w}^T_1, \boldsymbol{w}^T_2, \dots, \boldsymbol{w}^T_{n-1})^T. \\ -\end{align} -$$ - -In detail, each matrices are constructed as follows. - -$$ -\begin{align} - \begin{pmatrix} - \boldsymbol{x}_1 \\ - \boldsymbol{x}_2 \\ - \boldsymbol{x}_3 \\ - \vdots \\ - \boldsymbol{x}_{n-1} - \end{pmatrix} - = - \begin{pmatrix} - A_0 \\ - A_1 A_0 \\ - A_2 A_1 A_0\\ - \vdots \\ - \prod\limits_{k=0}^{n-1} A_{k} - \end{pmatrix} - \boldsymbol{x}_0 - + - \begin{pmatrix} - B_0 & 0 & & \dots & 0 \\ - A_0 B_0 & B_1 & 0 & \dots & 0 \\ - A_1 A_0 B_0 & A_0 B_1 & B_2 & \dots & 0 \\ - \vdots & \vdots & & \ddots & 0 \\ - \prod\limits_{k=0}^{n-3} A_k B_0 & \prod\limits_{k=0}^{n-4} A_k B_1 & \dots & A_0 B_{n-3} & B_{n-2} - \end{pmatrix} - \begin{pmatrix} - u_0 \\ - u_1 \\ - u_2 \\ - \vdots \\ - u_{n-2} - \end{pmatrix} - + - \begin{pmatrix} - I & 0 & & \dots & 0 \\ - A_0 & I & 0 & \dots & 0 \\ - A_1 A_0 & A_0 & I & \dots & 0 \\ - \vdots & \vdots & & \ddots & 0 \\ - \prod\limits_{k=0}^{n-3} A_k & \prod\limits_{k=0}^{n-4} A_k & \dots & A_0 & I - \end{pmatrix} - \begin{pmatrix} - \boldsymbol{w}_0 \\ - \boldsymbol{w}_1 \\ - \boldsymbol{w}_2 \\ - \vdots \\ - \boldsymbol{w}_{n-2} - \end{pmatrix} -\end{align} -$$ - -##### Free-boundary-conditioned time-series state equation - -For path planning which does not start from the current ego pose, $\boldsymbol{x}_0$ should be the design variable of optimization. -Therefore, we make $\boldsymbol{u}'$ by concatenating $\boldsymbol{x}_0$ and $\boldsymbol{u}$, and redefine $\boldsymbol{x}$ as follows. - -$$ -\begin{align} - \boldsymbol{u}' & = (\boldsymbol{x}^T_0, \boldsymbol{u}^T)^T \\ - \boldsymbol{x} & = (\boldsymbol{x}^T_0, \boldsymbol{x}^T_1, \boldsymbol{x}^T_2, \dots, \boldsymbol{x}^T_{n-1})^T -\end{align} -$$ - -Then we get the following state equation - -$$ -\begin{align} - \boldsymbol{x}' = B \boldsymbol{u}' + \boldsymbol{w}, -\end{align} -$$ - -which is in detail - -$$ -\begin{align} - \begin{pmatrix} - \boldsymbol{x}_0 \\ - \boldsymbol{x}_1 \\ - \boldsymbol{x}_2 \\ - \boldsymbol{x}_3 \\ - \vdots \\ - \boldsymbol{x}_{n-1} - \end{pmatrix} - = - \begin{pmatrix} - I & 0 & \dots & & & 0 \\ - A_0 & B_0 & 0 & & \dots & 0 \\ - A_1 A_0 & A_0 B_0 & B_1 & 0 & \dots & 0 \\ - A_2 A_1 A_0 & A_1 A_0 B_0 & A_0 B_1 & B_2 & \dots & 0 \\ - \vdots & \vdots & \vdots & & \ddots & 0 \\ - \prod\limits_{k=0}^{n-1} A_k & \prod\limits_{k=0}^{n-3} A_k B_0 & \prod\limits_{k=0}^{n-4} A_k B_1 & \dots & A_0 B_{n-3} & B_{n-2} - \end{pmatrix} - \begin{pmatrix} - \boldsymbol{x}_0 \\ - u_0 \\ - u_1 \\ - u_2 \\ - \vdots \\ - u_{n-2} - \end{pmatrix} - + - \begin{pmatrix} - 0 & \dots & & & 0 \\ - I & 0 & & \dots & 0 \\ - A_0 & I & 0 & \dots & 0 \\ - A_1 A_0 & A_0 & I & \dots & 0 \\ - \vdots & \vdots & & \ddots & 0 \\ - \prod\limits_{k=0}^{n-3} A_k & \prod\limits_{k=0}^{n-4} A_k & \dots & A_0 & I - \end{pmatrix} - \begin{pmatrix} - \boldsymbol{w}_0 \\ - \boldsymbol{w}_1 \\ - \boldsymbol{w}_2 \\ - \vdots \\ - \boldsymbol{w}_{n-2} - \end{pmatrix}. -\end{align} -$$ - -#### Objective function - -The objective function for smoothing and tracking is shown as follows, which can be formulated with value function matrices $Q, R$. - -$$ -\begin{align} -J_1 (\boldsymbol{x}', \boldsymbol{u}') & = w_y \sum_{k} y_k^2 + w_{\theta} \sum_{k} \theta_k^2 + w_{\delta} \sum_k \delta_k^2 + w_{\dot{\delta}} \sum_k \dot{\delta}_k^2 + w_{\ddot{\delta}} \sum_k \ddot{\delta}_k^2 \\ -& = \boldsymbol{x}'^T Q \boldsymbol{x}' + \boldsymbol{u}'^T R \boldsymbol{u}' \\ -& = \boldsymbol{u}'^T H \boldsymbol{u}' + \boldsymbol{u}'^T \boldsymbol{f} -\end{align} -$$ - -As mentioned before, the constraints to be collision free with obstacles and road boundaries are formulated to be soft constraints. -Assuming that the lateral distance to the road boundaries or obstacles from the back wheel center, front wheel center, and the point between them are $y_{\mathrm{base}, k}, y_{\mathrm{top}, k}, y_{\mathrm{mid}, k}$ respectively, and slack variables for each point are $\lambda_{\mathrm{base}}, \lambda_{\mathrm{top}}, \lambda_{\mathrm{mid}}$, the soft constraints can be formulated as follows. - -$$ -y_{\mathrm{base}, k, \min} - \lambda_{\mathrm{base}, k} \leq y_{\mathrm{base}, k} (y_k) \leq y_{\mathrm{base}, k, \max} + \lambda_{\mathrm{base}, k}\\ -y_{\mathrm{top}, k, \min} - \lambda_{\mathrm{top}, k} \leq y_{\mathrm{top}, k} (y_k) \leq y_{\mathrm{top}, k, \max} + \lambda_{\mathrm{top}, k}\\ -y_{\mathrm{mid}, k, \min} - \lambda_{\mathrm{mid}, k} \leq y_{\mathrm{mid}, k} (y_k) \leq y_{\mathrm{mid}, k, \max} + \lambda_{\mathrm{mid}, k} \\ -0 \leq \lambda_{\mathrm{base}, k} \\ -0 \leq \lambda_{\mathrm{top}, k} \\ -0 \leq \lambda_{\mathrm{mid}, k} -$$ - -Since $y_{\mathrm{base}, k}, y_{\mathrm{top}, k}, y_{\mathrm{mid}, k}$ is formulated as a linear function of $y_k$, the objective function for soft constraints is formulated as follows. - -$$ -\begin{align} -J_2 & (\boldsymbol{\lambda}_\mathrm{base}, \boldsymbol{\lambda}_\mathrm{top}, \boldsymbol {\lambda}_\mathrm{mid})\\ -& = w_{\mathrm{base}} \sum_{k} \lambda_{\mathrm{base}, k} + w_{\mathrm{mid}} \sum_k \lambda_{\mathrm{mid}, k} + w_{\mathrm{top}} \sum_k \lambda_{\mathrm{top}, k} -\end{align} -$$ - -Slack variables are also design variables for optimization. -We define a vector $\boldsymbol{v}$, that concatenates all the design variables. - -$$ -\begin{align} -\boldsymbol{v} = -\begin{pmatrix} - \boldsymbol{u}'^T & \boldsymbol{\lambda}_\mathrm{base}^T & \boldsymbol{\lambda}_\mathrm{top}^T & \boldsymbol{\lambda}_\mathrm{mid}^T -\end{pmatrix}^T -\end{align} -$$ - -The summation of these two objective functions is the objective function for the optimization problem. - -$$ -\begin{align} -\min_{\boldsymbol{v}} J (\boldsymbol{v}) = \min_{\boldsymbol{v}} J_1 (\boldsymbol{u}') + J_2 (\boldsymbol{\lambda}_\mathrm{base}, \boldsymbol{\lambda}_\mathrm{top}, \boldsymbol{\lambda}_\mathrm{mid}) -\end{align} -$$ - -As mentioned before, we use hard constraints where some trajectory points in front of the ego are the same as the previously generated trajectory points. -This hard constraints is formulated as follows. - -$$ -\begin{align} -\delta_k = \delta_{k}^{\mathrm{prev}} (0 \leq i \leq N_{\mathrm{fix}}) -\end{align} -$$ - -Finally we transform those objective functions to the following QP problem, and solve it. - -$$ -\begin{align} -\min_{\boldsymbol{v}} \ & \frac{1}{2} \boldsymbol{v}^T \boldsymbol{H} \boldsymbol{v} + \boldsymbol{f} \boldsymbol{v} \\ -\mathrm{s.t.} \ & \boldsymbol{b}_{lower} \leq \boldsymbol{A} \boldsymbol{v} \leq \boldsymbol{b}_{upper} -\end{align} -$$ - -#### Constraints - -##### Steer angle limitation - -Steer angle has a certain limitation ($\delta_{max}$, $\delta_{min}$). -Therefore we add linear inequality equations. - -$$ -\begin{align} -\delta_{min} \leq \delta_i \leq \delta_{max} -\end{align} -$$ - -##### Collision free - -To realize collision-free path planning, we have to formulate constraints that the vehicle is inside the road (moreover, a certain meter far from the road boundary) and does not collide with obstacles in linear equations. -For linearity, we chose a method to approximate the vehicle shape with a set of circles, that is reliable and easy to implement. - -Now we formulate the linear constraints where a set of circles on each trajectory point is collision-free. -For collision checking, we have a drivable area in the format of an image where walls or obstacles are filled with a color. -By using this drivable area, we calculate upper (left) and lower (right) boundaries along reference points so that we can interpolate boundaries on any position on the trajectory. - -Assuming that upper and lower boundaries are $b_l$, $b_u$ respectively, and $r$ is a radius of a circle, lateral deviation of the circle center $y'$ has to be - -$$ -b_l + r \leq y' \leq b_u - r. -$$ - -Based on the following figure, $y'$ can be formulated as follows. - -$$ -\begin{align} -y' & = L \sin(\theta + \beta) + y \cos \beta - l \sin(\gamma - \phi_a) \\ -& = L \sin \theta \cos \beta + L \cos \theta \sin \beta + y \cos \beta - l \sin(\gamma - \phi_a) \\ -& \approx L \theta \cos \beta + L \sin \beta + y \cos \beta - l \sin(\gamma - \phi_a) -\end{align} -$$ - -$$ -b_l + r - \lambda \leq y' \leq b_u - r + \lambda. -$$ - -$$ -\begin{align} -y' & = C_1 \boldsymbol{x} + C_2 \\ -& = C_1 (B \boldsymbol{v} + \boldsymbol{w}) + C_2 \\ -& = C_1 B \boldsymbol{v} + \boldsymbol{w} + C_2 -\end{align} -$$ - -Note that longitudinal position of the circle center and the trajectory point to calculate boundaries are different. -But each boundaries are vertical against the trajectory, resulting in less distortion by the longitudinal position difference since road boundaries does not change so much. -For example, if the boundaries are not vertical against the trajectory and there is a certain difference of longitudinal position between the circe center and the trajectory point, we can easily guess that there is much more distortion when comparing lateral deviation and boundaries. - -$$ -\begin{align} - A_{blk} & = - \begin{pmatrix} - C_1 B & O & \dots & O & I_{N_{ref} \times N_{ref}} & O \dots & O\\ - -C_1 B & O & \dots & O & I & O \dots & O\\ - O & O & \dots & O & I & O \dots & O - \end{pmatrix} - \in \boldsymbol{R}^{3 N_{ref} \times D_v + N_{circle} N_{ref}} \\ - \boldsymbol{b}_{lower, blk} & = - \begin{pmatrix} - \boldsymbol{b}_{lower} - C_1 \boldsymbol{w} - C_2 \\ - -\boldsymbol{b}_{upper} + C_1 \boldsymbol{w} + C_2 \\ - O - \end{pmatrix} - \in \boldsymbol{R}^{3 N_{ref}} \\ - \boldsymbol{b}_{upper, blk} & = \boldsymbol{\infty} - \in \boldsymbol{R}^{3 N_{ref}} -\end{align} -$$ - -We will explain options for optimization. - -###### L-infinity optimization - -The above formulation is called L2 norm for slack variables. -Instead, if we use L-infinity norm where slack variables are shared by enabling `l_inf_norm`. - -$$ -\begin{align} - A_{blk} = - \begin{pmatrix} - C_1 B & I_{N_{ref} \times N_{ref}} \\ - -C_1 B & I \\ - O & I - \end{pmatrix} -\in \boldsymbol{R}^{3N_{ref} \times D_v + N_{ref}} -\end{align} -$$ - -###### Two-step soft constraints - -$$ -\begin{align} -\boldsymbol{v}' = - \begin{pmatrix} - \boldsymbol{v} \\ - \boldsymbol{\lambda}^{soft_1} \\ - \boldsymbol{\lambda}^{soft_2} \\ - \end{pmatrix} - \in \boldsymbol{R}^{D_v + 2N_{slack}} -\end{align} -$$ - -$*$ depends on whether to use L2 norm or L-infinity optimization. - -$$ -\begin{align} - A_{blk} & = - \begin{pmatrix} - A^{soft_1}_{blk} \\ - A^{soft_2}_{blk} \\ - \end{pmatrix}\\ - & = - \begin{pmatrix} - C_1^{soft_1} B & & \\ - -C_1^{soft_1} B & \Huge{*} & \Huge{O} \\ - O & & \\ - C_1^{soft_2} B & & \\ - -C_1^{soft_2} B & \Huge{O} & \Huge{*} \\ - O & & - \end{pmatrix} - \in \boldsymbol{R}^{6 N_{ref} \times D_v + 2 N_{slack}} -\end{align} -$$ - -$N_{slack}$ is $N_{circle}$ when L2 optimization, or $1$ when L-infinity optimization. -$N_{circle}$ is the number of circles to check collision. - -## Tuning - -### Vehicle - -- max steering wheel degree - - `mpt.kinematics.max_steer_deg` - -### Boundary search - -- `advanced.mpt.bounds_search_widths` - - In order to efficiently search precise lateral boundaries on each trajectory point, different resolutions of search widths are defined. - - By default, [0.45, 0.15, 0.05, 0.01] is used. In this case, the goal is to get the boundaries' length on each trajectory point with 0.01 [m] resolution. - - Firstly, lateral boundaries are searched with a rough resolution (= 0.45 [m]). - - Then, within its 0.45 [m] resolution which boundaries are inside, they are searched again with a bit precise resolution (= 0.15 [m]). - - Following this rule, finally boundaries with 0.01 [m] will be found. - -### Assumptions - -- EB optimized trajectory length should be longer than MPT optimized trajectory length - - since MPT result may be jerky because of non-fixed reference path (= EB optimized trajectory) - - At least, EB fixed optimized trajectory length must be longer than MPT fixed optimization trajectory length - - This causes the case that there is a large difference between MPT fixed optimized point and MPT optimized point just after the point. +## Limitation + +- Computation cost is sometimes high. +- Because of the approximation such as linearization, some narrow roads cannot be run by the planner. +- Roles of planning for `behavior_path_planner` and `obstacle_avoidance_planner` are not decided clearly. Both can avoid obstacles. + +## Comparison to other methods + +Trajectory planning problem that satisfies kinematically-feasibility and collision-free has two main characteristics that makes hard to be solved: one is non-convex and the other is high dimension. +Based on the characteristics, we investigate pros/cons of the typical planning methods: optimization-based, sampling-based, and learning-based method. + +### Optimization-based method + +- pros: comparatively fast against high dimension by leveraging the gradient descent +- cons: often converge to the local minima in the non-convex problem + +### Sampling-based method + +- pros: realize global optimization +- cons: high computation cost especially in the complex case + +### Learning-based method + +- under research yet + +Based on these pros/cons, we chose the optimization-based planner first. +Although it has a cons to converge to the local minima, it can get a good solution by the preprocessing to approximate the problem to convex that almost equals to the original non-convex problem. + +## How to Tune Parameters ### Drivability in narrow roads @@ -737,7 +205,7 @@ $N_{circle}$ is the number of circles to check collision. - Loose EB optimization - - 1. make `eb.common.delta_arc_length_for_eb` large and `eb.common.num_sampling_points_for_eb` small + - 1. make `eb.common.delta_arc_length` large and `eb.common.num_points` small - This makes the number of design variables smaller - Be careful about the trajectory length between MPT and EB as shown in Assumptions. - However, empirically this causes large turn at the corner (e.g. The vehicle turns a steering wheel to the opposite side (=left) a bit just before the corner turning to right) @@ -770,44 +238,16 @@ $N_{circle}$ is the number of circles to check collision. ### Other options -- `option.skip_optimization` skips EB and MPT optimization. -- `option.enable_pre_smoothing` enables EB which is smoothing the trajectory for MPT. +- `option.enable_skip_optimization` skips EB and MPT optimization. +- `option.enable_smoothing` enables EB which is smoothing the trajectory for MPT. - EB is not required if the reference path for MPT is smooth enough and does not change its shape suddenly -- `option.is_showing_calculation_time` enables showing each calculation time for functions and total calculation time on the terminal. -- `option.is_stopping_if_outside_drivable_area` enables stopping just before the generated trajectory point will be outside the drivable area. +- `option.enable_calculation_time_info` enables showing each calculation time for functions and total calculation time on the terminal. +- `option.enable_outside_drivable_area_stop` enables stopping just before the generated trajectory point will be outside the drivable area. - `mpt.option.plan_from_ego` enables planning from the ego pose when the ego's velocity is zero. - `mpt.option.max_plan_from_ego_length` maximum length threshold to plan from ego. it is enabled when the length of trajectory is shorter than this value. - `mpt.option.two_step_soft_constraint` enables two step of soft constraints for collision free - `mpt.option.soft_clearance_from_road` and `mpt.option.soft_second_clearance_from_road` are the weight. -## Limitation - -- When turning right or left in the intersection, the output trajectory is close to the outside road boundary. -- Roles of planning for behavior_path_planner and obstacle_avoidance_planner are not decided clearly. -- High computation cost - -## Comparison to other methods - -Planning a trajectory that satisfies kinematic feasibility and collision-free has two main characteristics that makes hard to be solved: one is non-convex and the other is high dimension. -According to the characteristics, we investigate pros and cons of the typical planning methods: optimization-based, sampling-based, and learning-based method. - -### Optimization-based method - -- pros: comparatively fast against high dimension by leveraging the gradient descent -- cons: often converge to the local minima in the non-convex problem - -### Sampling-based method - -- pros: realize global optimization -- cons: high computation cost especially in the complex case - -### Learning-based method - -under research yet - -Based on these pros/cons, we chose the optimization-based planner first. -Although it has a cons to converge to the local minima, it can get a good solution by the preprocessing to approximate the convex problem that almost equals to the original non-convex problem. - -# Debug +## How To Debug -Debug information are written [here](debug.md). +How to debug can be seen [here](docs/debug.md). diff --git a/planning/obstacle_avoidance_planner/config/obstacle_avoidance_planner.param.yaml b/planning/obstacle_avoidance_planner/config/obstacle_avoidance_planner.param.yaml index c2aab909e8b6c..334da22dc27c0 100644 --- a/planning/obstacle_avoidance_planner/config/obstacle_avoidance_planner.param.yaml +++ b/planning/obstacle_avoidance_planner/config/obstacle_avoidance_planner.param.yaml @@ -1,166 +1,140 @@ /**: ros__parameters: option: - # publish - is_publishing_debug_visualization_marker: true - is_publishing_clearance_map: false # publish clearance map as nav_msgs::OccupancyGrid - is_publishing_object_clearance_map: false # publish clearance map as nav_msgs::OccupancyGrid - is_publishing_area_with_objects: false # publish occupancy map as nav_msgs::OccupancyGrid + enable_smoothing: true # enable path smoothing by elastic band + enable_skip_optimization: false # skip elastic band and model predictive trajectory + enable_reset_prev_optimization: false # If true, optimization has no fix constraint to the previous result. + enable_outside_drivable_area_stop: true # stop if the ego's trajectory footprint is outside the drivable area + use_footprint_polygon_for_outside_drivable_area_check: false # If false, only the footprint's corner points are considered. - is_stopping_if_outside_drivable_area: true # stop if the ego's footprint will be outside the drivable area + debug: + # flag to publish + enable_pub_debug_marker: true # publish debug marker - # show - is_showing_debug_info: false - is_showing_calculation_time: false - - # other - enable_avoidance: false # enable avoidance function - enable_pre_smoothing: true # enable EB - skip_optimization: false # skip MPT and EB - reset_prev_optimization: false - is_considering_footprint_edges: false # consider ego footprint edges to decide whether footprint is outside drivable area + # flag to show + enable_debug_info: false + enable_calculation_time_info: false common: - # sampling - num_sampling_points: 100 # number of optimizing points - - # trajectory total/fixing length - trajectory_length: 300.0 # total trajectory length[m] - - forward_fixing_min_distance: 1.0 # number of fixing points around ego vehicle [m] - forward_fixing_min_time: 0.5 # forward fixing time with current velocity [s] + # output + output_delta_arc_length: 0.5 # delta arc length for output trajectory [m] + output_backward_traj_length: 5.0 # backward length for backward trajectory from base_link [m] - backward_fixing_distance: 5.0 # backward fixing length from base_link [m] - delta_arc_length_for_trajectory: 0.1 # delta arc length for trajectory [m] + # replanning & trimming trajectory param outside algorithm + replan: + max_path_shape_around_ego_lat_dist: 2.0 # threshold of path shape change around ego [m] + max_ego_moving_dist: 5.0 # threshold of ego's moving distance for replan [m] + # make max_goal_moving_dist long to keep start point fixed for pull over + max_goal_moving_dist: 15.0 # threshold of goal's moving distance for replan [m] + max_delta_time_sec: 1.0 # threshold of delta time for replan [second] + + # eb param + eb: + option: + enable_warm_start: true + enable_optimization_validation: false - delta_dist_threshold_for_closest_point: 3.0 # delta dist thres for closest point [m] - delta_yaw_threshold_for_closest_point: 1.046 #M_PI/3.0, delta yaw thres for closest point - delta_yaw_threshold_for_straight: 0.02 # delta dist thres for straight point + common: + num_points: 100 # number of points for optimization [-] + delta_arc_length: 1.0 # delta arc length for optimization [m] - num_fix_points_for_extending: 50 # number of fixing points when extending - max_dist_for_extending_end_point: 0.0001 # minimum delta dist thres for extending last point [m] + clearance: + num_joint_points: 3 # number of joint points (joint means connecting fixing and smoothing) - enable_clipping_fixed_traj: false - non_fixed_trajectory_length: 5.0 # length of the trajectory merging optimized mpt trajectory to original(not optimized) trajectory + clearance_for_fix: 0.0 # maximum optimizing range when applying fixing + clearance_for_joint: 0.1 # maximum optimizing range when applying jointing + clearance_for_smooth: 0.1 # maximum optimizing range when applying smoothing - vehicle_stop_margin_outside_drivable_area: 1.5 # vehicle stop margin to let the vehicle stop before the calculated stop point if it is calculated outside the drivable area [m] . - # This margin will be realized with delta_arc_length_for_mpt_points m precision. + weight: + smooth_weight: 1.0 + lat_error_weight: 0.001 - object: # avoiding object - max_avoiding_objects_velocity_ms: 0.5 # maximum velocity for avoiding objects [m/s] - max_avoiding_ego_velocity_ms: 6.0 # maximum ego velocity when avoiding objects [m/s] + qp: + max_iteration: 10000 # max iteration when solving QP + eps_abs: 1.0e-7 # eps abs when solving OSQP + eps_rel: 1.0e-7 # eps rel when solving OSQP - avoiding_object_type: - unknown: true - car: true - truck: true - bus: true - bicycle: true - motorbike: true - pedestrian: true - animal: true + validation: # if enable_optimization_validation is true + max_error: 3.0 # [m] # mpt param mpt: option: - steer_limit_constraint: true - fix_points_around_ego: true - plan_from_ego: true - max_plan_from_ego_length: 10.0 + # TODO(murooka) enable the following. Currently enabling the flag makes QP so heavy + steer_limit_constraint: false # true visualize_sampling_num: 1 - enable_manual_warm_start: true - enable_warm_start: true # false - is_fixed_point_single: false + enable_manual_warm_start: false + enable_warm_start: true + enable_optimization_validation: false common: - num_curvature_sampling_points: 5 # number of sampling points when calculating curvature - delta_arc_length_for_mpt_points: 0.5 # delta arc length when generating mpt [m] + num_points: 100 # number of points for optimization [-] + delta_arc_length: 1.0 # delta arc length for optimization [m] # kinematics: # If this parameter is commented out, the parameter is set as below by default. - # The logic could be `optimization_center_offset = vehicle_info.wheel_base * 0.8` + # The logic could be `optimization_center_offset = vehicle_info.wheelbase * 0.8` # The 0.8 scale is adopted as it performed the best. # optimization_center_offset: 2.3 # optimization center offset from base link - # replanning & trimming trajectory param outside algorithm - replan: - max_path_shape_change_dist: 0.3 # threshold of path shape change from behavior path [m] - max_ego_moving_dist_for_replan: 3.0 # threshold of ego's moving distance for replan [m] - max_delta_time_sec_for_replan: 1.0 # threshold of delta time for replan [second] - - # advanced parameters to improve performance as much as possible - advanced: - eb: - common: - num_joint_buffer_points: 3 # number of joint buffer points - num_offset_for_begin_idx: 2 # number of shifting points for beginning non-fix idx - delta_arc_length_for_eb: 0.6 # 1.0 # delta arc length when optimizing[m] When the value is around 1.0, overshoot at the corner happens. - num_sampling_points_for_eb: 95 # number of optimizing points # must be longer than mpt points - - clearance: - clearance_for_straight_line: 0.05 # minimum optimizing range around straight points - clearance_for_joint: 0.1 # minimum optimizing range around joint points - clearance_for_only_smoothing: 0.1 # minimum optimizing range when applying only smoothing - - qp: - max_iteration: 10000 # max iteration when solving QP - eps_abs: 1.0e-8 # eps abs when solving OSQP - eps_rel: 1.0e-10 # eps rel when solving OSQP - - mpt: - bounds_search_widths: [0.45, 0.15, 0.05, 0.01] - - clearance: # clearance(distance) between vehicle and roads/objects when generating trajectory - hard_clearance_from_road: 0.0 # clearance from road boundary[m] - soft_clearance_from_road: 0.1 # clearance from road boundary[m] - soft_second_clearance_from_road: 1.0 # clearance from road boundary[m] - clearance_from_object: 1.0 # clearance from object[m] - extra_desired_clearance_from_road: 0.0 # extra desired clearance from road + clearance: # clearance(distance) between vehicle and roads/objects when generating trajectory + # if collision_free_constraints.option.hard_constraint is true + hard_clearance_from_road: 0.0 # clearance from road boundary[m] + # if collision_free_constraints.option.soft_constraint is true + soft_clearance_from_road: 0.1 # clearance from road boundary[m] + + # weight parameter for optimization + weight: + # collision free + soft_collision_free_weight: 1000.0 # soft weight for lateral error around the middle point + + # tracking error + lat_error_weight: 1.0 # weight for lateral error + yaw_error_weight: 0.0 # weight for yaw error + yaw_error_rate_weight: 0.0 # weight for yaw error rate + steer_input_weight: 1.0 # weight for steering input + steer_rate_weight: 1.0 # weight for steering rate + + terminal_lat_error_weight: 100.0 # weight for lateral error at terminal point + terminal_yaw_error_weight: 100.0 # weight for yaw error at terminal point + goal_lat_error_weight: 1000.0 # weight for lateral error at path end point + goal_yaw_error_weight: 1000.0 # weight for yaw error at path end point + + # avoidance + avoidance: + max_avoidance_cost: 0.5 # [m] + avoidance_cost_margin: 0.0 # [m] + avoidance_cost_band_length: 5.0 # [m] + avoidance_cost_decrease_rate: 0.05 # decreased cost per point interval weight: - soft_avoidance_weight: 1000.0 # slack weight for lateral error around the middle point - soft_second_avoidance_weight: 100.0 # slack weight for lateral error around the middle point - - lat_error_weight: 100.0 # weight for lateral error - yaw_error_weight: 0.0 # weight for yaw error - yaw_error_rate_weight: 0.0 # weight for yaw error rate - steer_input_weight: 10.0 # weight for steering input - steer_rate_weight: 10.0 # weight for steering rate - - obstacle_avoid_lat_error_weight: 3.0 # weight for lateral error - obstacle_avoid_yaw_error_weight: 0.0 # weight for yaw error - obstacle_avoid_steer_input_weight: 1000.0 # weight for yaw error - near_objects_length: 30.0 # weight for yaw error - - terminal_lat_error_weight: 100.0 # weight for lateral error at terminal point - terminal_yaw_error_weight: 100.0 # weight for yaw error at terminal point - terminal_path_lat_error_weight: 1000.0 # weight for lateral error at path end point - terminal_path_yaw_error_weight: 1000.0 # weight for yaw error at path end point - - # check if planned trajectory is outside drivable area - collision_free_constraints: - option: - l_inf_norm: true - soft_constraint: true - hard_constraint: false - # two_step_soft_constraint: false - - vehicle_circles: - method: "rear_drive" - - uniform_circle: - num: 3 - radius_ratio: 0.8 - - fitting_uniform_circle: - num: 3 # must be greater than 1 - - rear_drive: - num_for_calculation: 3 - front_radius_ratio: 1.0 - rear_radius_ratio: 1.0 - - bicycle_model: - num_for_calculation: 3 - front_radius_ratio: 1.0 - rear_radius_ratio: 1.0 + lat_error_weight: 0.0 # weight for lateral error + yaw_error_weight: 10.0 # weight for yaw error + steer_input_weight: 100.0 # weight for yaw error + + # collision free constraint for optimization + collision_free_constraints: + option: + l_inf_norm: true + soft_constraint: true + hard_constraint: false + + # how to represent footprint as circles + vehicle_circles: + method: "fitting_uniform_circle" + + bicycle_model: + num_for_calculation: 3 + front_radius_ratio: 1.0 + rear_radius_ratio: 1.0 + + uniform_circle: + num: 3 + radius_ratio: 1.0 + + fitting_uniform_circle: + num: 3 + + validation: # if enable_optimization_validation is true + max_lat_error: 5.0 # [m] + max_yaw_error: 1.046 # [rad] diff --git a/planning/obstacle_avoidance_planner/debug.md b/planning/obstacle_avoidance_planner/docs/debug.md similarity index 91% rename from planning/obstacle_avoidance_planner/debug.md rename to planning/obstacle_avoidance_planner/docs/debug.md index 26d9f3b9c6954..4c8b6eb910bcc 100644 --- a/planning/obstacle_avoidance_planner/debug.md +++ b/planning/obstacle_avoidance_planner/docs/debug.md @@ -8,50 +8,50 @@ Topics for debugging will be explained in this section. - Drivable area to cover the road. Whether this area is continuous and covers the road can be checked. - `/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/drivable_area`, whose type is `nav_msgs/msg/OccupancyGrid` -![drivable_area](./media/drivable_area.png) +![drivable_area](../media/drivable_area.png) - **Path from behavior** - The input path of obstacle_avoidance_planner. Whether this path is continuous and the curvature is not so high can be checked. - `Path` or `PathFootprint` rviz plugin. -![behavior_path](./media/behavior_path.png) +![behavior_path](../media/behavior_path.png) - **EB trajectory** - The output trajectory of elastic band. Whether this trajectory is very smooth and a sampling width is constant can be checked. - `Trajectory` or `TrajectoryFootprint` rviz plugin. -![eb_traj](./media/eb_traj.png) +![eb_traj](../media/eb_traj.png) - **MPT reference trajectory** - The reference trajectory of model predictive trajectory. Whether this trajectory is very smooth and a sampling width is constant can be checked. - `Trajectory` or `TrajectoryFootprint` rviz plugin. -![mpt_ref_traj](./media/mpt_ref_traj.png) +![mpt_ref_traj](../media/mpt_ref_traj.png) - **MPT fixed trajectory** - The fixed trajectory around the ego of model predictive trajectory. - `Trajectory` or `TrajectoryFootprint` rviz plugin. -![mpt_fixed_traj](./media/mpt_fixed_traj.png) +![mpt_fixed_traj](../media/mpt_fixed_traj.png) - **bounds** - Lateral Distance to the road or object boundaries to check collision in model predictive trajectory. - Whether these lines' ends align the road or obstacle boundaries can be checked. - `bounds*` of `/planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/debug/marker` whose type is `visualization_msgs/msg/MarkerArray` -![bounds](./media/bounds.png) +![bounds](../media/bounds.png) - **MPT trajectory** - The output of model predictive trajectory. Whether this trajectory is smooth enough and inside the drivable area can be checked. - `Trajectory` or `TrajectoryFootprint` rviz plugin. -![mpt_traj](./media/mpt_traj.png) +![mpt_traj](../media/mpt_traj.png) - **Output trajectory** - The output of obstacle_avoidance_planner. Whether this trajectory is smooth enough can be checked. - `Trajectory` or `TrajectoryFootprint` rviz plugin. -![output_traj](./media/output_traj.png) +![output_traj](../media/output_traj.png) ## Calculation cost @@ -114,4 +114,4 @@ With a script, we can plot some of above time as follows. python3 scripts/calclation_time_analyzer.py -f "solveOsqp,initOsqp,pathCallback" ``` -![calculation_cost_plot](./media/calculation_cost_plot.svg) +![calculation_cost_plot](../media/calculation_cost_plot.svg) diff --git a/planning/obstacle_avoidance_planner/docs/eb.md b/planning/obstacle_avoidance_planner/docs/eb.md new file mode 100644 index 0000000000000..6becc8b03cc24 --- /dev/null +++ b/planning/obstacle_avoidance_planner/docs/eb.md @@ -0,0 +1,120 @@ +# Elastic band + +## Abstract + +Elastic band smooths the input path. +Since the latter optimization (model predictive trajectory) is calculated on the frenet frame, path smoothing is applied here so that the latter optimization will be stable. + +Note that this smoothing process does not consider collision checking. +Therefore the output path may have a collision with road boundaries or obstacles. + +## Flowchart + +```plantuml +@startuml +title getEBTrajectory +start + +:crop trajectory; + +:insertFixedPoint; + +:resample trajectory; + +:getPaddedTrajectoryPoints + +:updateConstraint; + +:optimizeTrajectory; + +:convertOptimizedPointsToTrajectory; + +stop +@enduml +``` + +## Formulation + +We formulate a quadratic problem minimizing the diagonal length of the rhombus on each point generated by the current point and its previous and next points, shown as the red vector's length. + +![eb](../media/eb.svg){: style="width:600px"} + +Assuming that $k$'th point is $\boldsymbol{p}_k = (x_k, y_k)$, the objective function is as follows. + +$$ +\begin{align} +\ J & = \min \sum_{k=1}^{n-2} ||(\boldsymbol{p}_{k+1} - \boldsymbol{p}_{k}) - (\boldsymbol{p}_{k} - \boldsymbol{p}_{k-1})||^2 \\ +\ & = \min \sum_{k=1}^{n-2} ||\boldsymbol{p}_{k+1} - 2 \boldsymbol{p}_{k} + \boldsymbol{p}_{k-1}||^2 \\ +\ & = \min \sum_{k=1}^{n-2} \{(x_{k+1} - x_k + x_{k-1})^2 + (y_{k+1} - y_k + y_{k-1})^2\} \\ +\ & = \min + \begin{pmatrix} + \ x_0 \\ + \ x_1 \\ + \ x_2 \\ + \vdots \\ + \ x_{n-3}\\ + \ x_{n-2} \\ + \ x_{n-1} \\ + \ y_0 \\ + \ y_1 \\ + \ y_2 \\ + \vdots \\ + \ y_{n-3}\\ + \ y_{n-2} \\ + \ y_{n-1} \\ + \end{pmatrix}^T + \begin{pmatrix} + 1 & -2 & 1 & 0 & \dots& \\ + -2 & 5 & -4 & 1 & 0 &\dots \\ + 1 & -4 & 6 & -4 & 1 & \\ + 0 & 1 & -4 & 6 & -4 & \\ + \vdots & 0 & \ddots&\ddots& \ddots \\ + & \vdots & & & \\ + & & & 1 & -4 & 6 & -4 & 1 \\ + & & & & 1 & -4 & 5 & -2 \\ + & & & & & 1 & -2 & 1& \\ + & & & & & & & &1 & -2 & 1 & 0 & \dots& \\ + & & & & & & & &-2 & 5 & -4 & 1 & 0 &\dots \\ + & & & & & & & &1 & -4 & 6 & -4 & 1 & \\ + & & & & & & & &0 & 1 & -4 & 6 & -4 & \\ + & & & & & & & &\vdots & 0 & \ddots&\ddots& \ddots \\ + & & & & & & & & & \vdots & & & \\ + & & & & & & & & & & & 1 & -4 & 6 & -4 & 1 \\ + & & & & & & & & & & & & 1 & -4 & 5 & -2 \\ + & & & & & & & & & & & & & 1 & -2 & 1& \\ + \end{pmatrix} + \begin{pmatrix} + \ x_0 \\ + \ x_1 \\ + \ x_2 \\ + \vdots \\ + \ x_{n-3}\\ + \ x_{n-2} \\ + \ x_{n-1} \\ + \ y_0 \\ + \ y_1 \\ + \ y_2 \\ + \vdots \\ + \ y_{n-3}\\ + \ y_{n-2} \\ + \ y_{n-1} \\ + \end{pmatrix} +\end{align} +$$ + +Regarding the constraint, the distance that each point can move is limited so that the path will not changed a lot but will be smoother. +In detail, the longitudinal distance that each point can move is zero, and the lateral distance is parameterized as `eb.clearance.clearance_for_fix`, `eb.clearance.clearance_for_joint` and `eb.clearance.clearance_for_smooth`. + +The following figure describes how to constrain the lateral distance to move. +The red line is where the point can move. The points for the upper and lower bound are described as $(x_k^u, y_k^u)$ and $(x_k^l, y_k^l)$, respectively. + +![eb_constraint](../media/eb_constraint.svg){: style="width:700px"} + +Based on the line equation whose slope angle is $\theta_k$ and that passes through $(x_k, y_k)$, $(x_k^u, y_k^u)$ and $(x_k^l, y_k^l)$, the lateral constraint is formulated as follows. + +$$ +C_k^l \leq C_k \leq C_k^u +$$ + +In addition, the beginning point is fixed and the end point as well if the end point is considered as the goal. +This constraint can be applied with the upper equation by changing the distance that each point can move. diff --git a/planning/obstacle_avoidance_planner/docs/mpt.md b/planning/obstacle_avoidance_planner/docs/mpt.md new file mode 100644 index 0000000000000..6c07244bdb45e --- /dev/null +++ b/planning/obstacle_avoidance_planner/docs/mpt.md @@ -0,0 +1,484 @@ +# Model predictive trajectory + +## Abstract + +Model Predictive Trajectory (MPT) calculates the trajectory that meets the following conditions. + +- Kinematically feasible for linear vehicle kinematics model +- Collision free with obstacles and road boundaries + +Conditions for collision free is considered to be not hard constraints but soft constraints. +When the optimization failed or the optimized trajectory is not collision free, the output trajectory will be previously generated trajectory. + +Trajectory near the ego must be stable, therefore the condition where trajectory points near the ego are the same as previously generated trajectory is considered, and this is the only hard constraints in MPT. + +## Flowchart + +```plantuml +@startuml +title getModelPredictiveTrajectory +start + +:calcReferencePoints; + +:calc state matrix; + +:calcValueMatrix; + +:calcObjectiveMatrix; + +:calcConstraintMatrix; + +:calcOptimizedSteerAngles; + +:calcMPTPoints; + +:publishDebugTrajectories; + +stop +@enduml +``` + +## Vehicle kinematics + +As the following figure, we consider the bicycle kinematics model in the frenet frame to track the reference path. +At time step $k$, we define lateral distance to the reference path, heading angle against the reference path, and steer angle as $y_k$, $\theta_k$, and $\delta_k$ respectively. + +![vehicle_error_kinematics](../media/vehicle_error_kinematics.png) + +Assuming that the commanded steer angle is $\delta_{des, k}$, the kinematics model in the frenet frame is formulated as follows. +We also assume that the steer angle $\delta_k$ is first-order lag to the commanded one. + +$$ +\begin{align} +y_{k+1} & = y_{k} + v \sin \theta_k dt \\ +\theta_{k+1} & = \theta_k + \frac{v \tan \delta_k}{L}dt - \kappa_k v \cos \theta_k dt \\ +\delta_{k+1} & = \delta_k - \frac{\delta_k - \delta_{des,k}}{\tau}dt +\end{align} +$$ + +### Linearization + +Then we linearize these equations. +$y_k$ and $\theta_k$ are tracking errors, so we assume that those are small enough. +Therefore $\sin \theta_k \approx \theta_k$. + +Since $\delta_k$ is a steer angle, it is not always small. +By using a reference steer angle $\delta_{\mathrm{ref}, k}$ calculated by the reference path curvature $\kappa_k$, we express $\delta_k$ with a small value $\Delta \delta_k$. + +Note that the steer angle $\delta_k$ is within the steer angle limitation $\delta_{\max}$. +When the reference steer angle $\delta_{\mathrm{ref}, k}$ is larger than the steer angle limitation $\delta_{\max}$, and $\delta_{\mathrm{ref}, k}$ is used to linearize the steer angle, $\Delta \delta_k$ is $\Delta \delta_k = \delta - \delta_{\mathrm{ref}, k} = \delta_{\max} - \delta_{\mathrm{ref}, k}$, and the absolute $\Delta \delta_k$ gets larger. +Therefore, we have to apply the steer angle limitation to $\delta_{\mathrm{ref}, k}$ as well. + +$$ +\begin{align} +\delta_{\mathrm{ref}, k} & = \mathrm{clamp}(\arctan(L \kappa_k), -\delta_{\max}, \delta_{\max}) \\ +\delta_k & = \delta_{\mathrm{ref}, k} + \Delta \delta_k, \ \Delta \delta_k \ll 1 \\ +\end{align} +$$ + +$\mathrm{clamp}(v, v_{\min}, v_{\max})$ is a function to convert $v$ to be larger than $v_{\min}$ and smaller than $v_{\max}$. + +Using this $\delta_{\mathrm{ref}, k}$, $\tan \delta_k$ is linearized as follows. + +$$ +\begin{align} +\tan \delta_k & \approx \tan \delta_{\mathrm{ref}, k} + \left.\frac{d \tan \delta}{d \delta}\right|_{\delta = \delta_{\mathrm{ref}, k}} \Delta \delta_k \\ +& = \tan \delta_{\mathrm{ref}, k} + \left.\frac{d \tan \delta}{d \delta}\right|_{\delta = \delta_{\mathrm{ref}, k}} (\delta_{\mathrm{ref}, k} - \delta_k) \\ +& = \tan \delta_{\mathrm{ref}, k} - \frac{\delta_{\mathrm{ref}, k}}{\cos^2 \delta_{\mathrm{ref}, k}} + \frac{1}{\cos^2 \delta_{\mathrm{ref}, k}} \delta_k +\end{align} +$$ + +### One-step state equation + +Based on the linearization, the error kinematics is formulated with the following linear equations, + +$$ +\begin{align} + \begin{pmatrix} + y_{k+1} \\ + \theta_{k+1} + \end{pmatrix} + = + \begin{pmatrix} + 1 & v dt \\ + 0 & 1 \\ + \end{pmatrix} + \begin{pmatrix} + y_k \\ + \theta_k \\ + \end{pmatrix} + + + \begin{pmatrix} + 0 \\ + \frac{v dt}{L \cos^{2} \delta_{\mathrm{ref}, k}} \\ + \end{pmatrix} + \delta_{k} + + + \begin{pmatrix} + 0 \\ + \frac{v \tan(\delta_{\mathrm{ref}, k}) dt}{L} - \frac{v \delta_{\mathrm{ref}, k} dt}{L \cos^{2} \delta_{\mathrm{ref}, k}} - \kappa_k v dt\\ + \end{pmatrix} +\end{align} +$$ + +which can be formulated as follows with the state $\boldsymbol{x}$, control input $u$ and some matrices, where $\boldsymbol{x} = (y_k, \theta_k)$ + +$$ +\begin{align} + \boldsymbol{x}_{k+1} = A_k \boldsymbol{x}_k + \boldsymbol{b}_k u_k + \boldsymbol{w}_k +\end{align} +$$ + +### Time-series state equation + +Then, we formulate time-series state equation by concatenating states, control inputs and matrices respectively as + +$$ +\begin{align} + \boldsymbol{x} = A \boldsymbol{x}_0 + B \boldsymbol{u} + \boldsymbol{w} +\end{align} +$$ + +where + +$$ +\begin{align} +\boldsymbol{x} = (\boldsymbol{x}^T_1, \boldsymbol{x}^T_2, \boldsymbol{x}^T_3, \dots, \boldsymbol{x}^T_{n-1})^T \\ +\boldsymbol{u} = (u_0, u_1, u_2, \dots, u_{n-2})^T \\ +\boldsymbol{w} = (\boldsymbol{w}^T_0, \boldsymbol{w}^T_1, \boldsymbol{w}^T_2, \dots, \boldsymbol{w}^T_{n-1})^T. \\ +\end{align} +$$ + +In detail, each matrices are constructed as follows. + +$$ +\begin{align} + \begin{pmatrix} + \boldsymbol{x}_1 \\ + \boldsymbol{x}_2 \\ + \boldsymbol{x}_3 \\ + \vdots \\ + \boldsymbol{x}_{n-1} + \end{pmatrix} + = + \begin{pmatrix} + A_0 \\ + A_1 A_0 \\ + A_2 A_1 A_0\\ + \vdots \\ + \prod\limits_{k=0}^{n-1} A_{k} + \end{pmatrix} + \boldsymbol{x}_0 + + + \begin{pmatrix} + B_0 & 0 & & \dots & 0 \\ + A_0 B_0 & B_1 & 0 & \dots & 0 \\ + A_1 A_0 B_0 & A_0 B_1 & B_2 & \dots & 0 \\ + \vdots & \vdots & & \ddots & 0 \\ + \prod\limits_{k=0}^{n-3} A_k B_0 & \prod\limits_{k=0}^{n-4} A_k B_1 & \dots & A_0 B_{n-3} & B_{n-2} + \end{pmatrix} + \begin{pmatrix} + u_0 \\ + u_1 \\ + u_2 \\ + \vdots \\ + u_{n-2} + \end{pmatrix} + + + \begin{pmatrix} + I & 0 & & \dots & 0 \\ + A_0 & I & 0 & \dots & 0 \\ + A_1 A_0 & A_0 & I & \dots & 0 \\ + \vdots & \vdots & & \ddots & 0 \\ + \prod\limits_{k=0}^{n-3} A_k & \prod\limits_{k=0}^{n-4} A_k & \dots & A_0 & I + \end{pmatrix} + \begin{pmatrix} + \boldsymbol{w}_0 \\ + \boldsymbol{w}_1 \\ + \boldsymbol{w}_2 \\ + \vdots \\ + \boldsymbol{w}_{n-2} + \end{pmatrix} +\end{align} +$$ + +### Free-boundary-conditioned time-series state equation + +For path planning which does not start from the current ego pose, $\boldsymbol{x}_0$ should be the design variable of optimization. +Therefore, we make $\boldsymbol{u}'$ by concatenating $\boldsymbol{x}_0$ and $\boldsymbol{u}$, and redefine $\boldsymbol{x}$ as follows. + +$$ +\begin{align} + \boldsymbol{u}' & = (\boldsymbol{x}^T_0, \boldsymbol{u}^T)^T \\ + \boldsymbol{x} & = (\boldsymbol{x}^T_0, \boldsymbol{x}^T_1, \boldsymbol{x}^T_2, \dots, \boldsymbol{x}^T_{n-1})^T +\end{align} +$$ + +Then we get the following state equation + +$$ +\begin{align} + \boldsymbol{x}' = B \boldsymbol{u}' + \boldsymbol{w}, +\end{align} +$$ + +which is in detail + +$$ +\begin{align} + \begin{pmatrix} + \boldsymbol{x}_0 \\ + \boldsymbol{x}_1 \\ + \boldsymbol{x}_2 \\ + \boldsymbol{x}_3 \\ + \vdots \\ + \boldsymbol{x}_{n-1} + \end{pmatrix} + = + \begin{pmatrix} + I & 0 & \dots & & & 0 \\ + A_0 & B_0 & 0 & & \dots & 0 \\ + A_1 A_0 & A_0 B_0 & B_1 & 0 & \dots & 0 \\ + A_2 A_1 A_0 & A_1 A_0 B_0 & A_0 B_1 & B_2 & \dots & 0 \\ + \vdots & \vdots & \vdots & & \ddots & 0 \\ + \prod\limits_{k=0}^{n-1} A_k & \prod\limits_{k=0}^{n-3} A_k B_0 & \prod\limits_{k=0}^{n-4} A_k B_1 & \dots & A_0 B_{n-3} & B_{n-2} + \end{pmatrix} + \begin{pmatrix} + \boldsymbol{x}_0 \\ + u_0 \\ + u_1 \\ + u_2 \\ + \vdots \\ + u_{n-2} + \end{pmatrix} + + + \begin{pmatrix} + 0 & \dots & & & 0 \\ + I & 0 & & \dots & 0 \\ + A_0 & I & 0 & \dots & 0 \\ + A_1 A_0 & A_0 & I & \dots & 0 \\ + \vdots & \vdots & & \ddots & 0 \\ + \prod\limits_{k=0}^{n-3} A_k & \prod\limits_{k=0}^{n-4} A_k & \dots & A_0 & I + \end{pmatrix} + \begin{pmatrix} + \boldsymbol{w}_0 \\ + \boldsymbol{w}_1 \\ + \boldsymbol{w}_2 \\ + \vdots \\ + \boldsymbol{w}_{n-2} + \end{pmatrix}. +\end{align} +$$ + +## Objective function + +The objective function for smoothing and tracking is shown as follows, which can be formulated with value function matrices $Q, R$. + +$$ +\begin{align} +J_1 (\boldsymbol{x}', \boldsymbol{u}') & = w_y \sum_{k} y_k^2 + w_{\theta} \sum_{k} \theta_k^2 + w_{\delta} \sum_k \delta_k^2 + w_{\dot{\delta}} \sum_k \dot{\delta}_k^2 + w_{\ddot{\delta}} \sum_k \ddot{\delta}_k^2 \\ +& = \boldsymbol{x}'^T Q \boldsymbol{x}' + \boldsymbol{u}'^T R \boldsymbol{u}' \\ +& = \boldsymbol{u}'^T H \boldsymbol{u}' + \boldsymbol{u}'^T \boldsymbol{f} +\end{align} +$$ + +As mentioned before, the constraints to be collision free with obstacles and road boundaries are formulated to be soft constraints. +Assuming that the lateral distance to the road boundaries or obstacles from the back wheel center, front wheel center, and the point between them are $y_{\mathrm{base}, k}, y_{\mathrm{top}, k}, y_{\mathrm{mid}, k}$ respectively, and slack variables for each point are $\lambda_{\mathrm{base}}, \lambda_{\mathrm{top}}, \lambda_{\mathrm{mid}}$, the soft constraints can be formulated as follows. + +$$ +y_{\mathrm{base}, k, \min} - \lambda_{\mathrm{base}, k} \leq y_{\mathrm{base}, k} (y_k) \leq y_{\mathrm{base}, k, \max} + \lambda_{\mathrm{base}, k}\\ +y_{\mathrm{top}, k, \min} - \lambda_{\mathrm{top}, k} \leq y_{\mathrm{top}, k} (y_k) \leq y_{\mathrm{top}, k, \max} + \lambda_{\mathrm{top}, k}\\ +y_{\mathrm{mid}, k, \min} - \lambda_{\mathrm{mid}, k} \leq y_{\mathrm{mid}, k} (y_k) \leq y_{\mathrm{mid}, k, \max} + \lambda_{\mathrm{mid}, k} \\ +0 \leq \lambda_{\mathrm{base}, k} \\ +0 \leq \lambda_{\mathrm{top}, k} \\ +0 \leq \lambda_{\mathrm{mid}, k} +$$ + +Since $y_{\mathrm{base}, k}, y_{\mathrm{top}, k}, y_{\mathrm{mid}, k}$ is formulated as a linear function of $y_k$, the objective function for soft constraints is formulated as follows. + +$$ +\begin{align} +J_2 & (\boldsymbol{\lambda}_\mathrm{base}, \boldsymbol{\lambda}_\mathrm{top}, \boldsymbol {\lambda}_\mathrm{mid})\\ +& = w_{\mathrm{base}} \sum_{k} \lambda_{\mathrm{base}, k} + w_{\mathrm{mid}} \sum_k \lambda_{\mathrm{mid}, k} + w_{\mathrm{top}} \sum_k \lambda_{\mathrm{top}, k} +\end{align} +$$ + +Slack variables are also design variables for optimization. +We define a vector $\boldsymbol{v}$, that concatenates all the design variables. + +$$ +\begin{align} +\boldsymbol{v} = +\begin{pmatrix} + \boldsymbol{u}'^T & \boldsymbol{\lambda}_\mathrm{base}^T & \boldsymbol{\lambda}_\mathrm{top}^T & \boldsymbol{\lambda}_\mathrm{mid}^T +\end{pmatrix}^T +\end{align} +$$ + +The summation of these two objective functions is the objective function for the optimization problem. + +$$ +\begin{align} +\min_{\boldsymbol{v}} J (\boldsymbol{v}) = \min_{\boldsymbol{v}} J_1 (\boldsymbol{u}') + J_2 (\boldsymbol{\lambda}_\mathrm{base}, \boldsymbol{\lambda}_\mathrm{top}, \boldsymbol{\lambda}_\mathrm{mid}) +\end{align} +$$ + +As mentioned before, we use hard constraints where some trajectory points in front of the ego are the same as the previously generated trajectory points. +This hard constraints is formulated as follows. + +$$ +\begin{align} +\delta_k = \delta_{k}^{\mathrm{prev}} (0 \leq i \leq N_{\mathrm{fix}}) +\end{align} +$$ + +Finally we transform those objective functions to the following QP problem, and solve it. + +$$ +\begin{align} +\min_{\boldsymbol{v}} \ & \frac{1}{2} \boldsymbol{v}^T \boldsymbol{H} \boldsymbol{v} + \boldsymbol{f} \boldsymbol{v} \\ +\mathrm{s.t.} \ & \boldsymbol{b}_{lower} \leq \boldsymbol{A} \boldsymbol{v} \leq \boldsymbol{b}_{upper} +\end{align} +$$ + +## Constraints + +### Steer angle limitation + +Steer angle has a limitation $\delta_{max}$ and $\delta_{min}$. +Therefore we add linear inequality equations. + +$$ +\begin{align} +\delta_{min} \leq \delta_i \leq \delta_{max} +\end{align} +$$ + +### Collision free + +To realize collision-free trajectory planning, we have to formulate constraints that the vehicle is inside the road and also does not collide with obstacles in linear equations. +For linearity, we implemented some methods to approximate the vehicle shape with a set of circles, that is reliable and easy to implement. + +- 1. Bibycle Model +- 2. Uniform Circles +- 3. Fitting Uniform Circles + +![vehicle_circles](../media/vehicle_circles.svg) + +Now we formulate the linear constraints where a set of circles on each trajectory point is collision-free. +By using the drivable area, we calculate upper and lower boundaries along reference points, which will be interpolated on any position on the trajectory. +NOTE that upper and lower boundary is left and right, respectively. + +Assuming that upper and lower boundaries are $b_l$, $b_u$ respectively, and $r$ is a radius of a circle, lateral deviation of the circle center $y'$ has to be + +$$ +b_l + r \leq y' \leq b_u - r. +$$ + +![bounds](../media/bounds.svg) + +Based on the following figure, $y'$ can be formulated as follows. + +$$ +\begin{align} +y' & = L \sin(\theta + \beta) + y \cos \beta - l \sin(\gamma - \phi_a) \\ +& = L \sin \theta \cos \beta + L \cos \theta \sin \beta + y \cos \beta - l \sin(\gamma - \phi_a) \\ +& \approx L \theta \cos \beta + L \sin \beta + y \cos \beta - l \sin(\gamma - \phi_a) +\end{align} +$$ + +$$ +b_l + r - \lambda \leq y' \leq b_u - r + \lambda. +$$ + +$$ +\begin{align} +y' & = C_1 \boldsymbol{x} + C_2 \\ +& = C_1 (B \boldsymbol{v} + \boldsymbol{w}) + C_2 \\ +& = C_1 B \boldsymbol{v} + \boldsymbol{w} + C_2 +\end{align} +$$ + +Note that longitudinal position of the circle center and the trajectory point to calculate boundaries are different. +But each boundaries are vertical against the trajectory, resulting in less distortion by the longitudinal position difference since road boundaries does not change so much. +For example, if the boundaries are not vertical against the trajectory and there is a certain difference of longitudinal position between the circe center and the trajectory point, we can easily guess that there is much more distortion when comparing lateral deviation and boundaries. + +$$ +\begin{align} + A_{blk} & = + \begin{pmatrix} + C_1 B & O & \dots & O & I_{N_{ref} \times N_{ref}} & O \dots & O\\ + -C_1 B & O & \dots & O & I & O \dots & O\\ + O & O & \dots & O & I & O \dots & O + \end{pmatrix} + \in \boldsymbol{R}^{3 N_{ref} \times D_v + N_{circle} N_{ref}} \\ + \boldsymbol{b}_{lower, blk} & = + \begin{pmatrix} + \boldsymbol{b}_{lower} - C_1 \boldsymbol{w} - C_2 \\ + -\boldsymbol{b}_{upper} + C_1 \boldsymbol{w} + C_2 \\ + O + \end{pmatrix} + \in \boldsymbol{R}^{3 N_{ref}} \\ + \boldsymbol{b}_{upper, blk} & = \boldsymbol{\infty} + \in \boldsymbol{R}^{3 N_{ref}} +\end{align} +$$ + +We will explain options for optimization. + +#### L-infinity optimization + +The above formulation is called L2 norm for slack variables. +Instead, if we use L-infinity norm where slack variables are shared by enabling `l_inf_norm`. + +$$ +\begin{align} + A_{blk} = + \begin{pmatrix} + C_1 B & I_{N_{ref} \times N_{ref}} \\ + -C_1 B & I \\ + O & I + \end{pmatrix} +\in \boldsymbol{R}^{3N_{ref} \times D_v + N_{ref}} +\end{align} +$$ + +#### Two-step soft constraints + +$$ +\begin{align} +\boldsymbol{v}' = + \begin{pmatrix} + \boldsymbol{v} \\ + \boldsymbol{\lambda}^{soft_1} \\ + \boldsymbol{\lambda}^{soft_2} \\ + \end{pmatrix} + \in \boldsymbol{R}^{D_v + 2N_{slack}} +\end{align} +$$ + +$*$ depends on whether to use L2 norm or L-infinity optimization. + +$$ +\begin{align} + A_{blk} & = + \begin{pmatrix} + A^{soft_1}_{blk} \\ + A^{soft_2}_{blk} \\ + \end{pmatrix}\\ + & = + \begin{pmatrix} + C_1^{soft_1} B & & \\ + -C_1^{soft_1} B & \Huge{*} & \Huge{O} \\ + O & & \\ + C_1^{soft_2} B & & \\ + -C_1^{soft_2} B & \Huge{O} & \Huge{*} \\ + O & & + \end{pmatrix} + \in \boldsymbol{R}^{6 N_{ref} \times D_v + 2 N_{slack}} +\end{align} +$$ + +$N_{slack}$ is $N_{circle}$ when L2 optimization, or $1$ when L-infinity optimization. +$N_{circle}$ is the number of circles to check collision. diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/common_structs.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/common_structs.hpp index 54aa3356909ef..95656edf5f0e4 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/common_structs.hpp +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/common_structs.hpp @@ -1,4 +1,4 @@ -// Copyright 2021 Tier IV, Inc. +// Copyright 2023 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -15,246 +15,133 @@ #ifndef OBSTACLE_AVOIDANCE_PLANNER__COMMON_STRUCTS_HPP_ #define OBSTACLE_AVOIDANCE_PLANNER__COMMON_STRUCTS_HPP_ -#include "opencv2/core.hpp" -#include "opencv2/opencv.hpp" +#include "obstacle_avoidance_planner/type_alias.hpp" #include "rclcpp/rclcpp.hpp" - -#include "autoware_auto_perception_msgs/msg/predicted_object.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" -#include "nav_msgs/msg/map_meta_data.hpp" - -#include +#include "tier4_autoware_utils/tier4_autoware_utils.hpp" #include +#include #include #include +namespace obstacle_avoidance_planner +{ struct ReferencePoint; - struct Bounds; -using VehicleBounds = std::vector; -using SequentialBounds = std::vector; - -using BoundsCandidates = std::vector; - -struct QPParam -{ - int max_iteration; - double eps_abs; - double eps_rel; -}; -struct EBParam +struct PlannerData { - double clearance_for_fixing; - double clearance_for_straight_line; - double clearance_for_joint; - double clearance_for_only_smoothing; - QPParam qp_param; - - int num_joint_buffer_points; - int num_offset_for_begin_idx; - - double delta_arc_length_for_eb; - int num_sampling_points_for_eb; + // input + Header header; + std::vector traj_points; // converted from the input path + std::vector left_bound; + std::vector right_bound; + + // ego + geometry_msgs::msg::Pose ego_pose; + double ego_vel{}; }; -struct VehicleParam +struct TimeKeeper { - double wheelbase; - double length; - double width; - double rear_overhang; - double front_overhang; - double wheel_tread; - double right_overhang; - double left_overhang; - // double max_steer_rad; -}; - -struct ConstrainRectangle -{ - geometry_msgs::msg::Point top_left; - geometry_msgs::msg::Point top_right; - geometry_msgs::msg::Point bottom_left; - geometry_msgs::msg::Point bottom_right; - double velocity; - bool is_empty_driveable_area = false; - bool is_including_only_smooth_range = true; -}; + void init() { accumulated_msg = "\n"; } -struct DebugData -{ - struct StreamWithPrint + template + TimeKeeper & operator<<(const T & msg) { - StreamWithPrint & operator<<(const std::string & s) - { - sstream << s; - if (s.back() == '\n') { - std::string tmp_str = sstream.str(); - debug_str += tmp_str; + latest_stream << msg; + return *this; + } - if (is_showing_calculation_time) { - tmp_str.pop_back(); // NOTE* remove '\n' which is unnecessary for RCLCPP_INFO_STREAM - RCLCPP_INFO_STREAM(rclcpp::get_logger("obstacle_avoidance_planner.time"), tmp_str); - } - sstream.str(""); - } - return *this; - } + void endLine() + { + const auto msg = latest_stream.str(); + accumulated_msg += msg + "\n"; + latest_stream.str(""); - StreamWithPrint & operator<<(const double d) - { - sstream << d; - return *this; + if (enable_calculation_time_info) { + RCLCPP_INFO_STREAM(rclcpp::get_logger("obstacle_avoidance_planner.time"), msg); } + } - std::string getString() const { return debug_str; } - - bool is_showing_calculation_time; - std::string debug_str = "\n"; - std::stringstream sstream; - }; + void tic(const std::string & func_name) { stop_watch_.tic(func_name); } - void init( - const bool local_is_showing_calculation_time, const int local_mpt_visualize_sampling_num, - const geometry_msgs::msg::Pose & local_current_ego_pose, - const std::vector & local_vehicle_circle_radiuses, - const std::vector & local_vehicle_circle_longitudinal_offsets) + void toc(const std::string & func_name, const std::string & white_spaces) { - msg_stream.is_showing_calculation_time = local_is_showing_calculation_time; - mpt_visualize_sampling_num = local_mpt_visualize_sampling_num; - current_ego_pose = local_current_ego_pose; - vehicle_circle_radiuses = local_vehicle_circle_radiuses; - vehicle_circle_longitudinal_offsets = local_vehicle_circle_longitudinal_offsets; + const double elapsed_time = stop_watch_.toc(func_name); + *this << white_spaces << func_name << ":= " << elapsed_time << " [ms]"; + endLine(); } - StreamWithPrint msg_stream; - size_t mpt_visualize_sampling_num; - geometry_msgs::msg::Pose current_ego_pose; - std::vector vehicle_circle_radiuses; - std::vector vehicle_circle_longitudinal_offsets; - - boost::optional stop_pose_by_drivable_area = boost::none; - std::vector interpolated_points; - std::vector straight_points; - std::vector fixed_points; - std::vector non_fixed_points; - std::vector constrain_rectangles; - std::vector avoiding_traj_points; - std::vector avoiding_objects; - - cv::Mat clearance_map; - cv::Mat only_object_clearance_map; - cv::Mat area_with_objects_map; - - std::vector> vehicle_circles_pose; - std::vector ref_points; - - std::vector mpt_ref_poses; - std::vector lateral_errors; - std::vector yaw_errors; + std::string getLog() const { return accumulated_msg; } - std::vector eb_traj; - std::vector mpt_fixed_traj; - std::vector mpt_ref_traj; - std::vector mpt_traj; - std::vector extended_fixed_traj; - std::vector extended_non_fixed_traj; + bool enable_calculation_time_info; + std::string accumulated_msg = "\n"; + std::stringstream latest_stream; - BoundsCandidates bounds_candidates; + tier4_autoware_utils::StopWatch< + std::chrono::milliseconds, std::chrono::microseconds, std::chrono::steady_clock> + stop_watch_; }; -struct Trajectories +struct DebugData { - std::vector smoothed_trajectory; - std::vector mpt_ref_points; - std::vector model_predictive_trajectory; -}; + // settting + size_t mpt_visualize_sampling_num; + geometry_msgs::msg::Pose ego_pose; + std::vector vehicle_circle_radiuses; + std::vector vehicle_circle_longitudinal_offsets; -struct TrajectoryParam -{ - bool is_avoiding_unknown; - bool is_avoiding_car; - bool is_avoiding_truck; - bool is_avoiding_bus; - bool is_avoiding_bicycle; - bool is_avoiding_motorbike; - bool is_avoiding_pedestrian; - bool is_avoiding_animal; - int num_sampling_points; - double delta_arc_length_for_trajectory; - double delta_dist_threshold_for_closest_point; - double delta_yaw_threshold_for_closest_point; - double delta_yaw_threshold_for_straight; - double trajectory_length; - double forward_fixing_min_distance; - double forward_fixing_min_time; - double backward_fixing_distance; - double max_avoiding_ego_velocity_ms; - double max_avoiding_objects_velocity_ms; - double center_line_width; - double acceleration_for_non_deceleration_range; - int num_fix_points_for_extending; - double max_dist_for_extending_end_point; - double non_fixed_trajectory_length; + // mpt + std::vector ref_points; + std::vector> vehicle_circles_pose; - double ego_nearest_dist_threshold; - double ego_nearest_yaw_threshold; - bool enable_clipping_fixed_traj; + std::vector extended_traj_points; }; -struct MPTParam +struct TrajectoryParam { - bool enable_warm_start; - bool enable_manual_warm_start; - bool steer_limit_constraint; - bool fix_points_around_ego; - int num_curvature_sampling_points; - bool is_fixed_point_single; - - std::vector vehicle_circle_longitudinal_offsets; // from base_link - std::vector vehicle_circle_radiuses; - - double delta_arc_length_for_mpt_points; - - double hard_clearance_from_road; - double soft_clearance_from_road; - double soft_second_clearance_from_road; - double extra_desired_clearance_from_road; - double clearance_from_object; - double soft_avoidance_weight; - double soft_second_avoidance_weight; - - double lat_error_weight; - double yaw_error_weight; - double yaw_error_rate_weight; - - double near_objects_length; + TrajectoryParam() = default; + explicit TrajectoryParam(rclcpp::Node * node) + { + output_backward_traj_length = + node->declare_parameter("common.output_backward_traj_length"); + output_delta_arc_length = node->declare_parameter("common.output_delta_arc_length"); + } - double terminal_lat_error_weight; - double terminal_yaw_error_weight; - double terminal_path_lat_error_weight; - double terminal_path_yaw_error_weight; + void onParam(const std::vector & parameters) + { + using tier4_autoware_utils::updateParam; - double steer_input_weight; - double steer_rate_weight; + // common + updateParam( + parameters, "common.output_backward_traj_length", output_backward_traj_length); + updateParam(parameters, "common.output_delta_arc_length", output_delta_arc_length); + } - double obstacle_avoid_lat_error_weight; - double obstacle_avoid_yaw_error_weight; - double obstacle_avoid_steer_input_weight; + double output_delta_arc_length; + double output_backward_traj_length; +}; - double optimization_center_offset; - double max_steer_rad; +struct EgoNearestParam +{ + EgoNearestParam() = default; + explicit EgoNearestParam(rclcpp::Node * node) + { + dist_threshold = node->declare_parameter("ego_nearest_dist_threshold"); + yaw_threshold = node->declare_parameter("ego_nearest_yaw_threshold"); + } - std::vector bounds_search_widths; + void onParam(const std::vector & parameters) + { + using tier4_autoware_utils::updateParam; + updateParam(parameters, "ego_nearest_dist_threshold", dist_threshold); + updateParam(parameters, "ego_nearest_yaw_threshold", yaw_threshold); + } - bool soft_constraint; - bool hard_constraint; - bool l_inf_norm; - bool two_step_soft_constraint; - bool plan_from_ego; - double max_plan_from_ego_length; + double dist_threshold{0.0}; + double yaw_threshold{0.0}; }; +} // namespace obstacle_avoidance_planner #endif // OBSTACLE_AVOIDANCE_PLANNER__COMMON_STRUCTS_HPP_ diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/debug_marker.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/debug_marker.hpp new file mode 100644 index 0000000000000..8753699ab9d4b --- /dev/null +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/debug_marker.hpp @@ -0,0 +1,35 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#ifndef OBSTACLE_AVOIDANCE_PLANNER__DEBUG_MARKER_HPP_ +#define OBSTACLE_AVOIDANCE_PLANNER__DEBUG_MARKER_HPP_ + +#include "obstacle_avoidance_planner/common_structs.hpp" +#include "obstacle_avoidance_planner/type_alias.hpp" +#include "rclcpp/clock.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" +#include "tier4_autoware_utils/ros/marker_helper.hpp" +#include "vehicle_info_util/vehicle_info_util.hpp" + +#include +#include +#include + +namespace obstacle_avoidance_planner +{ +MarkerArray getDebugMarker( + const DebugData & debug_data, + const std::vector & optimized_points, + const vehicle_info_util::VehicleInfo & vehicle_info); +} // namespace obstacle_avoidance_planner +#endif // OBSTACLE_AVOIDANCE_PLANNER__DEBUG_MARKER_HPP_ diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/eb_path_optimizer.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/eb_path_optimizer.hpp deleted file mode 100644 index ae412c046f98c..0000000000000 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/eb_path_optimizer.hpp +++ /dev/null @@ -1,187 +0,0 @@ -// Copyright 2020 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef OBSTACLE_AVOIDANCE_PLANNER__EB_PATH_OPTIMIZER_HPP_ -#define OBSTACLE_AVOIDANCE_PLANNER__EB_PATH_OPTIMIZER_HPP_ - -#include "eigen3/Eigen/Core" -#include "obstacle_avoidance_planner/common_structs.hpp" -#include "osqp_interface/osqp_interface.hpp" -#include "tier4_autoware_utils/system/stop_watch.hpp" - -#include "autoware_auto_planning_msgs/msg/path.hpp" -#include "autoware_auto_planning_msgs/msg/path_point.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" -#include "geometry_msgs/msg/point.hpp" -#include "geometry_msgs/msg/pose.hpp" -#include "nav_msgs/msg/map_meta_data.hpp" - -#include "boost/optional.hpp" - -#include -#include -#include - -class EBPathOptimizer -{ -private: - struct CandidatePoints - { - std::vector fixed_points; - std::vector non_fixed_points; - int begin_path_idx; - int end_path_idx; - }; - - struct Anchor - { - geometry_msgs::msg::Pose pose; - }; - - struct ConstrainRectangles - { - ConstrainRectangle object_constrain_rectangle; - ConstrainRectangle road_constrain_rectangle; - }; - - struct ConstrainLines - { - double x_coef; - double y_coef; - double lower_bound; - double upper_bound; - }; - - struct Constrain - { - ConstrainLines top_and_bottom; - ConstrainLines left_and_right; - }; - - struct Rectangle - { - geometry_msgs::msg::Point top_left; - geometry_msgs::msg::Point top_right; - geometry_msgs::msg::Point bottom_left; - geometry_msgs::msg::Point bottom_right; - }; - - struct OccupancyMaps - { - std::vector> object_occupancy_map; - std::vector> road_occupancy_map; - }; - - const bool is_showing_debug_info_; - const double epsilon_; - - const QPParam qp_param_; - const TrajectoryParam traj_param_; - const EBParam eb_param_; - const VehicleParam vehicle_param_; - - Eigen::MatrixXd default_a_matrix_; - std::unique_ptr osqp_solver_ptr_; - - double current_ego_vel_; - - mutable tier4_autoware_utils::StopWatch< - std::chrono::milliseconds, std::chrono::microseconds, std::chrono::steady_clock> - stop_watch_; - - Eigen::MatrixXd makePMatrix(); - - Eigen::MatrixXd makeAMatrix(); - - Anchor getAnchor( - const std::vector & interpolated_points, const int interpolated_idx, - const std::vector & path_points) const; - - Constrain getConstrainFromConstrainRectangle( - const geometry_msgs::msg::Point & interpolated_point, - const ConstrainRectangle & constrain_range); - - ConstrainLines getConstrainLines( - const double dx, const double dy, const geometry_msgs::msg::Point & point, - const geometry_msgs::msg::Point & opposite_point); - - ConstrainRectangle getConstrainRectangle(const Anchor & anchor, const double clearance) const; - - ConstrainRectangle getConstrainRectangle( - const Anchor & anchor, const double min_x, const double max_x, const double min_y, - const double max_y) const; - - int getStraightLineIdx( - const std::vector & interpolated_points, - const int farthest_point_idx, - std::vector & debug_detected_straight_points); - - boost::optional> getConstrainRectangleVec( - const autoware_auto_planning_msgs::msg::Path & path, - const std::vector & interpolated_points, const int num_fixed_points, - const int farthest_point_idx, const int straight_idx); - - std::vector getPaddedInterpolatedPoints( - const std::vector & interpolated_points, const int farthest_idx); - - int getNumFixedPoints( - const std::vector & fixed_points, - const std::vector & interpolated_points, const int farthest_idx); - - boost::optional> - getOptimizedTrajectory( - const autoware_auto_planning_msgs::msg::Path & path, const CandidatePoints & candidate_points, - DebugData & debug_data); - - void updateConstrain( - const std::vector & interpolated_points, - const std::vector & rectangle_points); - - std::vector convertOptimizedPointsToTrajectory( - const std::vector optimized_points, const std::vector & constraint, - const int farthest_idx); - - std::vector getFixedPoints( - const geometry_msgs::msg::Pose & ego_pose, - const std::vector & path_points, - const std::unique_ptr & prev_optimized_points); - - CandidatePoints getCandidatePoints( - const geometry_msgs::msg::Pose & ego_pose, - const std::vector & path_points, - const std::unique_ptr & prev_trajs, DebugData & debug_data); - - CandidatePoints getDefaultCandidatePoints( - const std::vector & path_points); - - std::pair, int64_t> solveQP(); - - boost::optional> - calculateTrajectory( - const std::vector & padded_interpolated_points, - const std::vector & constrain_rectangles, const int farthest_idx, - DebugData & debug_data); - -public: - EBPathOptimizer( - const bool is_showing_debug_info, const TrajectoryParam & traj_param, const EBParam & eb_param, - const VehicleParam & vehicle_param); - - boost::optional> getEBTrajectory( - const geometry_msgs::msg::Pose & ego_pose, const autoware_auto_planning_msgs::msg::Path & path, - const std::unique_ptr & prev_trajs, const double current_ego_vel, - DebugData & debug_data); -}; - -#endif // OBSTACLE_AVOIDANCE_PLANNER__EB_PATH_OPTIMIZER_HPP_ diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/eb_path_smoother.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/eb_path_smoother.hpp new file mode 100644 index 0000000000000..e4ac55dba5f4e --- /dev/null +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/eb_path_smoother.hpp @@ -0,0 +1,132 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef OBSTACLE_AVOIDANCE_PLANNER__EB_PATH_SMOOTHER_HPP_ +#define OBSTACLE_AVOIDANCE_PLANNER__EB_PATH_SMOOTHER_HPP_ + +#include "eigen3/Eigen/Core" +#include "obstacle_avoidance_planner/common_structs.hpp" +#include "obstacle_avoidance_planner/type_alias.hpp" +#include "osqp_interface/osqp_interface.hpp" + +#include +#include +#include +#include +#include + +namespace obstacle_avoidance_planner +{ +class EBPathSmoother +{ +public: + EBPathSmoother( + rclcpp::Node * node, const bool enable_debug_info, const EgoNearestParam ego_nearest_param, + const TrajectoryParam & traj_param, const std::shared_ptr time_keeper_ptr); + + std::optional> getEBTrajectory( + const PlannerData & planner_data); + + void initialize(const bool enable_debug_info, const TrajectoryParam & traj_param); + void resetPreviousData(); + void onParam(const std::vector & parameters); + +private: + struct EBParam + { + // qp + struct QPParam + { + int max_iteration; + double eps_abs; + double eps_rel; + }; + + EBParam() = default; + explicit EBParam(rclcpp::Node * node); + void onParam(const std::vector & parameters); + + // option + bool enable_warm_start; + bool enable_optimization_validation; + + // common + double delta_arc_length; + int num_points; + + // clearance + int num_joint_points; + double clearance_for_fix; + double clearance_for_joint; + double clearance_for_smooth; + + // weight + double smooth_weight; + double lat_error_weight; + + // qp + QPParam qp_param; + + // validation + double max_validation_error; + }; + + struct Constraint2d + { + struct Constraint + { + Eigen::Vector2d coef; + double upper_bound; + double lower_bound; + }; + + Constraint lon; + Constraint lat; + }; + + // arguments + bool enable_debug_info_; + EgoNearestParam ego_nearest_param_; + TrajectoryParam traj_param_; + EBParam eb_param_; + mutable std::shared_ptr time_keeper_ptr_; + rclcpp::Logger logger_; + + // publisher + rclcpp::Publisher::SharedPtr debug_eb_traj_pub_; + rclcpp::Publisher::SharedPtr debug_eb_fixed_traj_pub_; + + std::unique_ptr osqp_solver_ptr_; + std::shared_ptr> prev_eb_traj_points_ptr_{nullptr}; + + std::vector insertFixedPoint( + const std::vector & traj_point) const; + + std::tuple, size_t> getPaddedTrajectoryPoints( + const std::vector & traj_points) const; + + void updateConstraint( + const std_msgs::msg::Header & header, const std::vector & traj_points, + const bool is_goal_contained, const int pad_start_idx); + + std::optional> optimizeTrajectory(); + + std::optional> + convertOptimizedPointsToTrajectory( + const std::vector & optimized_points, const std::vector & traj_points, + const int pad_start_idx) const; +}; +} // namespace obstacle_avoidance_planner + +#endif // OBSTACLE_AVOIDANCE_PLANNER__EB_PATH_SMOOTHER_HPP_ diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/mpt_optimizer.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/mpt_optimizer.hpp index d48f1f384ae52..4c70a02ce650b 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/mpt_optimizer.hpp +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/mpt_optimizer.hpp @@ -1,4 +1,4 @@ -// Copyright 2020 Tier IV, Inc. +// Copyright 2023 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -11,83 +11,35 @@ // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. -/* - * This Code is inspired by code from https://github.com/LiJiangnanBit/path_optimizer - * - * MIT License - * - * Copyright (c) 2020 Li Jiangnan - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ #ifndef OBSTACLE_AVOIDANCE_PLANNER__MPT_OPTIMIZER_HPP_ #define OBSTACLE_AVOIDANCE_PLANNER__MPT_OPTIMIZER_HPP_ #include "eigen3/Eigen/Core" #include "eigen3/Eigen/Sparse" +#include "gtest/gtest.h" #include "interpolation/linear_interpolation.hpp" +#include "interpolation/spline_interpolation_points_2d.hpp" #include "obstacle_avoidance_planner/common_structs.hpp" -#include "obstacle_avoidance_planner/vehicle_model/vehicle_model_interface.hpp" +#include "obstacle_avoidance_planner/state_equation_generator.hpp" +#include "obstacle_avoidance_planner/type_alias.hpp" #include "osqp_interface/osqp_interface.hpp" -#include "tier4_autoware_utils/system/stop_watch.hpp" #include "tier4_autoware_utils/tier4_autoware_utils.hpp" - -#include "autoware_auto_planning_msgs/msg/path_point.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" -#include "nav_msgs/msg/map_meta_data.hpp" - -#include "boost/optional.hpp" +#include "vehicle_info_util/vehicle_info_util.hpp" #include +#include +#include +#include #include -enum class CollisionType { NO_COLLISION = 0, OUT_OF_SIGHT = 1, OUT_OF_ROAD = 2, OBJECT = 3 }; - +namespace obstacle_avoidance_planner +{ struct Bounds { - Bounds() = default; - Bounds( - const double lower_bound_, const double upper_bound_, CollisionType lower_collision_type_, - CollisionType upper_collision_type_) - : lower_bound(lower_bound_), - upper_bound(upper_bound_), - lower_collision_type(lower_collision_type_), - upper_collision_type(upper_collision_type_) - { - } - double lower_bound; double upper_bound; - CollisionType lower_collision_type; - CollisionType upper_collision_type; - - bool hasCollisionWithRightObject() const { return lower_collision_type == CollisionType::OBJECT; } - - bool hasCollisionWithLeftObject() const { return upper_collision_type == CollisionType::OBJECT; } - - bool hasCollisionWithObject() const - { - return hasCollisionWithRightObject() || hasCollisionWithLeftObject(); - } - static Bounds lerp(Bounds prev_bounds, Bounds next_bounds, double ratio) { const double lower_bound = @@ -95,14 +47,7 @@ struct Bounds const double upper_bound = interpolation::lerp(prev_bounds.upper_bound, next_bounds.upper_bound, ratio); - if (ratio < 0.5) { - return Bounds{ - lower_bound, upper_bound, prev_bounds.lower_collision_type, - prev_bounds.upper_collision_type}; - } - - return Bounds{ - lower_bound, upper_bound, next_bounds.lower_collision_type, next_bounds.upper_collision_type}; + return Bounds{lower_bound, upper_bound}; } void translate(const double offset) @@ -112,53 +57,72 @@ struct Bounds } }; +struct KinematicState +{ + double lat{0.0}; + double yaw{0.0}; + + Eigen::Vector2d toEigenVector() const { return Eigen::Vector2d{lat, yaw}; } +}; + struct ReferencePoint { - geometry_msgs::msg::Point p; - double k = 0; - double v = 0; - double yaw = 0; - double s = 0; - double alpha = 0.0; - Bounds bounds; - bool near_objects; - - // NOTE: fix_kinematic_state is used for two purposes - // one is fixing points around ego for stability - // second is fixing current ego pose when no velocity for planning from ego pose - boost::optional fix_kinematic_state = boost::none; - bool plan_from_ego = true; - Eigen::Vector2d optimized_kinematic_state; - double optimized_input; - - // - std::vector> beta; - VehicleBounds vehicle_bounds; - - // SequentialBoundsCandidates sequential_bounds_candidates; - std::vector vehicle_bounds_poses; // for debug visualization + geometry_msgs::msg::Pose pose; + double longitudinal_velocity_mps; + + // additional information + double curvature{0.0}; + double delta_arc_length{0.0}; + double alpha{0.0}; // for minimizing lateral error + Bounds bounds{}; // bounds on `pose` + std::vector> beta{}; // for collision-free constraint + double normalized_avoidance_cost{0.0}; + + // bounds and its local pose on each collision-free constraint + std::vector bounds_on_constraints{}; + std::vector pose_on_constraints{}; + + // optimization result + std::optional fixed_kinematic_state{std::nullopt}; + KinematicState optimized_kinematic_state{}; + double optimized_input{}; + std::optional> slack_variables{std::nullopt}; + + double getYaw() const { return tf2::getYaw(pose.orientation); } + + geometry_msgs::msg::Pose offsetDeviation(const double lat_dev, const double yaw_dev) const + { + auto pose_with_deviation = tier4_autoware_utils::calcOffsetPose(pose, 0.0, lat_dev, 0.0); + pose_with_deviation.orientation = + tier4_autoware_utils::createQuaternionFromYaw(getYaw() + yaw_dev); + return pose_with_deviation; + } }; class MPTOptimizer { -private: - struct MPTMatrix - { - // Eigen::MatrixXd Aex; - Eigen::MatrixXd Bex; - Eigen::VectorXd Wex; - // Eigen::SparseMatrix Cex; - // Eigen::SparseMatrix Qex; - // Eigen::SparseMatrix Rex; - // Eigen::MatrixXd R1ex; - // Eigen::MatrixXd R2ex; - // Eigen::MatrixXd Uref_ex; - }; +public: + MPTOptimizer( + rclcpp::Node * node, const bool enable_debug_info, const EgoNearestParam ego_nearest_param, + const vehicle_info_util::VehicleInfo & vehicle_info, const TrajectoryParam & traj_param, + const std::shared_ptr debug_data_ptr, + const std::shared_ptr time_keeper_ptr); + + std::optional> getModelPredictiveTrajectory( + const PlannerData & planner_data, const std::vector & smoothed_points); + + void initialize(const bool enable_debug_info, const TrajectoryParam & traj_param); + void resetPreviousData(); + void onParam(const std::vector & parameters); + double getTrajectoryLength() const; + int getNumberOfPoints() const; + +private: struct ValueMatrix { - Eigen::SparseMatrix Qex; - Eigen::SparseMatrix Rex; + Eigen::SparseMatrix Q; + Eigen::SparseMatrix R; }; struct ObjectiveMatrix @@ -174,123 +138,161 @@ class MPTOptimizer Eigen::VectorXd upper_bound; }; - struct MPTTrajs + struct MPTParam { - std::vector mpt; - std::vector ref_points; + explicit MPTParam(rclcpp::Node * node, const vehicle_info_util::VehicleInfo & vehicle_info); + MPTParam() = default; + void onParam(const std::vector & parameters); + + // option + bool enable_warm_start; + bool enable_manual_warm_start; + bool enable_optimization_validation; + bool steer_limit_constraint; + int mpt_visualize_sampling_num; // for debug + + // common + double delta_arc_length; + int num_points; + + // kinematics + double optimization_center_offset; + double max_steer_rad; + + // clearance + double hard_clearance_from_road; + double soft_clearance_from_road; + double soft_collision_free_weight; + + // weight + double lat_error_weight; + double yaw_error_weight; + double yaw_error_rate_weight; + + double terminal_lat_error_weight; + double terminal_yaw_error_weight; + double goal_lat_error_weight; + double goal_yaw_error_weight; + + double steer_input_weight; + double steer_rate_weight; + + // avoidance + double max_avoidance_cost; + double avoidance_cost_margin; + double avoidance_cost_band_length; + double avoidance_cost_decrease_rate; + double avoidance_lat_error_weight; + double avoidance_yaw_error_weight; + double avoidance_steer_input_weight; + + // constraint type + bool soft_constraint; + bool hard_constraint; + bool l_inf_norm; + + // vehicle circles + std::string vehicle_circles_method; + int vehicle_circles_uniform_circle_num; + double vehicle_circles_uniform_circle_radius_ratio; + int vehicle_circles_bicycle_model_num; + double vehicle_circles_bicycle_model_rear_radius_ratio; + double vehicle_circles_bicycle_model_front_radius_ratio; + int vehicle_circles_fitting_uniform_circle_num; + + // validation + double max_validation_lat_error; + double max_validation_yaw_error; }; - const double osqp_epsilon_ = 1.0e-3; + // publisher + rclcpp::Publisher::SharedPtr debug_fixed_traj_pub_; + rclcpp::Publisher::SharedPtr debug_ref_traj_pub_; + rclcpp::Publisher::SharedPtr debug_mpt_traj_pub_; - bool is_showing_debug_info_; + // argument + bool enable_debug_info_; + EgoNearestParam ego_nearest_param_; + vehicle_info_util::VehicleInfo vehicle_info_; TrajectoryParam traj_param_; - VehicleParam vehicle_param_; + mutable std::shared_ptr debug_data_ptr_; + mutable std::shared_ptr time_keeper_ptr_; + rclcpp::Logger logger_; MPTParam mpt_param_; - std::unique_ptr vehicle_model_ptr_; - std::unique_ptr osqp_solver_ptr_; - geometry_msgs::msg::Pose current_ego_pose_; - double current_ego_vel_; - - int prev_mat_n = 0; - int prev_mat_m = 0; - - mutable tier4_autoware_utils::StopWatch< - std::chrono::milliseconds, std::chrono::microseconds, std::chrono::steady_clock> - stop_watch_; + StateEquationGenerator state_equation_generator_; + std::unique_ptr osqp_solver_ptr_; - std::vector getReferencePoints( - const std::vector & points, - const std::vector & left_bound, - const std::vector & right_bound, - const std::unique_ptr & prev_mpt_points, DebugData & debug_data) const; + const double osqp_epsilon_ = 1.0e-3; - void calcPlanningFromEgo(std::vector & ref_points) const; + // vehicle circles + std::vector vehicle_circle_longitudinal_offsets_; // from base_link + std::vector vehicle_circle_radiuses_; - /* - std::vector convertToReferencePoints( - const std::vector & points, - const std::unique_ptr & prev_mpt_points, const bool enable_avoidance, - const CVMaps & maps, DebugData & debug_data) const; - */ + // previous data + int prev_mat_n_ = 0; + int prev_mat_m_ = 0; + std::shared_ptr> prev_ref_points_ptr_{nullptr}; - std::vector getFixedReferencePoints( - const std::vector & points, - const std::unique_ptr & prev_trajs) const; + void updateVehicleCircles(); - void calcBounds( + std::vector calcReferencePoints( + const PlannerData & planner_data, const std::vector & smoothed_points) const; + void updateCurvature( + std::vector & ref_points, + const SplineInterpolationPoints2d & ref_points_spline) const; + void updateOrientation( + std::vector & ref_points, + const SplineInterpolationPoints2d & ref_points_spline) const; + void updateBounds( std::vector & ref_points, const std::vector & left_bound, - const std::vector & right_bound, DebugData & debug_data) const; - - void calcVehicleBounds(std::vector & ref_points, DebugData & debug_data) const; - - // void calcFixState( - // std::vector & ref_points, - // const std::unique_ptr & prev_trajs) const; - - void calcOrientation(std::vector & ref_points) const; - - void calcCurvature(std::vector & ref_points) const; - - void calcArcLength(std::vector & ref_points) const; - - void calcExtraPoints( + const std::vector & right_bound) const; + void updateVehicleBounds( std::vector & ref_points, - const std::unique_ptr & prev_trajs) const; - - /* - * predict equation: Xec = Aex * x0 + Bex * Uex + Wex - * cost function: J = Xex' * Qex * Xex + (Uex - Uref)' * R1ex * (Uex - Uref_ex) + Uex' * R2ex * - * Uex Qex = diag([Q,Q,...]), R1ex = diag([R,R,...]) - */ - MPTMatrix generateMPTMatrix( - const std::vector & reference_points, DebugData & debug_data) const; + const SplineInterpolationPoints2d & ref_points_spline) const; + void updateFixedPoint(std::vector & ref_points) const; + void updateDeltaArcLength(std::vector & ref_points) const; + void updateExtraPoints(std::vector & ref_points) const; - ValueMatrix generateValueMatrix( + ValueMatrix calcValueMatrix( const std::vector & reference_points, - const std::vector & path_points, - DebugData & debug_data) const; + const std::vector & traj_points) const; - void addSteerWeightR( - std::vector> & Rex_triplet_vec, + ObjectiveMatrix calcObjectiveMatrix( + const StateEquationGenerator::Matrix & mpt_mat, const ValueMatrix & obj_mat, const std::vector & ref_points) const; - boost::optional executeOptimization( - const std::unique_ptr & prev_trajs, const bool enable_avoidance, - const MPTMatrix & mpt_mat, const ValueMatrix & obj_mat, - const std::vector & ref_points, DebugData & debug_data); + ConstraintMatrix calcConstraintMatrix( + const StateEquationGenerator::Matrix & mpt_mat, + const std::vector & ref_points) const; - std::vector getMPTPoints( - std::vector & fixed_ref_points, - std::vector & non_fixed_ref_points, const Eigen::VectorXd & Uex, - const MPTMatrix & mpt_matrix, DebugData & debug_data); + std::optional calcOptimizedSteerAngles( + const std::vector & ref_points, const ObjectiveMatrix & obj_mat, + const ConstraintMatrix & const_mat); + Eigen::VectorXd calcInitialSolutionForManualWarmStart( + const std::vector & ref_points, + const std::vector & prev_ref_points) const; + std::pair updateMatrixForManualWarmStart( + const ObjectiveMatrix & obj_mat, const ConstraintMatrix & const_mat, + const std::optional & u0) const; - std::vector getMPTFixedPoints( + void addSteerWeightR( + std::vector> & R_triplet_vec, const std::vector & ref_points) const; - ObjectiveMatrix getObjectiveMatrix( - const MPTMatrix & mpt_mat, const ValueMatrix & obj_mat, - [[maybe_unused]] const std::vector & ref_points, DebugData & debug_data) const; + std::optional> calcMPTPoints( + std::vector & ref_points, const Eigen::VectorXd & U, + const StateEquationGenerator::Matrix & mpt_matrix) const; - ConstraintMatrix getConstraintMatrix( - const bool enable_avoidance, const MPTMatrix & mpt_mat, - const std::vector & ref_points, DebugData & debug_data) const; - -public: - MPTOptimizer( - const bool is_showing_debug_info, const TrajectoryParam & traj_param, - const VehicleParam & vehicle_param, const MPTParam & mpt_param); + void publishDebugTrajectories( + const std_msgs::msg::Header & header, const std::vector & ref_points, + const std::vector & mpt_traj_points) const; + std::vector extractFixedPoints( + const std::vector & ref_points) const; - boost::optional getModelPredictiveTrajectory( - const bool enable_avoidance, - const std::vector & smoothed_points, - const std::vector & path_points, - const std::vector & left_bound, - const std::vector & right_bound, - const std::unique_ptr & prev_trajs, - const geometry_msgs::msg::Pose & current_ego_pose, const double current_ego_vel, - DebugData & debug_data); + size_t getNumberOfSlackVariables() const; + std::optional calcNormalizedAvoidanceCost(const ReferencePoint & ref_point) const; }; - +} // namespace obstacle_avoidance_planner #endif // OBSTACLE_AVOIDANCE_PLANNER__MPT_OPTIMIZER_HPP_ diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp index 0f843926e9be2..b51b35fa99d49 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp @@ -1,4 +1,4 @@ -// Copyright 2020 Tier IV, Inc. +// Copyright 2023 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -11,305 +11,126 @@ // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. + #ifndef OBSTACLE_AVOIDANCE_PLANNER__NODE_HPP_ #define OBSTACLE_AVOIDANCE_PLANNER__NODE_HPP_ -#include "motion_utils/trajectory/trajectory.hpp" +#include "motion_utils/motion_utils.hpp" #include "obstacle_avoidance_planner/common_structs.hpp" -#include "obstacle_avoidance_planner/eb_path_optimizer.hpp" +#include "obstacle_avoidance_planner/eb_path_smoother.hpp" #include "obstacle_avoidance_planner/mpt_optimizer.hpp" -#include "opencv2/core.hpp" -#include "rclcpp/clock.hpp" +#include "obstacle_avoidance_planner/replan_checker.hpp" +#include "obstacle_avoidance_planner/type_alias.hpp" #include "rclcpp/rclcpp.hpp" -#include "tier4_autoware_utils/ros/self_pose_listener.hpp" -#include "tier4_autoware_utils/system/stop_watch.hpp" - -#include "autoware_auto_perception_msgs/msg/predicted_objects.hpp" -#include "autoware_auto_planning_msgs/msg/path.hpp" -#include "autoware_auto_planning_msgs/msg/path_point.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" -#include "geometry_msgs/msg/point.hpp" -#include "geometry_msgs/msg/pose.hpp" -#include "geometry_msgs/msg/twist.hpp" -#include "geometry_msgs/msg/twist_stamped.hpp" -#include "nav_msgs/msg/map_meta_data.hpp" -#include "nav_msgs/msg/occupancy_grid.hpp" -#include "nav_msgs/msg/odometry.hpp" -#include "tier4_debug_msgs/msg/string_stamped.hpp" -#include "tier4_planning_msgs/msg/enable_avoidance.hpp" -#include "visualization_msgs/msg/marker_array.hpp" - -#include "boost/optional.hpp" +#include "tier4_autoware_utils/tier4_autoware_utils.hpp" +#include "vehicle_info_util/vehicle_info_util.hpp" #include #include +#include #include #include -using autoware_auto_perception_msgs::msg::PredictedObject; -using autoware_auto_perception_msgs::msg::PredictedObjects; -using autoware_auto_planning_msgs::msg::Path; -using autoware_auto_planning_msgs::msg::PathPoint; -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; -using nav_msgs::msg::OccupancyGrid; -using nav_msgs::msg::Odometry; - -namespace +namespace obstacle_avoidance_planner { -template -boost::optional lerpPose( - const T & points, const geometry_msgs::msg::Point & target_pos, const size_t closest_seg_idx) -{ - constexpr double epsilon = 1e-6; - - const double closest_to_target_dist = - motion_utils::calcSignedArcLength(points, closest_seg_idx, target_pos); - const double seg_dist = - motion_utils::calcSignedArcLength(points, closest_seg_idx, closest_seg_idx + 1); - - const auto & closest_pose = points[closest_seg_idx].pose; - const auto & next_pose = points[closest_seg_idx + 1].pose; - - geometry_msgs::msg::Pose interpolated_pose; - if (std::abs(seg_dist) < epsilon) { - interpolated_pose.position.x = next_pose.position.x; - interpolated_pose.position.y = next_pose.position.y; - interpolated_pose.position.z = next_pose.position.z; - interpolated_pose.orientation = next_pose.orientation; - } else { - const double ratio = closest_to_target_dist / seg_dist; - if (ratio < 0 || 1 < ratio) { - return {}; - } - - interpolated_pose.position.x = - interpolation::lerp(closest_pose.position.x, next_pose.position.x, ratio); - interpolated_pose.position.y = - interpolation::lerp(closest_pose.position.y, next_pose.position.y, ratio); - interpolated_pose.position.z = - interpolation::lerp(closest_pose.position.z, next_pose.position.z, ratio); - - const double closest_yaw = tf2::getYaw(closest_pose.orientation); - const double next_yaw = tf2::getYaw(next_pose.orientation); - const double interpolated_yaw = interpolation::lerp(closest_yaw, next_yaw, ratio); - interpolated_pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(interpolated_yaw); - } - return interpolated_pose; -} - -template -double lerpTwistX( - const T & points, const geometry_msgs::msg::Point & target_pos, const size_t closest_seg_idx) -{ - if (points.size() == 1) { - return points.at(0).longitudinal_velocity_mps; - } - - constexpr double epsilon = 1e-6; - - const double closest_to_target_dist = - motion_utils::calcSignedArcLength(points, closest_seg_idx, target_pos); - const double seg_dist = - motion_utils::calcSignedArcLength(points, closest_seg_idx, closest_seg_idx + 1); - - const double closest_vel = points[closest_seg_idx].longitudinal_velocity_mps; - const double next_vel = points[closest_seg_idx + 1].longitudinal_velocity_mps; - - if (std::abs(seg_dist) < epsilon) { - return next_vel; - } - - const double ratio = std::min(1.0, std::max(0.0, closest_to_target_dist / seg_dist)); - return interpolation::lerp(closest_vel, next_vel, ratio); -} - -template -double lerpPoseZ( - const T & points, const geometry_msgs::msg::Point & target_pos, const size_t closest_seg_idx) -{ - if (points.size() == 1) { - return points.at(0).pose.position.z; - } - - constexpr double epsilon = 1e-6; - - const double closest_to_target_dist = - motion_utils::calcSignedArcLength(points, closest_seg_idx, target_pos); - const double seg_dist = - motion_utils::calcSignedArcLength(points, closest_seg_idx, closest_seg_idx + 1); - - const double closest_z = points[closest_seg_idx].pose.position.z; - const double next_z = points[closest_seg_idx + 1].pose.position.z; - - return std::abs(seg_dist) < epsilon - ? next_z - : interpolation::lerp(closest_z, next_z, closest_to_target_dist / seg_dist); -} -} // namespace - class ObstacleAvoidancePlanner : public rclcpp::Node { public: - struct PlannerData - { - Path path; - geometry_msgs::msg::Pose ego_pose; - double ego_vel; - std::vector objects; - }; - explicit ObstacleAvoidancePlanner(const rclcpp::NodeOptions & node_options); -private: - rclcpp::Clock logger_ros_clock_; - int eb_solved_count_; - bool is_driving_forward_{true}; - - bool is_publishing_debug_visualization_marker_; - bool is_publishing_area_with_objects_; - bool is_publishing_object_clearance_map_; - bool is_publishing_clearance_map_; - bool is_showing_debug_info_; - bool is_showing_calculation_time_; - bool is_stopping_if_outside_drivable_area_; - bool enable_avoidance_; - bool enable_pre_smoothing_; - bool skip_optimization_; - bool reset_prev_optimization_; - bool is_considering_footprint_edges_; - - // vehicle stop margin - double vehicle_stop_margin_outside_drivable_area_; - - // vehicle circles info for for mpt constraints - std::string vehicle_circle_method_; - int vehicle_circle_num_for_calculation_; - std::vector vehicle_circle_radius_ratios_; - - // params for replan - double max_path_shape_change_dist_for_replan_; - double max_ego_moving_dist_for_replan_; - double max_delta_time_sec_for_replan_; - - // core algorithm - std::unique_ptr eb_path_optimizer_ptr_; - std::unique_ptr mpt_optimizer_ptr_; - - // params - TrajectoryParam traj_param_; - EBParam eb_param_; - VehicleParam vehicle_param_; - MPTParam mpt_param_; - int mpt_visualize_sampling_num_; +protected: // for the static_centerline_optimizer package + // TODO(murooka) move this node to common + class DrivingDirectionChecker + { + public: + bool isDrivingForward(const std::vector & path_points) + { + const auto is_driving_forward = motion_utils::isDrivingForward(path_points); + is_driving_forward_ = is_driving_forward ? is_driving_forward.get() : is_driving_forward_; + return is_driving_forward_; + } - // variables for debug - mutable DebugData debug_data_; - mutable tier4_autoware_utils::StopWatch< - std::chrono::milliseconds, std::chrono::microseconds, std::chrono::steady_clock> - stop_watch_; + private: + bool is_driving_forward_{true}; + }; + DrivingDirectionChecker driving_direction_checker_{}; + + // argument variables + vehicle_info_util::VehicleInfo vehicle_info_{}; + mutable std::shared_ptr debug_data_ptr_{nullptr}; + mutable std::shared_ptr time_keeper_ptr_{nullptr}; + + // flags for some functions + bool enable_pub_debug_marker_; + bool enable_debug_info_; + bool enable_outside_drivable_area_stop_; + bool enable_smoothing_; + bool enable_skip_optimization_; + bool enable_reset_prev_optimization_; + bool use_footprint_polygon_for_outside_drivable_area_check_; + + // core algorithms + std::shared_ptr replan_checker_ptr_{nullptr}; + std::shared_ptr eb_path_smoother_ptr_{nullptr}; + std::shared_ptr mpt_optimizer_ptr_{nullptr}; + + // parameters + TrajectoryParam traj_param_{}; + EgoNearestParam ego_nearest_param_{}; // variables for subscribers - std::unique_ptr current_odometry_ptr_; - std::unique_ptr objects_ptr_; + Odometry::SharedPtr ego_state_ptr_; // variables for previous information - std::unique_ptr prev_ego_pose_ptr_; - std::unique_ptr prev_optimal_trajs_ptr_; - std::unique_ptr> prev_path_points_ptr_; - std::unique_ptr prev_replanned_time_ptr_; + std::shared_ptr> prev_optimized_traj_points_ptr_; - // ROS + // interface publisher rclcpp::Publisher::SharedPtr traj_pub_; - rclcpp::Publisher::SharedPtr debug_extended_fixed_traj_pub_; - rclcpp::Publisher::SharedPtr debug_extended_non_fixed_traj_pub_; - rclcpp::Publisher::SharedPtr debug_eb_traj_pub_; - rclcpp::Publisher::SharedPtr debug_mpt_fixed_traj_pub_; - rclcpp::Publisher::SharedPtr debug_mpt_ref_traj_pub_; - rclcpp::Publisher::SharedPtr debug_mpt_traj_pub_; - - rclcpp::Publisher::SharedPtr debug_markers_pub_; - rclcpp::Publisher::SharedPtr debug_wall_markers_pub_; - rclcpp::Publisher::SharedPtr debug_clearance_map_pub_; - rclcpp::Publisher::SharedPtr debug_object_clearance_map_pub_; - rclcpp::Publisher::SharedPtr debug_area_with_objects_pub_; - rclcpp::Publisher::SharedPtr debug_msg_pub_; + rclcpp::Publisher::SharedPtr virtual_wall_pub_; + // interface subscriber rclcpp::Subscription::SharedPtr path_sub_; rclcpp::Subscription::SharedPtr odom_sub_; - rclcpp::Subscription::SharedPtr objects_sub_; - rclcpp::Subscription::SharedPtr is_avoidance_sub_; - // callback function for dynamic parameters + // debug publisher + rclcpp::Publisher::SharedPtr debug_extended_traj_pub_; + rclcpp::Publisher::SharedPtr debug_markers_pub_; + rclcpp::Publisher::SharedPtr debug_calculation_time_pub_; + + // parameter callback rcl_interfaces::msg::SetParametersResult onParam( const std::vector & parameters); OnSetParametersCallbackHandle::SharedPtr set_param_res_; - // subscriber callback functions - void onOdometry(const Odometry::SharedPtr); - void onObjects(const PredictedObjects::SharedPtr); - void onEnableAvoidance(const tier4_planning_msgs::msg::EnableAvoidance::SharedPtr); + // subscriber callback function void onPath(const Path::SharedPtr); - // functions - void resetPlanning(); - void resetPrevOptimization(); + // reset functions + void initializePlanning(); + void resetPreviousData(); + // main functions + bool isDataReady(const Path & path, rclcpp::Clock clock) const; + PlannerData createPlannerData(const Path & path) const; std::vector generateOptimizedTrajectory(const PlannerData & planner_data); - - // functions for replan - bool checkReplan(const PlannerData & planner_data); - bool isPathShapeChanged(const PlannerData & planner_data); - bool isPathGoalChanged(const PlannerData & planner_data); - bool isEgoNearToPrevTrajectory(const geometry_msgs::msg::Pose & ego_pose); - - Trajectory generateTrajectory(const PlannerData & planner_data); - - Trajectories optimizeTrajectory(const PlannerData & planner_data); - - Trajectories getPrevTrajs(const std::vector & path_points) const; - - void calcVelocity( - const std::vector & path_points, std::vector & traj_points) const; - - void insertZeroVelocityOutsideDrivableArea( - const PlannerData & planner_data, std::vector & traj_points); - - void publishDebugDataInOptimization( - const PlannerData & planner_data, const std::vector & traj_points); - - Trajectories makePrevTrajectories( - const std::vector & path_points, const Trajectories & trajs, - const PlannerData & planner_data); - - std::vector generatePostProcessedTrajectory( - const std::vector & path_points, - const std::vector & merged_optimized_points, const PlannerData & planner_data); - - std::vector getExtendedTrajectory( - const std::vector & path_points, - const std::vector & optimized_points); - - std::vector generateFineTrajectoryPoints( - const std::vector & path_points, - const std::vector & traj_points) const; - - std::vector alignVelocity( - const std::vector & fine_traj_points, - const std::vector & path_points, + std::vector extendTrajectory( + const std::vector & traj_points, + const std::vector & optimized_points) const; + void publishDebugData(const Header & header) const; + + // functions in generateOptimizedTrajectory + std::vector optimizeTrajectory(const PlannerData & planner_data); + std::vector getPrevOptimizedTrajectory( const std::vector & traj_points) const; - - void publishDebugDataInMain(const Path & path) const; - - template - size_t findEgoNearestIndex( - const std::vector & points, const geometry_msgs::msg::Pose & ego_pose) - { - return motion_utils::findFirstNearestIndexWithSoftConstraints( - points, ego_pose, traj_param_.ego_nearest_dist_threshold, - traj_param_.ego_nearest_yaw_threshold); - } + void applyInputVelocity( + std::vector & output_traj_points, + const std::vector & input_traj_points, + const geometry_msgs::msg::Pose & ego_pose) const; + void insertZeroVelocityOutsideDrivableArea( + const PlannerData & planner_data, std::vector & traj_points) const; + void publishVirtualWall(const geometry_msgs::msg::Pose & stop_pose) const; + void publishDebugMarkerOfOptimization(const std::vector & traj_points) const; }; +} // namespace obstacle_avoidance_planner #endif // OBSTACLE_AVOIDANCE_PLANNER__NODE_HPP_ diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/replan_checker.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/replan_checker.hpp new file mode 100644 index 0000000000000..6b456a6757626 --- /dev/null +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/replan_checker.hpp @@ -0,0 +1,63 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef OBSTACLE_AVOIDANCE_PLANNER__REPLAN_CHECKER_HPP_ +#define OBSTACLE_AVOIDANCE_PLANNER__REPLAN_CHECKER_HPP_ + +#include "obstacle_avoidance_planner/common_structs.hpp" +#include "obstacle_avoidance_planner/mpt_optimizer.hpp" +#include "obstacle_avoidance_planner/type_alias.hpp" + +#include + +#include +#include + +namespace obstacle_avoidance_planner +{ +class ReplanChecker +{ +public: + explicit ReplanChecker(rclcpp::Node * node, const EgoNearestParam & ego_nearest_param); + void onParam(const std::vector & parameters); + + bool isResetRequired(const PlannerData & planner_data); + + bool isReplanRequired(const rclcpp::Time & current_time); + +private: + EgoNearestParam ego_nearest_param_; + rclcpp::Logger logger_; + + // previous variables for isResetRequired + std::shared_ptr> prev_traj_points_ptr_{nullptr}; + std::shared_ptr prev_ego_pose_ptr_{nullptr}; + + // previous variable for isReplanRequired + std::shared_ptr prev_replanned_time_ptr_{nullptr}; + + // algorithm parameters + double max_path_shape_around_ego_lat_dist_; + double max_ego_moving_dist_; + double max_goal_moving_dist_; + double max_delta_time_sec_; + + bool isPathAroundEgoChanged( + const PlannerData & planner_data, const std::vector & prev_traj_points) const; + bool isPathGoalChanged( + const PlannerData & planner_data, const std::vector & prev_traj_points) const; +}; +} // namespace obstacle_avoidance_planner + +#endif // OBSTACLE_AVOIDANCE_PLANNER__REPLAN_CHECKER_HPP_ diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/state_equation_generator.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/state_equation_generator.hpp new file mode 100644 index 0000000000000..d4f70e8e09227 --- /dev/null +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/state_equation_generator.hpp @@ -0,0 +1,61 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef OBSTACLE_AVOIDANCE_PLANNER__STATE_EQUATION_GENERATOR_HPP_ +#define OBSTACLE_AVOIDANCE_PLANNER__STATE_EQUATION_GENERATOR_HPP_ + +#include "obstacle_avoidance_planner/common_structs.hpp" +#include "obstacle_avoidance_planner/vehicle_model/vehicle_model_bicycle_kinematics.hpp" +#include "obstacle_avoidance_planner/vehicle_model/vehicle_model_interface.hpp" + +#include +#include + +namespace obstacle_avoidance_planner +{ +struct ReferencePoint; + +class StateEquationGenerator +{ +public: + struct Matrix + { + Eigen::MatrixXd B; + Eigen::VectorXd W; + }; + + StateEquationGenerator() = default; + StateEquationGenerator( + const double wheel_base, const double max_steer_rad, + const std::shared_ptr time_keeper_ptr) + : vehicle_model_ptr_(std::make_unique(wheel_base, max_steer_rad)), + time_keeper_ptr_(time_keeper_ptr) + { + } + + int getDimX() const { return vehicle_model_ptr_->getDimX(); } + int getDimU() const { return vehicle_model_ptr_->getDimU(); } + + // time-series state equation: x = B u + W (u includes x_0) + // NOTE: one-step state equation is x_t+1 = Ad x_t + Bd u + Wd. + Matrix calcMatrix(const std::vector & ref_points) const; + + Eigen::VectorXd predict(const Matrix & mat, const Eigen::VectorXd U) const; + +private: + std::unique_ptr vehicle_model_ptr_; + mutable std::shared_ptr time_keeper_ptr_; +}; +} // namespace obstacle_avoidance_planner +#endif // OBSTACLE_AVOIDANCE_PLANNER__STATE_EQUATION_GENERATOR_HPP_ diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/type_alias.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/type_alias.hpp new file mode 100644 index 0000000000000..0f45e1223551f --- /dev/null +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/type_alias.hpp @@ -0,0 +1,48 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef OBSTACLE_AVOIDANCE_PLANNER__TYPE_ALIAS_HPP_ +#define OBSTACLE_AVOIDANCE_PLANNER__TYPE_ALIAS_HPP_ + +#include "autoware_auto_planning_msgs/msg/path.hpp" +#include "autoware_auto_planning_msgs/msg/path_point.hpp" +#include "autoware_auto_planning_msgs/msg/trajectory.hpp" +#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" +#include "geometry_msgs/msg/point.hpp" +#include "geometry_msgs/msg/pose.hpp" +#include "geometry_msgs/msg/twist.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "std_msgs/msg/header.hpp" +#include "tier4_debug_msgs/msg/string_stamped.hpp" +#include "visualization_msgs/msg/marker_array.hpp" + +namespace obstacle_avoidance_planner +{ +// std_msgs +using std_msgs::msg::Header; +// planning +using autoware_auto_planning_msgs::msg::Path; +using autoware_auto_planning_msgs::msg::PathPoint; +using autoware_auto_planning_msgs::msg::Trajectory; +using autoware_auto_planning_msgs::msg::TrajectoryPoint; +// navigation +using nav_msgs::msg::Odometry; +// visualization +using visualization_msgs::msg::Marker; +using visualization_msgs::msg::MarkerArray; +// debug +using tier4_debug_msgs::msg::StringStamped; +} // namespace obstacle_avoidance_planner + +#endif // OBSTACLE_AVOIDANCE_PLANNER__TYPE_ALIAS_HPP_ diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/debug_utils.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/debug_utils.hpp deleted file mode 100644 index 506587f63f5e6..0000000000000 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/debug_utils.hpp +++ /dev/null @@ -1,47 +0,0 @@ -// Copyright 2020 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -#ifndef OBSTACLE_AVOIDANCE_PLANNER__UTILS__DEBUG_UTILS_HPP_ -#define OBSTACLE_AVOIDANCE_PLANNER__UTILS__DEBUG_UTILS_HPP_ - -#include "obstacle_avoidance_planner/common_structs.hpp" -#include "opencv2/core.hpp" -#include "rclcpp/clock.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" -#include "tier4_autoware_utils/ros/marker_helper.hpp" - -#include "autoware_auto_perception_msgs/msg/predicted_object.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" -#include "geometry_msgs/msg/pose.hpp" -#include "nav_msgs/msg/occupancy_grid.hpp" -#include "visualization_msgs/msg/marker_array.hpp" - -#include -#include -#include - -using tier4_autoware_utils::createMarkerColor; -using tier4_autoware_utils::createMarkerScale; - -namespace debug_utils -{ -visualization_msgs::msg::MarkerArray getDebugVisualizationMarker( - DebugData & debug_data, - const std::vector & optimized_points, - const VehicleParam & vehicle_param, const bool is_showing_debug_detail); - -visualization_msgs::msg::MarkerArray getDebugVisualizationWallMarker( - DebugData & debug_data, const VehicleParam & vehicle_param); -} // namespace debug_utils - -#endif // OBSTACLE_AVOIDANCE_PLANNER__UTILS__DEBUG_UTILS_HPP_ diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/geometry_utils.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/geometry_utils.hpp new file mode 100644 index 0000000000000..6673058ba59a2 --- /dev/null +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/geometry_utils.hpp @@ -0,0 +1,71 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef OBSTACLE_AVOIDANCE_PLANNER__UTILS__GEOMETRY_UTILS_HPP_ +#define OBSTACLE_AVOIDANCE_PLANNER__UTILS__GEOMETRY_UTILS_HPP_ + +#include "eigen3/Eigen/Core" +#include "interpolation/linear_interpolation.hpp" +#include "interpolation/spline_interpolation.hpp" +#include "interpolation/spline_interpolation_points_2d.hpp" +#include "motion_utils/trajectory/trajectory.hpp" +#include "obstacle_avoidance_planner/common_structs.hpp" +#include "obstacle_avoidance_planner/type_alias.hpp" +#include "vehicle_info_util/vehicle_info_util.hpp" + +#include "autoware_auto_planning_msgs/msg/path_point.hpp" +#include "autoware_auto_planning_msgs/msg/trajectory.hpp" + +#include "boost/optional/optional_fwd.hpp" + +#include +#include +#include +#include +#include + +namespace tier4_autoware_utils +{ +template <> +geometry_msgs::msg::Point getPoint(const obstacle_avoidance_planner::ReferencePoint & p); + +template <> +geometry_msgs::msg::Pose getPose(const obstacle_avoidance_planner::ReferencePoint & p); +} // namespace tier4_autoware_utils + +namespace obstacle_avoidance_planner +{ +namespace geometry_utils +{ +template +bool isSamePoint(const T1 & t1, const T2 & t2) +{ + const auto p1 = tier4_autoware_utils::getPoint(t1); + const auto p2 = tier4_autoware_utils::getPoint(t2); + + constexpr double epsilon = 1e-6; + if (epsilon < std::abs(p1.x - p2.x) || epsilon < std::abs(p1.y - p2.y)) { + return false; + } + return true; +} + +bool isOutsideDrivableAreaFromRectangleFootprint( + const geometry_msgs::msg::Pose & pose, const std::vector & left_bound, + const std::vector & right_bound, + const vehicle_info_util::VehicleInfo & vehicle_info, + const bool use_footprint_polygon_for_outside_drivable_area_check); +} // namespace geometry_utils +} // namespace obstacle_avoidance_planner +#endif // OBSTACLE_AVOIDANCE_PLANNER__UTILS__GEOMETRY_UTILS_HPP_ diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/trajectory_utils.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/trajectory_utils.hpp new file mode 100644 index 0000000000000..ba72dadc35608 --- /dev/null +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/trajectory_utils.hpp @@ -0,0 +1,284 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef OBSTACLE_AVOIDANCE_PLANNER__UTILS__TRAJECTORY_UTILS_HPP_ +#define OBSTACLE_AVOIDANCE_PLANNER__UTILS__TRAJECTORY_UTILS_HPP_ + +#include "eigen3/Eigen/Core" +#include "interpolation/linear_interpolation.hpp" +#include "interpolation/spline_interpolation.hpp" +#include "interpolation/spline_interpolation_points_2d.hpp" +#include "motion_utils/trajectory/trajectory.hpp" +#include "obstacle_avoidance_planner/common_structs.hpp" +#include "obstacle_avoidance_planner/type_alias.hpp" + +#include "autoware_auto_planning_msgs/msg/path_point.hpp" +#include "autoware_auto_planning_msgs/msg/trajectory.hpp" + +#include "boost/optional/optional_fwd.hpp" + +#include +#include +#include +#include +#include + +namespace tier4_autoware_utils +{ +template <> +geometry_msgs::msg::Point getPoint(const obstacle_avoidance_planner::ReferencePoint & p); + +template <> +geometry_msgs::msg::Pose getPose(const obstacle_avoidance_planner::ReferencePoint & p); + +template <> +double getLongitudinalVelocity(const obstacle_avoidance_planner::ReferencePoint & p); +} // namespace tier4_autoware_utils + +namespace obstacle_avoidance_planner +{ +namespace trajectory_utils +{ +template +std::optional getPointIndexAfter( + const T & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, + const double offset) +{ + if (points.empty()) { + return std::nullopt; + } + + double sum_length = + -motion_utils::calcLongitudinalOffsetToSegment(points, target_seg_idx, target_pos); + + // search forward + if (sum_length < offset) { + for (size_t i = target_seg_idx + 1; i < points.size(); ++i) { + sum_length += tier4_autoware_utils::calcDistance2d(points.at(i), points.at(i - 1)); + if (offset < sum_length) { + return i - 1; + } + } + + return std::nullopt; + } + + // search backward + for (size_t i = target_seg_idx; 0 < i; + --i) { // NOTE: use size_t since i is always positive value + sum_length -= tier4_autoware_utils::calcDistance2d(points.at(i), points.at(i - 1)); + if (sum_length < offset) { + return i - 1; + } + } + + return 0; +} + +template +T cropForwardPoints( + const T & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, + const double forward_length) +{ + if (points.empty()) { + return T{}; + } + + double sum_length = + -motion_utils::calcLongitudinalOffsetToSegment(points, target_seg_idx, target_pos); + for (size_t i = target_seg_idx + 1; i < points.size(); ++i) { + sum_length += tier4_autoware_utils::calcDistance2d(points.at(i), points.at(i - 1)); + if (forward_length < sum_length) { + const size_t end_idx = i; + return T{points.begin(), points.begin() + end_idx}; + } + } + + return points; +} + +template +T cropBackwardPoints( + const T & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, + const double backward_length) +{ + if (points.empty()) { + return T{}; + } + + double sum_length = + -motion_utils::calcLongitudinalOffsetToSegment(points, target_seg_idx, target_pos); + for (size_t i = target_seg_idx; 0 < i; + --i) { // NOTE: use size_t since i is always positive value + sum_length -= tier4_autoware_utils::calcDistance2d(points.at(i), points.at(i - 1)); + if (sum_length < backward_length) { + const size_t begin_idx = i - 1; + return T{points.begin() + begin_idx, points.end()}; + } + } + + return points; +} + +template +T cropPoints( + const T & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, + const double forward_length, const double backward_length) +{ + if (points.empty()) { + return T{}; + } + + // NOTE: Cropping forward must be done first in order to keep target_seg_idx. + const auto cropped_forward_points = + cropForwardPoints(points, target_pos, target_seg_idx, forward_length); + + const size_t modified_target_seg_idx = + std::min(target_seg_idx, cropped_forward_points.size() - 2); + const auto cropped_points = cropBackwardPoints( + cropped_forward_points, target_pos, modified_target_seg_idx, backward_length); + + if (cropped_points.size() < 2) { + RCLCPP_ERROR( + rclcpp::get_logger("obstacle_avoidance_planner.trajectory_utils"), + ". Return original points since cropped_points size is less than 2."); + return points; + } + + return cropped_points; +} + +template +TrajectoryPoint convertToTrajectoryPoint(const T & point) +{ + TrajectoryPoint traj_point; + traj_point.pose = tier4_autoware_utils::getPose(point); + traj_point.longitudinal_velocity_mps = point.longitudinal_velocity_mps; + traj_point.lateral_velocity_mps = point.lateral_velocity_mps; + traj_point.heading_rate_rps = point.heading_rate_rps; + return traj_point; +} + +template <> +inline TrajectoryPoint convertToTrajectoryPoint(const ReferencePoint & ref_point) +{ + TrajectoryPoint traj_point; + traj_point.pose = tier4_autoware_utils::getPose(ref_point); + traj_point.longitudinal_velocity_mps = tier4_autoware_utils::getLongitudinalVelocity(ref_point); + return traj_point; +} + +template +std::vector convertToTrajectoryPoints(const std::vector & points) +{ + std::vector traj_points; + for (const auto & point : points) { + const auto traj_point = convertToTrajectoryPoint(point); + traj_points.push_back(traj_point); + } + return traj_points; +} + +ReferencePoint convertToReferencePoint(const TrajectoryPoint & traj_point); +std::vector convertToReferencePoints( + const std::vector & traj_points); + +void compensateLastPose( + const PathPoint & last_path_point, std::vector & traj_points, + const double delta_dist_threshold, const double delta_yaw_threshold); + +geometry_msgs::msg::Point getNearestPosition( + const std::vector & points, const int target_idx, const double offset); + +template +size_t findEgoIndex( + const std::vector & points, const geometry_msgs::msg::Pose & ego_pose, + const EgoNearestParam & ego_nearest_param) +{ + return motion_utils::findFirstNearestIndexWithSoftConstraints( + points, ego_pose, ego_nearest_param.dist_threshold, ego_nearest_param.yaw_threshold); +} + +template +size_t findEgoSegmentIndex( + const std::vector & points, const geometry_msgs::msg::Pose & ego_pose, + const EgoNearestParam & ego_nearest_param) +{ + return motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + points, ego_pose, ego_nearest_param.dist_threshold, ego_nearest_param.yaw_threshold); +} + +Trajectory createTrajectory( + const std_msgs::msg::Header & header, const std::vector & traj_points); + +std::vector resampleTrajectoryPoints( + const std::vector traj_points, const double interval); + +std::vector resampleTrajectoryPointsWithoutStopPoint( + const std::vector traj_points, const double interval); + +std::vector resampleReferencePoints( + const std::vector ref_points, const double interval); + +template +std::optional updateFrontPointForFix( + std::vector & points, std::vector & points_for_fix, const double delta_arc_length, + const EgoNearestParam & ego_nearest_param) +{ + // calculate front point to insert in points as a fixed point + const size_t front_seg_idx_for_fix = trajectory_utils::findEgoSegmentIndex( + points_for_fix, tier4_autoware_utils::getPose(points.front()), ego_nearest_param); + const size_t front_point_idx_for_fix = front_seg_idx_for_fix; + const auto & front_fix_point = points_for_fix.at(front_point_idx_for_fix); + + // check if the points_for_fix is longer in front than points + const double lon_offset_to_prev_front = + motion_utils::calcSignedArcLength(points, 0, front_fix_point.pose.position); + if (0 < lon_offset_to_prev_front) { + RCLCPP_WARN( + rclcpp::get_logger("obstacle_avoidance_planner.trajectory_utils"), + "Fixed point will not be inserted due to the error during calculation."); + return std::nullopt; + } + + const double dist = tier4_autoware_utils::calcDistance2d(points.front(), front_fix_point); + + // check if deviation is not too large + constexpr double max_lat_error = 3.0; + if (max_lat_error < dist) { + RCLCPP_WARN( + rclcpp::get_logger("obstacle_avoidance_planner.trajectory_utils"), + "New Fixed point is too far from points %f [m]", dist); + } + + // update pose + if (dist < delta_arc_length) { + // only pose is updated + points.front() = front_fix_point; + } else { + // add new front point + T new_front_point; + new_front_point = front_fix_point; + points.insert(points.begin(), new_front_point); + } + + return front_point_idx_for_fix; +} + +void insertStopPoint( + std::vector & traj_points, const geometry_msgs::msg::Pose & input_stop_pose, + const size_t stop_seg_idx); +} // namespace trajectory_utils +} // namespace obstacle_avoidance_planner +#endif // OBSTACLE_AVOIDANCE_PLANNER__UTILS__TRAJECTORY_UTILS_HPP_ diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/utils.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/utils.hpp deleted file mode 100644 index e6c6c2d4dc467..0000000000000 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/utils.hpp +++ /dev/null @@ -1,366 +0,0 @@ -// Copyright 2020 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef OBSTACLE_AVOIDANCE_PLANNER__UTILS__UTILS_HPP_ -#define OBSTACLE_AVOIDANCE_PLANNER__UTILS__UTILS_HPP_ - -#include "eigen3/Eigen/Core" -#include "interpolation/linear_interpolation.hpp" -#include "interpolation/spline_interpolation.hpp" -#include "interpolation/spline_interpolation_points_2d.hpp" -#include "motion_utils/trajectory/trajectory.hpp" -#include "obstacle_avoidance_planner/common_structs.hpp" - -#include "autoware_auto_planning_msgs/msg/path_point.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" - -#include "boost/optional/optional_fwd.hpp" - -#include -#include -#include -#include -#include - -struct ReferencePoint; - -namespace tier4_autoware_utils -{ -template <> -geometry_msgs::msg::Point getPoint(const ReferencePoint & p); - -template <> -geometry_msgs::msg::Pose getPose(const ReferencePoint & p); -} // namespace tier4_autoware_utils - -namespace geometry_utils -{ -template -geometry_msgs::msg::Point transformToRelativeCoordinate2D( - const T & point, const geometry_msgs::msg::Pose & origin) -{ - // NOTE: implement transformation without defining yaw variable - // but directly sin/cos of yaw for fast calculation - const auto & q = origin.orientation; - const double cos_yaw = 1 - 2 * q.z * q.z; - const double sin_yaw = 2 * q.w * q.z; - - geometry_msgs::msg::Point relative_p; - const double tmp_x = point.x - origin.position.x; - const double tmp_y = point.y - origin.position.y; - relative_p.x = tmp_x * cos_yaw + tmp_y * sin_yaw; - relative_p.y = -tmp_x * sin_yaw + tmp_y * cos_yaw; - relative_p.z = point.z; - - return relative_p; -} - -geometry_msgs::msg::Point transformToAbsoluteCoordinate2D( - const geometry_msgs::msg::Point & point, const geometry_msgs::msg::Pose & origin); - -geometry_msgs::msg::Quaternion getQuaternionFromPoints( - const geometry_msgs::msg::Point & a, const geometry_msgs::msg::Point & a_root); - -geometry_msgs::msg::Quaternion getQuaternionFromPoints( - const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2, - const geometry_msgs::msg::Point & p3, const geometry_msgs::msg::Point & p4); - -template -geometry_msgs::msg::Point transformMapToImage( - const T & map_point, const nav_msgs::msg::MapMetaData & occupancy_grid_info) -{ - geometry_msgs::msg::Point relative_p = - transformToRelativeCoordinate2D(map_point, occupancy_grid_info.origin); - double resolution = occupancy_grid_info.resolution; - double map_y_height = occupancy_grid_info.height; - double map_x_width = occupancy_grid_info.width; - double map_x_in_image_resolution = relative_p.x / resolution; - double map_y_in_image_resolution = relative_p.y / resolution; - geometry_msgs::msg::Point image_point; - image_point.x = map_y_height - map_y_in_image_resolution; - image_point.y = map_x_width - map_x_in_image_resolution; - return image_point; -} - -boost::optional transformMapToOptionalImage( - const geometry_msgs::msg::Point & map_point, - const nav_msgs::msg::MapMetaData & occupancy_grid_info); - -bool transformMapToImage( - const geometry_msgs::msg::Point & map_point, - const nav_msgs::msg::MapMetaData & occupancy_grid_info, geometry_msgs::msg::Point & image_point); -} // namespace geometry_utils - -namespace interpolation_utils -{ -std::vector interpolate2DPoints( - const std::vector & base_x, const std::vector & base_y, const double resolution, - const double offset); - -std::vector interpolateConnected2DPoints( - const std::vector & base_x, const std::vector & base_y, const double resolution, - const double begin_yaw, const double end_yaw); - -std::vector interpolate2DTrajectoryPoints( - const std::vector & base_x, const std::vector & base_y, - const std::vector & base_yaw, const double resolution); - -std::vector interpolate2DTrajectoryPoints( - const std::vector & base_x, const std::vector & base_y, const double resolution); - -template -std::vector getInterpolatedPoints( - const T & points, const double delta_arc_length, const double offset = 0) -{ - constexpr double epsilon = 1e-6; - std::vector tmp_x; - std::vector tmp_y; - for (size_t i = 0; i < points.size(); i++) { - const auto & current_point = tier4_autoware_utils::getPoint(points.at(i)); - if (i > 0) { - const auto & prev_point = tier4_autoware_utils::getPoint(points.at(i - 1)); - if ( - std::fabs(current_point.x - prev_point.x) < epsilon && - std::fabs(current_point.y - prev_point.y) < epsilon) { - continue; - } - } - tmp_x.push_back(current_point.x); - tmp_y.push_back(current_point.y); - } - - return interpolation_utils::interpolate2DPoints(tmp_x, tmp_y, delta_arc_length, offset); -} - -std::vector getInterpolatedTrajectoryPoints( - const std::vector & points, - const double delta_arc_length); - -std::vector getConnectedInterpolatedPoints( - const std::vector & points, - const double delta_arc_length, const double begin_yaw, const double end_yaw); -} // namespace interpolation_utils - -namespace points_utils -{ -template -size_t findForwardIndex(const T & points, const size_t begin_idx, const double forward_length) -{ - double sum_length = 0.0; - for (size_t i = begin_idx; i < points.size() - 1; ++i) { - sum_length += tier4_autoware_utils::calcDistance2d(points.at(i), points.at(i + 1)); - if (sum_length > forward_length) { - return i; - } - } - return points.size() - 1; -} - -template -T clipForwardPoints(const T & points, const size_t begin_idx, const double forward_length) -{ - if (points.empty()) { - return T{}; - } - - const size_t end_idx = findForwardIndex(points, begin_idx, forward_length); - return T{points.begin() + begin_idx, points.begin() + end_idx}; -} - -template -T clipBackwardPoints( - const T & points, const size_t target_idx, const double backward_length, - const double delta_length) -{ - if (points.empty()) { - return T{}; - } - - const int begin_idx = - std::max(0, static_cast(target_idx) - static_cast(backward_length / delta_length)); - return T{points.begin() + begin_idx, points.end()}; -} - -template -T clipBackwardPoints( - const T & points, const geometry_msgs::msg::Pose pose, const double backward_length, - const double delta_length, const double delta_yaw) -{ - if (points.empty()) { - return T{}; - } - - const auto target_idx_optional = - motion_utils::findNearestIndex(points, pose, std::numeric_limits::max(), delta_yaw); - - const size_t target_idx = target_idx_optional - ? *target_idx_optional - : motion_utils::findNearestIndex(points, pose.position); - - const int begin_idx = - std::max(0, static_cast(target_idx) - static_cast(backward_length / delta_length)); - return T{points.begin() + begin_idx, points.end()}; -} - -// NOTE: acceleration is not converted -template -std::vector convertToPoints(const std::vector & points) -{ - std::vector geom_points; - for (const auto & point : points) { - geom_points.push_back(tier4_autoware_utils::getPoint(point)); - } - return geom_points; -} - -template -std::vector convertToPoses(const std::vector & points) -{ - std::vector geom_points; - for (const auto & point : points) { - geom_points.push_back(tier4_autoware_utils::getPose(point)); - } - return geom_points; -} - -template -autoware_auto_planning_msgs::msg::TrajectoryPoint convertToTrajectoryPoint(const T & point) -{ - autoware_auto_planning_msgs::msg::TrajectoryPoint traj_point; - traj_point.pose = tier4_autoware_utils::getPose(point); - traj_point.longitudinal_velocity_mps = point.longitudinal_velocity_mps; - traj_point.lateral_velocity_mps = point.lateral_velocity_mps; - traj_point.heading_rate_rps = point.heading_rate_rps; - return traj_point; -} - -template <> -inline autoware_auto_planning_msgs::msg::TrajectoryPoint convertToTrajectoryPoint( - const ReferencePoint & point) -{ - autoware_auto_planning_msgs::msg::TrajectoryPoint traj_point; - traj_point.pose = tier4_autoware_utils::getPose(point); - return traj_point; -} - -// functions to convert to another type of points -template -std::vector convertToTrajectoryPoints( - const std::vector & points) -{ - std::vector traj_points; - for (const auto & point : points) { - const auto traj_point = convertToTrajectoryPoint(point); - traj_points.push_back(traj_point); - } - return traj_points; -} - -template -ReferencePoint convertToReferencePoint(const T & point); - -template -std::vector convertToReferencePoints(const std::vector & points) -{ - std::vector ref_points; - for (const auto & point : points) { - const auto ref_point = convertToReferencePoint(point); - ref_points.push_back(ref_point); - } - return ref_points; -} - -std::vector convertToPosesWithYawEstimation( - const std::vector points); - -std::vector concatTrajectory( - const std::vector & traj_points, - const std::vector & extended_traj_points); - -void compensateLastPose( - const autoware_auto_planning_msgs::msg::PathPoint & last_path_point, - std::vector & traj_points, - const double delta_dist_threshold, const double delta_yaw_threshold); - -template -std::vector calcCurvature(const T & points, const size_t num_sampling_points) -{ - std::vector res(points.size()); - const size_t num_points = static_cast(points.size()); - - /* calculate curvature by circle fitting from three points */ - geometry_msgs::msg::Point p1, p2, p3; - size_t max_smoothing_num = static_cast(std::floor(0.5 * (num_points - 1))); - size_t L = std::min(num_sampling_points, max_smoothing_num); - for (size_t i = L; i < num_points - L; ++i) { - p1 = tier4_autoware_utils::getPoint(points.at(i - L)); - p2 = tier4_autoware_utils::getPoint(points.at(i)); - p3 = tier4_autoware_utils::getPoint(points.at(i + L)); - double den = std::max( - tier4_autoware_utils::calcDistance2d(p1, p2) * tier4_autoware_utils::calcDistance2d(p2, p3) * - tier4_autoware_utils::calcDistance2d(p3, p1), - 0.0001); - const double curvature = - 2.0 * ((p2.x - p1.x) * (p3.y - p1.y) - (p2.y - p1.y) * (p3.x - p1.x)) / den; - res.at(i) = curvature; - } - - /* first and last curvature is copied from next value */ - for (size_t i = 0; i < std::min(L, num_points); ++i) { - res.at(i) = res.at(std::min(L, num_points - 1)); - res.at(num_points - i - 1) = - res.at(std::max(static_cast(num_points) - static_cast(L) - 1, 0)); - } - return res; -} - -int getNearestIdx( - const std::vector & points, const double target_s, const int begin_idx); - -template -bool isNearLastPathPoint( - const T & ref_point, const std::vector & path_points, - const double delta_dist_threshold, const double delta_yaw_threshold) -{ - const geometry_msgs::msg::Pose last_ref_pose = tier4_autoware_utils::getPose(ref_point); - - if ( - tier4_autoware_utils::calcDistance2d(last_ref_pose, path_points.back().pose) > - delta_dist_threshold) { - return false; - } - if ( - std::fabs(tier4_autoware_utils::calcYawDeviation(last_ref_pose, path_points.back().pose)) > - delta_yaw_threshold) { - return false; - } - return true; -} -} // namespace points_utils - -namespace utils -{ -void logOSQPSolutionStatus(const int solution_status, const std::string & msg); -} // namespace utils - -namespace drivable_area_utils -{ -bool isOutsideDrivableAreaFromRectangleFootprint( - const autoware_auto_planning_msgs::msg::TrajectoryPoint & traj_point, - const std::vector & left_bound, - const std::vector & right_bound, const VehicleParam & vehicle_param, - const bool is_considering_footprint_edges); -} - -#endif // OBSTACLE_AVOIDANCE_PLANNER__UTILS__UTILS_HPP_ diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/vehicle_model/vehicle_model_bicycle_kinematics.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/vehicle_model/vehicle_model_bicycle_kinematics.hpp index 6cce41c70f227..13fd2995befb0 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/vehicle_model/vehicle_model_bicycle_kinematics.hpp +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/vehicle_model/vehicle_model_bicycle_kinematics.hpp @@ -1,4 +1,4 @@ -// Copyright 2018-2019 Autoware Foundation +// Copyright 2023 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,39 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -/** - * @file vehicle_model_bicycle_dynamics.h - * @brief vehicle model class of bicycle kinematics - * @author Takamasa Horibe - * @date 2019.05.01 - */ - -/* - * Representation - * e : lateral error - * th : heading angle error - * steer : steering angle - * steer_d: desired steering angle (input) - * v : velocity - * W : wheelbase length - * tau : time constant for steering dynamics - * - * State & Input - * x = [e, th, steer]^T - * u = steer_d - * - * Nonlinear model - * dx1/dt = v * sin(x2) - * dx2/dt = v * tan(x3) / W - * dx3/dt = -(x3 - u) / tau - * - * Linearized model around reference point (v = v_r, th = th_r, steer = steer_r) - * [0, vr, 0] [ 0] [ 0] - * dx/dt = [0, 0, vr/W/cos(steer_r)^2] * x + [ 0] * u + [-vr*steer_r/W/cos(steer_r)^2] - * [0, 0, 1/tau] [1/tau] [ 0] - * - */ - #ifndef OBSTACLE_AVOIDANCE_PLANNER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_HPP_ #define OBSTACLE_AVOIDANCE_PLANNER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_HPP_ @@ -54,51 +21,14 @@ #include -/** - * @class vehicle model class of bicycle kinematics - * @brief calculate model-related values - */ class KinematicsBicycleModel : public VehicleModelInterface { public: - /** - * @brief constructor with parameter initialization - * @param [in] wheelbase wheelbase length [m] - * @param [in] steer_lim steering angle limit [rad] - */ - KinematicsBicycleModel(const double wheel_base, const double steer_limit); - - /** - * @brief destructor - */ + KinematicsBicycleModel(const double wheelbase, const double steer_limit); virtual ~KinematicsBicycleModel() = default; - /** - * @brief calculate discrete kinematics equation matrix of x_k+1 = Ad * x_k + Bd * uk + Wd - * @param [out] Ad coefficient matrix - * @param [out] Bd coefficient matrix - * @param [out] Wd coefficient matrix - * @param [in] ds discretization arc length - */ void calculateStateEquationMatrix( - Eigen::MatrixXd & Ad, Eigen::MatrixXd & Bd, Eigen::MatrixXd & Wd, const double ds) override; - - /** - * @brief calculate discrete observation matrix of y_k = Cd * x_k - * @param [out] Cd coefficient matrix - */ - void calculateObservationMatrix(Eigen::MatrixXd & Cd) override; - - /** - * @brief calculate discrete observation matrix of y_k = Cd * x_k - * @param [out] Cd_vec sparse matrix information of coefficient matrix - */ - void calculateObservationSparseMatrix(std::vector> & Cd_vec) override; - - /** - * @brief calculate reference input - * @param [out] reference input - */ - void calculateReferenceInput(Eigen::MatrixXd * Uref) override; + Eigen::MatrixXd & Ad, Eigen::MatrixXd & Bd, Eigen::MatrixXd & Wd, const double curvature, + const double ds) const override; }; #endif // OBSTACLE_AVOIDANCE_PLANNER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_HPP_ diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp deleted file mode 100644 index 291f35c0e15bb..0000000000000 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp +++ /dev/null @@ -1,93 +0,0 @@ -// Copyright 2018-2019 Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -/** - * @file vehicle_model_bicycle_dynamics_no_delay.h - * @brief vehicle model class of bicycle kinematics without steering delay - * @author Takamasa Horibe - * @date 2019.05.01 - */ - -/* - * Representation - * e : lateral error - * th : heading angle error - * steer : steering angle (input) - * v : velocity - * W : wheelbase length - * - * State & Input - * x = [e, th]^T - * u = steer - * - * Nonlinear model - * dx1/dt = v * sin(x2) - * dx2/dt = v * tan(u) / W - * - * Linearized model around reference point (v = v_r, th = th_r, steer = steer_r) - * dx/dt = [0, vr] * x + [ 0] * u + [ 0] - * [0, 0] [vr/W/cos(steer_r)^2] [-vr*steer_r/W/cos(steer_r)^2] - * - */ - -#ifndef OBSTACLE_AVOIDANCE_PLANNER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_NO_DELAY_HPP_ -#define OBSTACLE_AVOIDANCE_PLANNER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_NO_DELAY_HPP_ - -#include "obstacle_avoidance_planner/vehicle_model/vehicle_model_interface.hpp" - -#include -#include - -/** - * @class vehicle model class of bicycle kinematics without steering delay - * @brief calculate model-related values - */ -class KinematicsBicycleModelNoDelay : public VehicleModelInterface -{ -public: - /** - * @brief constructor with parameter initialization - * @param [in] wheelbase wheelbase length [m] - * @param [in] steer_lim steering angle limit [rad] - */ - KinematicsBicycleModelNoDelay(const double & wheelbase, const double & steer_lim); - - /** - * @brief destructor - */ - ~KinematicsBicycleModelNoDelay() = default; - - /** - * @brief calculate discrete model matrix of x_k+1 = Ad * xk + Bd * uk + Wd, yk = Cd * xk - * @param [out] Ad coefficient matrix - * @param [out] Bd coefficient matrix - * @param [out] Cd coefficient matrix - * @param [out] Wd coefficient matrix - * @param [in] dt Discretization time - */ - void calculateDiscreteMatrix( - Eigen::MatrixXd * Ad, Eigen::MatrixXd * Bd, Eigen::MatrixXd * Cd, Eigen::MatrixXd * Wd, - const double ds) override; - - /** - * @brief calculate reference input - * @param [out] reference input - */ - void calculateReferenceInput(Eigen::MatrixXd * Uref) override; - -private: - double wheelbase_; //!< @brief wheelbase length [m] - double steer_lim_; //!< @brief steering angle limit [rad] -}; -#endif // OBSTACLE_AVOIDANCE_PLANNER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_NO_DELAY_HPP_ diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/vehicle_model/vehicle_model_interface.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/vehicle_model/vehicle_model_interface.hpp index 55de7dadc3d03..77dbaa80c7bbe 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/vehicle_model/vehicle_model_interface.hpp +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/vehicle_model/vehicle_model_interface.hpp @@ -1,4 +1,4 @@ -// Copyright 2018-2019 Autoware Foundation +// Copyright 2023 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,13 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -/** - * @file vehicle_model_interface.h - * @brief vehicle model interface class - * @author Takamasa Horibe - * @date 2019.05.01 - */ - #ifndef OBSTACLE_AVOIDANCE_PLANNER__VEHICLE_MODEL__VEHICLE_MODEL_INTERFACE_HPP_ #define OBSTACLE_AVOIDANCE_PLANNER__VEHICLE_MODEL__VEHICLE_MODEL_INTERFACE_HPP_ @@ -27,89 +20,28 @@ #include -/** - * @class vehicle model class - * @brief calculate model-related values - */ class VehicleModelInterface { protected: - const int dim_x_; // !< @brief dimension of kinematics x - const int dim_u_; // !< @brief dimension of input u - const int dim_y_; // !< @brief dimension of output y - double velocity_; // !< @brief vehicle velocity - double curvature_; // !< @brief curvature on the linearized point on path - double wheel_base_; // !< @brief wheel base of vehicle - double steer_limit_; // !< @brief vehicle velocity - double center_offset_from_base_; // !< @brief length from base lin to optimization center [m] + const int dim_x_; + const int dim_u_; + const int dim_y_; + const double wheelbase_; + const double steer_limit_; public: - /** - * @brief constructor - * @param [in] dim_x dimension of kinematics x - * @param [in] dim_u dimension of input u - * @param [in] dim_y dimension of output y - */ - VehicleModelInterface(int dim_x, int dim_u, int dim_y, double wheel_base, double steer_limit); - - /** - * @brief destructor - */ + VehicleModelInterface( + const int dim_x, const int dim_u, const int dim_y, const double wheelbase, + const double steer_limit); virtual ~VehicleModelInterface() = default; - /** - * @brief get kinematics x dimension - * @return kinematics dimension - */ - int getDimX(); - - /** - * @brief get input u dimension - * @return input dimension - */ - int getDimU(); - - /** - * @brief get output y dimension - * @return output dimension - */ - int getDimY(); - - void updateCenterOffset(const double center_offset_from_base); + int getDimX() const; + int getDimU() const; + int getDimY() const; - /** - * @brief set curvature - * @param [in] curvature curvature on the linearized point on path - */ - void setCurvature(const double curvature); - - /** - * @brief calculate discrete kinematics equation matrix of x_k+1 = Ad * x_k + Bd * uk + Wd - * @param [out] Ad coefficient matrix - * @param [out] Bd coefficient matrix - * @param [out] Wd coefficient matrix - * @param [in] ds discretization arc length - */ virtual void calculateStateEquationMatrix( - Eigen::MatrixXd & Ad, Eigen::MatrixXd & Bd, Eigen::MatrixXd & Wd, const double ds) = 0; - - /** - * @brief calculate discrete observation matrix of y_k = Cd * x_k - * @param [out] Cd coefficient matrix - */ - virtual void calculateObservationMatrix(Eigen::MatrixXd & Cd) = 0; - - /** - * @brief calculate discrete observation matrix of y_k = Cd * x_k - * @param [out] Cd_vec sparse matrix information of coefficient matrix - */ - virtual void calculateObservationSparseMatrix(std::vector> & Cd_vec) = 0; - - /** - * @brief calculate reference input - * @param [out] reference input - */ - virtual void calculateReferenceInput(Eigen::MatrixXd * Uref) = 0; + Eigen::MatrixXd & Ad, Eigen::MatrixXd & Bd, Eigen::MatrixXd & Wd, const double k, + const double ds) const = 0; }; #endif // OBSTACLE_AVOIDANCE_PLANNER__VEHICLE_MODEL__VEHICLE_MODEL_INTERFACE_HPP_ diff --git a/planning/obstacle_avoidance_planner/launch/obstacle_avoidance_planner.launch.xml b/planning/obstacle_avoidance_planner/launch/obstacle_avoidance_planner.launch.xml index b35969aadbc6c..5a64fc970c867 100644 --- a/planning/obstacle_avoidance_planner/launch/obstacle_avoidance_planner.launch.xml +++ b/planning/obstacle_avoidance_planner/launch/obstacle_avoidance_planner.launch.xml @@ -1,16 +1,15 @@ - - - + + - + - + diff --git a/planning/obstacle_avoidance_planner/media/bounds.svg b/planning/obstacle_avoidance_planner/media/bounds.svg new file mode 100644 index 0000000000000..1655ef6cb2818 --- /dev/null +++ b/planning/obstacle_avoidance_planner/media/bounds.svg @@ -0,0 +1,190 @@ + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ upper bound +
+
+
+ + upper bound + + + + + + + +
+
+
+ lower bound +
+
+
+ + lower bound + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + `l_... + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + `u_... + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/obstacle_avoidance_planner/media/eb.svg b/planning/obstacle_avoidance_planner/media/eb.svg new file mode 100644 index 0000000000000..452332cf4b5f2 --- /dev/null +++ b/planning/obstacle_avoidance_planner/media/eb.svg @@ -0,0 +1,371 @@ + + + + + + + + + + + + + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + `p_k` + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + `p_{k... + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + `p_{k... + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + `(p_{k+1}-... + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/obstacle_avoidance_planner/media/eb_constraint.svg b/planning/obstacle_avoidance_planner/media/eb_constraint.svg new file mode 100644 index 0000000000000..ef91561297b86 --- /dev/null +++ b/planning/obstacle_avoidance_planner/media/eb_constraint.svg @@ -0,0 +1,991 @@ + + + + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + , + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + `x_{k}^l`, `y_{... + + + + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + , + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + `x_{k}`, `y_{k}` + + + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + `sin(theta_k)... + + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + `sin(theta_k)... + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + `sin(theta_k)... + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + , + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + `x_{k}^u`, `y_{... + + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + `theta_{k}` + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/obstacle_avoidance_planner/media/vehicle_circles.svg b/planning/obstacle_avoidance_planner/media/vehicle_circles.svg new file mode 100644 index 0000000000000..4efa37daf4bd3 --- /dev/null +++ b/planning/obstacle_avoidance_planner/media/vehicle_circles.svg @@ -0,0 +1,124 @@ + + + + + + + + + + + + + + + + + + + + +
+
+
+ (when n = 3) +
+
+
+ + (when n = 3) + + + + + + +
+
+
+ (when n = 3) +
+
+
+ + (when n = 3) + + + + + + +
+
+
+ Bicycle Model +
+
+
+ + Bicycle Model + + + + + + +
+
+
+ Uniform Circles +
+
+
+ + Uniform Circles + + + + + + +
+
+
+ Fitting Uniform Circles +
+
+
+ + Fitting Uniform Circl... + + + + + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/obstacle_avoidance_planner/package.xml b/planning/obstacle_avoidance_planner/package.xml index ad9dbd2c98f73..5f2e036847358 100644 --- a/planning/obstacle_avoidance_planner/package.xml +++ b/planning/obstacle_avoidance_planner/package.xml @@ -5,7 +5,6 @@ 0.1.0 The obstacle_avoidance_planner package Takayuki Murooka - Kosuke Takeuchi Apache License 2.0 Takayuki Murooka @@ -14,7 +13,6 @@ autoware_cmake - autoware_auto_perception_msgs autoware_auto_planning_msgs geometry_msgs interpolation @@ -31,8 +29,12 @@ vehicle_info_util visualization_msgs + ament_cmake_ros + ament_index_python ament_lint_auto autoware_lint_common + autoware_testing + fake_test_node ament_cmake diff --git a/planning/obstacle_avoidance_planner/scripts/calclation_time_analyzer.py b/planning/obstacle_avoidance_planner/scripts/calclation_time_plotter.py similarity index 75% rename from planning/obstacle_avoidance_planner/scripts/calclation_time_analyzer.py rename to planning/obstacle_avoidance_planner/scripts/calclation_time_plotter.py index 908cc204da2e5..275ae69ef177c 100644 --- a/planning/obstacle_avoidance_planner/scripts/calclation_time_analyzer.py +++ b/planning/obstacle_avoidance_planner/scripts/calclation_time_plotter.py @@ -1,6 +1,6 @@ #!/usr/bin/env python3 -# Copyright 2022 Tier IV, Inc. +# Copyright 2023 TIER IV, Inc. # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. @@ -53,23 +53,42 @@ def __init__(self, args): def CallbackCalculationCost(self, msg): max_y = 0 max_x = 0 + for f_idx, function_name in enumerate(self.functions_name): + is_found = False for line in msg.data.split("\n"): if function_name in line: y = float(line.split(":=")[1].split("[ms]")[0]) self.y_vec[f_idx].append(y) + is_found = True + break + + if not is_found: + self.y_vec[f_idx].append(None) + if len(self.y_vec[f_idx]) > 100: self.y_vec[f_idx].popleft() + print(len(self.y_vec[f_idx])) + x_vec = list(range(len(self.y_vec[f_idx]))) - self.lines[f_idx].set_xdata(x_vec) - self.lines[f_idx].set_ydata(self.y_vec[f_idx]) + valid_x_vec = [] + valid_y_vec = [] + for i in range(len(x_vec)): + if self.y_vec[f_idx][i] is not None: + valid_x_vec.append(x_vec[i]) + valid_y_vec.append(self.y_vec[f_idx][i]) + + print(len(valid_x_vec), len(valid_y_vec)) + + self.lines[f_idx].set_xdata(valid_x_vec) + self.lines[f_idx].set_ydata(valid_y_vec) - if len(self.y_vec[f_idx]) > 0: - max_x = max(max_x, max(x_vec)) - max_y = max(max_y, max(self.y_vec[f_idx])) + if len(valid_y_vec) > 0: + max_x = max(max_x, max(valid_x_vec)) + max_y = max(max_y, max(valid_y_vec)) plt.xlim(0, max_x) plt.ylim(0.0, max_y) diff --git a/planning/obstacle_avoidance_planner/scripts/trajectory_visualizer.py b/planning/obstacle_avoidance_planner/scripts/trajectory_visualizer.py deleted file mode 100755 index b59643bca185a..0000000000000 --- a/planning/obstacle_avoidance_planner/scripts/trajectory_visualizer.py +++ /dev/null @@ -1,134 +0,0 @@ -#!/usr/bin/env python3 - -# Copyright 2020 Tier IV, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -# TODO(kosuke murakami): write ros2 visualizer -# import rospy -# from tier4_planning_msgs.msg import Trajectory -# from tier4_planning_msgs.msg import TrajectoryPoint -# import matplotlib.pyplot as plt -# import numpy as np -# import tf -# from geometry_msgs.msg import Vector3 - - -# def quaternion_to_euler(quaternion): -# """Convert Quaternion to Euler Angles - -# quaternion: geometry_msgs/Quaternion -# euler: geometry_msgs/Vector3 -# """ -# e = tf.transformations.euler_from_quaternion( -# (quaternion.x, quaternion.y, quaternion.z, quaternion.w)) -# return Vector3(x=e[0], y=e[1], z=e[2]) - - -# class TrajectoryVisualizer(): - -# def __init__(self): -# self.in_trajectory = Trajectory() -# self.debug_trajectory = Trajectory() -# self.debug_fixed_trajectory = Trajectory() - -# self.plot_done1 = True -# self.plot_done2 = True -# self.plot_done3 = True - -# self.length = 50 - -# self.sub_status1 = rospy.Subscriber( -# "/planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/trajectory", -# Trajectory, self.CallBackTraj, queue_size=1, tcp_nodelay=True) -# rospy.Timer(rospy.Duration(0.3), self.timerCallback) - -# def CallBackTraj(self, cmd): -# if (self.plot_done1): -# self.in_trajectory = cmd -# self.plot_done1 = False - -# def CallBackDebugTraj(self, cmd): -# if (self.plot_done2): -# self.debug_trajectory = cmd -# self.plot_done2 = False - -# def CallBackDebugFixedTraj(self, cmd): -# if (self.plot_done3): -# self.debug_fixed_trajectory = cmd -# self.plot_done3 = False - -# def timerCallback(self, event): -# self.plotTrajectory() -# self.plot_done1 = True -# self.plot_done2 = True -# self.plot_done3 = True - -# def CalcArcLength(self, traj): -# s_arr = [] -# ds = 0.0 -# s_sum = 0.0 - -# if len(traj.points) > 0: -# s_arr.append(s_sum) - -# for i in range(1, len(traj.points)): -# p0 = traj.points[i-1] -# p1 = traj.points[i] -# dx = p1.pose.position.x - p0.pose.position.x -# dy = p1.pose.position.y - p0.pose.position.y -# ds = np.sqrt(dx**2 + dy**2) -# s_sum += ds -# if(s_sum > self.length): -# break -# s_arr.append(s_sum) -# return s_arr - -# def CalcX(self, traj): -# v_list = [] -# for p in traj.points: -# v_list.append(p.pose.position.x) -# return v_list - -# def CalcY(self, traj): -# v_list = [] -# for p in traj.points: -# v_list.append(p.pose.position.y) -# return v_list - -# def CalcYaw(self, traj, s_arr): -# v_list = [] -# for p in traj.points: -# v_list.append(quaternion_to_euler(p.pose.orientation).z) -# return v_list[0: len(s_arr)] - -# def plotTrajectory(self): -# plt.clf() -# ax3 = plt.subplot(1, 1, 1) -# x = self.CalcArcLength(self.in_trajectory) -# y = self.CalcYaw(self.in_trajectory, x) -# if len(x) == len(y): -# ax3.plot(x, y, label="final", marker="*") -# ax3.set_xlabel("arclength [m]") -# ax3.set_ylabel("yaw") -# plt.pause(0.01) - - -# def main(): -# rospy.init_node("trajectory_visualizer") -# TrajectoryVisualizer() -# rospy.spin() - - -# if __name__ == "__main__": -# main() diff --git a/planning/obstacle_avoidance_planner/src/debug_marker.cpp b/planning/obstacle_avoidance_planner/src/debug_marker.cpp new file mode 100644 index 0000000000000..c517ade57c6c4 --- /dev/null +++ b/planning/obstacle_avoidance_planner/src/debug_marker.cpp @@ -0,0 +1,344 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "obstacle_avoidance_planner/debug_marker.hpp" + +#include "motion_utils/motion_utils.hpp" +#include "obstacle_avoidance_planner/eb_path_smoother.hpp" +#include "obstacle_avoidance_planner/mpt_optimizer.hpp" + +#include "visualization_msgs/msg/marker_array.hpp" + +namespace obstacle_avoidance_planner +{ +using tier4_autoware_utils::appendMarkerArray; +using tier4_autoware_utils::createDefaultMarker; +using tier4_autoware_utils::createMarkerColor; +using tier4_autoware_utils::createMarkerScale; + +namespace +{ +MarkerArray getFootprintsMarkerArray( + const std::vector & mpt_traj, + const vehicle_info_util::VehicleInfo & vehicle_info, const size_t sampling_num) +{ + auto marker = createDefaultMarker( + "map", rclcpp::Clock().now(), "mpt_footprints", 0, Marker::LINE_STRIP, + createMarkerScale(0.05, 0.0, 0.0), createMarkerColor(0.99, 0.99, 0.2, 0.99)); + marker.lifetime = rclcpp::Duration::from_seconds(1.5); + + MarkerArray marker_array; + for (size_t i = 0; i < mpt_traj.size(); ++i) { + if (i % sampling_num != 0) { + continue; + } + + marker.id = i; + marker.points.clear(); + + const auto & traj_point = mpt_traj.at(i); + + const double half_width = vehicle_info.vehicle_width_m / 2.0; + const double base_to_front = vehicle_info.vehicle_length_m - vehicle_info.rear_overhang_m; + const double base_to_rear = vehicle_info.rear_overhang_m; + + marker.points.push_back( + tier4_autoware_utils::calcOffsetPose(traj_point.pose, base_to_front, -half_width, 0.0) + .position); + marker.points.push_back( + tier4_autoware_utils::calcOffsetPose(traj_point.pose, base_to_front, half_width, 0.0) + .position); + marker.points.push_back( + tier4_autoware_utils::calcOffsetPose(traj_point.pose, -base_to_rear, half_width, 0.0) + .position); + marker.points.push_back( + tier4_autoware_utils::calcOffsetPose(traj_point.pose, -base_to_rear, -half_width, 0.0) + .position); + marker.points.push_back(marker.points.front()); + + marker_array.markers.push_back(marker); + } + return marker_array; +} + +MarkerArray getBoundsLineMarkerArray( + const std::vector & ref_points, const double vehicle_width, + const size_t sampling_num) +{ + const auto current_time = rclcpp::Clock().now(); + MarkerArray marker_array; + + if (ref_points.empty()) return marker_array; + + // create lower bound marker + auto lb_marker = createDefaultMarker( + "map", rclcpp::Clock().now(), "", 0, Marker::LINE_LIST, createMarkerScale(0.05, 0.0, 0.0), + createMarkerColor(0.99 + 0.5, 0.99, 0.2, 0.3)); + lb_marker.lifetime = rclcpp::Duration::from_seconds(1.5); + + // create upper bound marker + auto ub_marker = createDefaultMarker( + "map", rclcpp::Clock().now(), "", 1, Marker::LINE_LIST, createMarkerScale(0.05, 0.0, 0.0), + createMarkerColor(0.99, 0.99 + 0.5, 0.2, 0.3)); + ub_marker.lifetime = rclcpp::Duration::from_seconds(1.5); + + for (size_t bound_idx = 0; bound_idx < ref_points.at(0).bounds_on_constraints.size(); + ++bound_idx) { + const std::string ns = "base_bounds_" + std::to_string(bound_idx); + + { // lower bound + lb_marker.points.clear(); + lb_marker.ns = ns; + + for (size_t i = 0; i < ref_points.size(); i++) { + if (i % sampling_num != 0) { + continue; + } + + const geometry_msgs::msg::Pose & pose = ref_points.at(i).pose_on_constraints.at(bound_idx); + const double lb_y = + ref_points.at(i).bounds_on_constraints.at(bound_idx).lower_bound - vehicle_width / 2.0; + const auto lb = tier4_autoware_utils::calcOffsetPose(pose, 0.0, lb_y, 0.0).position; + + lb_marker.points.push_back(pose.position); + lb_marker.points.push_back(lb); + } + marker_array.markers.push_back(lb_marker); + } + + { // upper bound + ub_marker.points.clear(); + ub_marker.ns = ns; + + for (size_t i = 0; i < ref_points.size(); i++) { + if (i % sampling_num != 0) { + continue; + } + + const geometry_msgs::msg::Pose & pose = ref_points.at(i).pose_on_constraints.at(bound_idx); + const double ub_y = + ref_points.at(i).bounds_on_constraints.at(bound_idx).upper_bound + vehicle_width / 2.0; + const auto ub = tier4_autoware_utils::calcOffsetPose(pose, 0.0, ub_y, 0.0).position; + + ub_marker.points.push_back(pose.position); + ub_marker.points.push_back(ub); + } + marker_array.markers.push_back(ub_marker); + } + } + + return marker_array; +} + +MarkerArray getVehicleCircleLinesMarkerArray( + const std::vector & ref_points, + const std::vector & vehicle_circle_longitudinal_offsets, const double vehicle_width, + const size_t sampling_num, const std::string & ns) +{ + const auto current_time = rclcpp::Clock().now(); + MarkerArray msg; + + for (size_t i = 0; i < ref_points.size(); ++i) { + if (i % sampling_num != 0) { + continue; + } + const auto & ref_point = ref_points.at(i); + + auto marker = createDefaultMarker( + "map", rclcpp::Clock().now(), ns, i, Marker::LINE_LIST, createMarkerScale(0.1, 0, 0), + createMarkerColor(0.99, 0.99, 0.2, 0.25)); + marker.lifetime = rclcpp::Duration::from_seconds(1.5); + + const double lat_dev = ref_point.optimized_kinematic_state.lat; + const double yaw_dev = ref_point.optimized_kinematic_state.yaw; + + // apply lateral and yaw deviation + auto pose_with_deviation = + tier4_autoware_utils::calcOffsetPose(ref_point.pose, 0.0, lat_dev, 0.0); + pose_with_deviation.orientation = + tier4_autoware_utils::createQuaternionFromYaw(ref_point.getYaw() + yaw_dev); + + for (const double d : vehicle_circle_longitudinal_offsets) { + // apply longitudinal offset + auto base_pose = tier4_autoware_utils::calcOffsetPose(pose_with_deviation, d, 0.0, 0.0); + base_pose.orientation = + tier4_autoware_utils::createQuaternionFromYaw(ref_point.getYaw() + ref_point.alpha); + + const auto ub = + tier4_autoware_utils::calcOffsetPose(base_pose, 0.0, vehicle_width / 2.0, 0.0).position; + const auto lb = + tier4_autoware_utils::calcOffsetPose(base_pose, 0.0, -vehicle_width / 2.0, 0.0).position; + + marker.points.push_back(ub); + marker.points.push_back(lb); + } + msg.markers.push_back(marker); + } + + return msg; +} + +MarkerArray getCurrentVehicleCirclesMarkerArray( + const geometry_msgs::msg::Pose & ego_pose, + const std::vector & vehicle_circle_longitudinal_offsets, + const std::vector & vehicle_circle_radiuses, const std::string & ns, const double r, + const double g, const double b) +{ + MarkerArray msg; + + size_t id = 0; + for (size_t v_idx = 0; v_idx < vehicle_circle_longitudinal_offsets.size(); ++v_idx) { + const double offset = vehicle_circle_longitudinal_offsets.at(v_idx); + + auto marker = createDefaultMarker( + "map", rclcpp::Clock().now(), ns, id, Marker::LINE_STRIP, createMarkerScale(0.05, 0.0, 0.0), + createMarkerColor(r, g, b, 0.8)); + marker.lifetime = rclcpp::Duration::from_seconds(1.5); + marker.pose = tier4_autoware_utils::calcOffsetPose(ego_pose, offset, 0.0, 0.0); + + constexpr size_t circle_dividing_num = 16; + for (size_t e_idx = 0; e_idx < circle_dividing_num + 1; ++e_idx) { + geometry_msgs::msg::Point edge_pos; + + const double edge_angle = + static_cast(e_idx) / static_cast(circle_dividing_num) * 2.0 * M_PI; + edge_pos.x = vehicle_circle_radiuses.at(v_idx) * std::cos(edge_angle); + edge_pos.y = vehicle_circle_radiuses.at(v_idx) * std::sin(edge_angle); + + marker.points.push_back(edge_pos); + } + + msg.markers.push_back(marker); + ++id; + } + return msg; +} + +MarkerArray getVehicleCirclesMarkerArray( + const std::vector & mpt_traj_points, + const std::vector & vehicle_circle_longitudinal_offsets, + const std::vector & vehicle_circle_radiuses, const size_t sampling_num, + const std::string & ns, const double r, const double g, const double b) +{ + MarkerArray msg; + + size_t id = 0; + for (size_t i = 0; i < mpt_traj_points.size(); ++i) { + if (i % sampling_num != 0) { + continue; + } + const auto & mpt_traj_point = mpt_traj_points.at(i); + + for (size_t v_idx = 0; v_idx < vehicle_circle_longitudinal_offsets.size(); ++v_idx) { + const double offset = vehicle_circle_longitudinal_offsets.at(v_idx); + + auto marker = createDefaultMarker( + "map", rclcpp::Clock().now(), ns, id, Marker::LINE_STRIP, createMarkerScale(0.05, 0.0, 0.0), + createMarkerColor(r, g, b, 0.8)); + marker.lifetime = rclcpp::Duration::from_seconds(1.5); + marker.pose = tier4_autoware_utils::calcOffsetPose(mpt_traj_point.pose, offset, 0.0, 0.0); + + constexpr size_t circle_dividing_num = 16; + for (size_t e_idx = 0; e_idx < circle_dividing_num + 1; ++e_idx) { + geometry_msgs::msg::Point edge_pos; + + const double edge_angle = + static_cast(e_idx) / static_cast(circle_dividing_num) * 2.0 * M_PI; + edge_pos.x = vehicle_circle_radiuses.at(v_idx) * std::cos(edge_angle); + edge_pos.y = vehicle_circle_radiuses.at(v_idx) * std::sin(edge_angle); + + marker.points.push_back(edge_pos); + } + + msg.markers.push_back(marker); + ++id; + } + } + return msg; +} + +visualization_msgs::msg::MarkerArray getPointsTextMarkerArray( + const std::vector & ref_points) +{ + if (ref_points.empty()) { + return visualization_msgs::msg::MarkerArray{}; + } + + auto marker = createDefaultMarker( + "map", rclcpp::Clock().now(), "text", 0, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, + createMarkerScale(0.0, 0.0, 0.15), createMarkerColor(1.0, 1.0, 0.0, 0.99)); + marker.lifetime = rclcpp::Duration::from_seconds(1.5); + + visualization_msgs::msg::MarkerArray msg; + for (size_t i = 0; i < ref_points.size(); i++) { + marker.id = i; + // marker.text = std::to_string(tf2::getYaw(ref_points[i].pose.orientation)) + "\n" + + // std::to_string(ref_points[i].delta_arc_length); marker.text = + // std::to_string(ref_points[i].alpha) + "\n" + std::to_string(ref_points[i].beta); marker.text + // marker.text = std::to_string(ref_points.at(i).curvature); + marker.text = std::to_string(ref_points.at(i).curvature) + " \n" + + std::to_string(ref_points.at(i).optimized_input); + marker.pose.position = ref_points.at(i).pose.position; + msg.markers.push_back(marker); + } + + return msg; +} +} // namespace + +MarkerArray getDebugMarker( + const DebugData & debug_data, const std::vector & optimized_points, + const vehicle_info_util::VehicleInfo & vehicle_info) +{ + MarkerArray marker_array; + + // mpt footprints + appendMarkerArray( + getFootprintsMarkerArray(optimized_points, vehicle_info, debug_data.mpt_visualize_sampling_num), + &marker_array); + + // bounds + appendMarkerArray( + getBoundsLineMarkerArray( + debug_data.ref_points, vehicle_info.vehicle_width_m, debug_data.mpt_visualize_sampling_num), + &marker_array); + + // vehicle circle line + appendMarkerArray( + getVehicleCircleLinesMarkerArray( + debug_data.ref_points, debug_data.vehicle_circle_longitudinal_offsets, + vehicle_info.vehicle_width_m, debug_data.mpt_visualize_sampling_num, "vehicle_circle_lines"), + &marker_array); + + // current vehicle circles + appendMarkerArray( + getCurrentVehicleCirclesMarkerArray( + debug_data.ego_pose, debug_data.vehicle_circle_longitudinal_offsets, + debug_data.vehicle_circle_radiuses, "current_vehicle_circles", 1.0, 0.3, 0.3), + &marker_array); + + // vehicle circles + appendMarkerArray( + getVehicleCirclesMarkerArray( + optimized_points, debug_data.vehicle_circle_longitudinal_offsets, + debug_data.vehicle_circle_radiuses, debug_data.mpt_visualize_sampling_num, "vehicle_circles", + 1.0, 0.3, 0.3), + &marker_array); + + // debug text + appendMarkerArray(getPointsTextMarkerArray(debug_data.ref_points), &marker_array); + + return marker_array; +} +} // namespace obstacle_avoidance_planner diff --git a/planning/obstacle_avoidance_planner/src/eb_path_optimizer.cpp b/planning/obstacle_avoidance_planner/src/eb_path_optimizer.cpp deleted file mode 100644 index d26180f604e88..0000000000000 --- a/planning/obstacle_avoidance_planner/src/eb_path_optimizer.cpp +++ /dev/null @@ -1,635 +0,0 @@ -// Copyright 2020 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "obstacle_avoidance_planner/eb_path_optimizer.hpp" - -#include "obstacle_avoidance_planner/utils/utils.hpp" - -#include "geometry_msgs/msg/vector3.hpp" - -#include -#include -#include -#include -#include - -EBPathOptimizer::EBPathOptimizer( - const bool is_showing_debug_info, const TrajectoryParam & traj_param, const EBParam & eb_param, - const VehicleParam & vehicle_param) -: is_showing_debug_info_(is_showing_debug_info), - epsilon_(1e-8), - qp_param_(eb_param.qp_param), - traj_param_(traj_param), - eb_param_(eb_param), - vehicle_param_(vehicle_param) -{ - const Eigen::MatrixXd p = makePMatrix(); - default_a_matrix_ = makeAMatrix(); - - const int num_points = eb_param_.num_sampling_points_for_eb; - const std::vector q(num_points * 2, 0.0); - const std::vector lower_bound(num_points * 2, 0.0); - const std::vector upper_bound(num_points * 2, 0.0); - - osqp_solver_ptr_ = std::make_unique( - p, default_a_matrix_, q, lower_bound, upper_bound, qp_param_.eps_abs); - osqp_solver_ptr_->updateEpsRel(qp_param_.eps_rel); - osqp_solver_ptr_->updateMaxIter(qp_param_.max_iteration); -} - -// make positive semidefinite matrix for objective function -// reference: https://ieeexplore.ieee.org/document/7402333 -Eigen::MatrixXd EBPathOptimizer::makePMatrix() -{ - const int num_points = eb_param_.num_sampling_points_for_eb; - - Eigen::MatrixXd P = Eigen::MatrixXd::Zero(num_points * 2, num_points * 2); - for (int r = 0; r < num_points * 2; ++r) { - for (int c = 0; c < num_points * 2; ++c) { - if (r == c) { - P(r, c) = 6.0; - } else if (std::abs(c - r) == 1) { - P(r, c) = -4.0; - } else if (std::abs(c - r) == 2) { - P(r, c) = 1.0; - } else { - P(r, c) = 0.0; - } - } - } - return P; -} - -// make default linear constrain matrix -Eigen::MatrixXd EBPathOptimizer::makeAMatrix() -{ - const int num_points = eb_param_.num_sampling_points_for_eb; - - Eigen::MatrixXd A = Eigen::MatrixXd::Identity(num_points * 2, num_points * 2); - for (int i = 0; i < num_points * 2; ++i) { - if (i < num_points) { - A(i, i + num_points) = 1.0; - } else { - A(i, i - num_points) = 1.0; - } - } - return A; -} - -boost::optional> -EBPathOptimizer::getEBTrajectory( - const geometry_msgs::msg::Pose & ego_pose, const autoware_auto_planning_msgs::msg::Path & path, - const std::unique_ptr & prev_trajs, const double current_ego_vel, - DebugData & debug_data) -{ - stop_watch_.tic(__func__); - - current_ego_vel_ = current_ego_vel; - - // get candidate points for optimization - // decide fix or non fix, might not required only for smoothing purpose - const CandidatePoints candidate_points = - getCandidatePoints(ego_pose, path.points, prev_trajs, debug_data); - if (candidate_points.fixed_points.empty() && candidate_points.non_fixed_points.empty()) { - RCLCPP_INFO_EXPRESSION( - rclcpp::get_logger("EBPathOptimizer"), is_showing_debug_info_, - "return boost::none since empty candidate points"); - return boost::none; - } - - // get optimized smooth points with elastic band - const auto eb_traj_points = getOptimizedTrajectory(path, candidate_points, debug_data); - if (!eb_traj_points) { - RCLCPP_INFO_EXPRESSION( - rclcpp::get_logger("EBPathOptimizer"), is_showing_debug_info_, - "return boost::none since smoothing failed"); - return boost::none; - } - - debug_data.msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) << " [ms]\n"; - return eb_traj_points; -} - -boost::optional> -EBPathOptimizer::getOptimizedTrajectory( - const autoware_auto_planning_msgs::msg::Path & path, const CandidatePoints & candidate_points, - DebugData & debug_data) -{ - stop_watch_.tic(__func__); - - // get constrain rectangles around each point - auto full_points = candidate_points.fixed_points; - full_points.insert( - full_points.end(), candidate_points.non_fixed_points.begin(), - candidate_points.non_fixed_points.end()); - - // interpolate points for logic purpose - std::vector interpolated_points = - interpolation_utils::getInterpolatedPoints(full_points, eb_param_.delta_arc_length_for_eb); - if (interpolated_points.empty()) { - return boost::none; - } - - // clip interpolated points with the length of path - if (traj_param_.enable_clipping_fixed_traj) { - if (path.points.size() < 2) { - return boost::none; - } - const auto interpolated_poses = - points_utils::convertToPosesWithYawEstimation(interpolated_points); - const auto interpolated_points_end_seg_idx = motion_utils::findNearestSegmentIndex( - interpolated_poses, path.points.back().pose, 3.0, 0.785); - if (interpolated_points_end_seg_idx) { - interpolated_points = std::vector( - interpolated_points.begin(), - interpolated_points.begin() + interpolated_points_end_seg_idx.get()); - if (interpolated_points.empty()) { - return boost::none; - } - } - } - - debug_data.interpolated_points = interpolated_points; - // number of optimizing points - const int farthest_idx = std::min( - (eb_param_.num_sampling_points_for_eb - 1), static_cast(interpolated_points.size() - 1)); - // number of fixed points in interpolated_points - const int num_fixed_points = - getNumFixedPoints(candidate_points.fixed_points, interpolated_points, farthest_idx); - // TODO(murooka) try this printing. something may be wrong - // std::cerr << num_fixed_points << std::endl; - - // consider straight after `straight_line_idx` and then tighten space for optimization after - // `straight_line_idx` - const int straight_line_idx = - getStraightLineIdx(interpolated_points, farthest_idx, debug_data.straight_points); - - // if `farthest_idx` is lower than `number_of_sampling_points`, duplicate the point at the end of - // `interpolated_points` - // This aims for using hotstart by not changing the size of matrix - std::vector padded_interpolated_points = - getPaddedInterpolatedPoints(interpolated_points, farthest_idx); - - const auto rectangles = getConstrainRectangleVec( - path, padded_interpolated_points, num_fixed_points, farthest_idx, straight_line_idx); - if (!rectangles) { - return boost::none; - } - - const auto traj_points = - calculateTrajectory(padded_interpolated_points, rectangles.get(), farthest_idx, debug_data); - if (!traj_points) { - return boost::none; - } - - debug_data.msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) - << " [ms]\n"; - return *traj_points; -} - -boost::optional> -EBPathOptimizer::calculateTrajectory( - const std::vector & padded_interpolated_points, - const std::vector & constrain_rectangles, const int farthest_idx, - DebugData & debug_data) -{ - stop_watch_.tic(__func__); - - // update constrain for QP based on constrain rectangles - updateConstrain(padded_interpolated_points, constrain_rectangles); - - // solve QP and get optimized trajectory - const auto result = solveQP(); - const auto optimized_points = result.first; - const auto status = result.second; - if (status != 1) { - utils::logOSQPSolutionStatus(status, "EB: "); - return boost::none; - } - - const auto traj_points = - convertOptimizedPointsToTrajectory(optimized_points, constrain_rectangles, farthest_idx); - - debug_data.msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) - << " [ms]\n"; - return traj_points; -} - -std::pair, int64_t> EBPathOptimizer::solveQP() -{ - osqp_solver_ptr_->updateEpsRel(qp_param_.eps_rel); - osqp_solver_ptr_->updateEpsAbs(qp_param_.eps_abs); - - const auto result = osqp_solver_ptr_->optimize(); - const auto optimized_points = std::get<0>(result); - const auto status = std::get<3>(result); - - utils::logOSQPSolutionStatus(std::get<3>(result), "EB: "); - - return std::make_pair(optimized_points, status); -} - -std::vector EBPathOptimizer::getFixedPoints( - const geometry_msgs::msg::Pose & ego_pose, - [[maybe_unused]] const std::vector & path_points, - const std::unique_ptr & prev_trajs) -{ - /* use of prev_traj_points(fine resolution) instead of prev_opt_traj(coarse resolution) - stabilize trajectory's yaw*/ - if (prev_trajs) { - if (prev_trajs->smoothed_trajectory.empty()) { - std::vector empty_points; - return empty_points; - } - const size_t begin_idx = motion_utils::findFirstNearestIndexWithSoftConstraints( - prev_trajs->smoothed_trajectory, ego_pose, traj_param_.ego_nearest_dist_threshold, - traj_param_.ego_nearest_yaw_threshold); - const int backward_fixing_idx = std::max( - static_cast( - begin_idx - - traj_param_.backward_fixing_distance / traj_param_.delta_arc_length_for_trajectory), - 0); - - // NOTE: Fixed length of EB has to be longer than that of MPT. - constexpr double forward_fixed_length_margin = 5.0; - const double forward_fixed_length = std::max( - current_ego_vel_ * traj_param_.forward_fixing_min_time + forward_fixed_length_margin, - traj_param_.forward_fixing_min_distance); - - const int forward_fixing_idx = std::min( - static_cast( - begin_idx + forward_fixed_length / traj_param_.delta_arc_length_for_trajectory), - static_cast(prev_trajs->smoothed_trajectory.size() - 1)); - std::vector fixed_points; - for (int i = backward_fixing_idx; i <= forward_fixing_idx; i++) { - fixed_points.push_back(prev_trajs->smoothed_trajectory.at(i).pose); - } - return fixed_points; - } else { - std::vector empty_points; - return empty_points; - } -} - -EBPathOptimizer::CandidatePoints EBPathOptimizer::getCandidatePoints( - const geometry_msgs::msg::Pose & ego_pose, - const std::vector & path_points, - const std::unique_ptr & prev_trajs, DebugData & debug_data) -{ - const std::vector fixed_points = - getFixedPoints(ego_pose, path_points, prev_trajs); - if (fixed_points.empty()) { - CandidatePoints candidate_points = getDefaultCandidatePoints(path_points); - return candidate_points; - } - - // try to find non-fix points - const auto opt_begin_idx = motion_utils::findNearestIndex( - path_points, fixed_points.back(), std::numeric_limits::max(), - traj_param_.delta_yaw_threshold_for_closest_point); - if (!opt_begin_idx) { - CandidatePoints candidate_points; - candidate_points.fixed_points = fixed_points; - candidate_points.begin_path_idx = path_points.size(); - candidate_points.end_path_idx = path_points.size(); - return candidate_points; - } - - const int begin_idx = std::min( - static_cast(opt_begin_idx.get()) + eb_param_.num_offset_for_begin_idx, - static_cast(path_points.size()) - 1); - - std::vector non_fixed_points; - for (size_t i = begin_idx; i < path_points.size(); i++) { - non_fixed_points.push_back(path_points[i].pose); - } - CandidatePoints candidate_points; - candidate_points.fixed_points = fixed_points; - candidate_points.non_fixed_points = non_fixed_points; - candidate_points.begin_path_idx = begin_idx; - candidate_points.end_path_idx = path_points.size() - 1; - - debug_data.fixed_points = candidate_points.fixed_points; - debug_data.non_fixed_points = candidate_points.non_fixed_points; - return candidate_points; -} - -std::vector EBPathOptimizer::getPaddedInterpolatedPoints( - const std::vector & interpolated_points, const int farthest_point_idx) -{ - std::vector padded_interpolated_points; - for (int i = 0; i < eb_param_.num_sampling_points_for_eb; i++) { - if (i > farthest_point_idx) { - padded_interpolated_points.push_back(interpolated_points[farthest_point_idx]); - } else { - padded_interpolated_points.push_back(interpolated_points[i]); - } - } - return padded_interpolated_points; -} - -EBPathOptimizer::CandidatePoints EBPathOptimizer::getDefaultCandidatePoints( - const std::vector & path_points) -{ - double accum_arc_length = 0; - int end_path_idx = 0; - std::vector fixed_points; - for (size_t i = 0; i < path_points.size(); i++) { - if (i > 0) { - accum_arc_length += tier4_autoware_utils::calcDistance2d( - path_points[i].pose.position, path_points[i - 1].pose.position); - } - if ( - accum_arc_length > eb_param_.num_sampling_points_for_eb * eb_param_.delta_arc_length_for_eb) { - break; - } - end_path_idx = i; - fixed_points.push_back(path_points[i].pose); - } - CandidatePoints candidate_points; - candidate_points.fixed_points = fixed_points; - candidate_points.begin_path_idx = 0; - candidate_points.end_path_idx = end_path_idx; - return candidate_points; -} - -int EBPathOptimizer::getNumFixedPoints( - const std::vector & fixed_points, - const std::vector & interpolated_points, const int farthest_idx) -{ - int num_fixed_points = 0; - if (!fixed_points.empty() && !interpolated_points.empty()) { - std::vector interpolated_points = - interpolation_utils::getInterpolatedPoints(fixed_points, eb_param_.delta_arc_length_for_eb); - num_fixed_points = interpolated_points.size(); - } - num_fixed_points = std::min(num_fixed_points, farthest_idx); - return num_fixed_points; -} - -std::vector -EBPathOptimizer::convertOptimizedPointsToTrajectory( - const std::vector optimized_points, - [[maybe_unused]] const std::vector & constraints, const int farthest_idx) -{ - std::vector traj_points; - for (int i = 0; i <= farthest_idx; i++) { - autoware_auto_planning_msgs::msg::TrajectoryPoint tmp_point; - tmp_point.pose.position.x = optimized_points[i]; - tmp_point.pose.position.y = optimized_points[i + eb_param_.num_sampling_points_for_eb]; - traj_points.push_back(tmp_point); - } - for (size_t i = 0; i < traj_points.size(); i++) { - if (i > 0) { - traj_points[i].pose.orientation = geometry_utils::getQuaternionFromPoints( - traj_points[i].pose.position, traj_points[i - 1].pose.position); - } else if (i == 0 && traj_points.size() > 1) { - traj_points[i].pose.orientation = geometry_utils::getQuaternionFromPoints( - traj_points[i + 1].pose.position, traj_points[i].pose.position); - } - } - return traj_points; -} - -boost::optional> EBPathOptimizer::getConstrainRectangleVec( - const autoware_auto_planning_msgs::msg::Path & path, - const std::vector & interpolated_points, const int num_fixed_points, - const int farthest_point_idx, const int straight_idx) -{ - auto curvatures = points_utils::calcCurvature(interpolated_points, 10); - - std::vector smooth_constrain_rects(eb_param_.num_sampling_points_for_eb); - for (int i = 0; i < eb_param_.num_sampling_points_for_eb; i++) { - // `Anchor` has pose + velocity - const Anchor anchor = getAnchor(interpolated_points, i, path.points); - - // four types of rectangle for various types - if (i == 0 || i == 1 || i >= farthest_point_idx - 1 || i < num_fixed_points - 1) { - // rect has four points for a rectangle in map coordinate - const auto rect = getConstrainRectangle(anchor, eb_param_.clearance_for_fixing); - smooth_constrain_rects[i] = rect; - } else if ( // NOLINT - i >= num_fixed_points - eb_param_.num_joint_buffer_points && - i <= num_fixed_points + eb_param_.num_joint_buffer_points) { - const auto rect = getConstrainRectangle(anchor, eb_param_.clearance_for_joint); - smooth_constrain_rects[i] = rect; - } else if (i >= straight_idx) { - const auto rect = getConstrainRectangle(anchor, eb_param_.clearance_for_straight_line); - smooth_constrain_rects[i] = rect; - } else { - const double min_x = -eb_param_.clearance_for_only_smoothing; - const double max_x = eb_param_.clearance_for_only_smoothing; - const double min_y = curvatures[i] > 0 ? 0 : -eb_param_.clearance_for_only_smoothing; - const double max_y = curvatures[i] <= 0 ? 0 : eb_param_.clearance_for_only_smoothing; - const auto rect = getConstrainRectangle(anchor, min_x, max_x, min_y, max_y); - smooth_constrain_rects[i] = rect; - } - } - return smooth_constrain_rects; -} - -void EBPathOptimizer::updateConstrain( - const std::vector & interpolated_points, - const std::vector & rectangle_points) -{ - const int num_points = eb_param_.num_sampling_points_for_eb; - - Eigen::MatrixXd A = default_a_matrix_; - std::vector lower_bound(num_points * 2, 0.0); - std::vector upper_bound(num_points * 2, 0.0); - for (int i = 0; i < num_points; ++i) { - Constrain constrain = - getConstrainFromConstrainRectangle(interpolated_points[i], rectangle_points[i]); - A(i, i) = constrain.top_and_bottom.x_coef; - A(i, i + num_points) = constrain.top_and_bottom.y_coef; - A(i + num_points, i) = constrain.left_and_right.x_coef; - A(i + num_points, i + num_points) = constrain.left_and_right.y_coef; - lower_bound[i] = constrain.top_and_bottom.lower_bound; - upper_bound[i] = constrain.top_and_bottom.upper_bound; - lower_bound[i + num_points] = constrain.left_and_right.lower_bound; - upper_bound[i + num_points] = constrain.left_and_right.upper_bound; - } - - osqp_solver_ptr_->updateBounds(lower_bound, upper_bound); - osqp_solver_ptr_->updateA(A); -} - -EBPathOptimizer::Anchor EBPathOptimizer::getAnchor( - const std::vector & interpolated_points, const int interpolated_idx, - const std::vector & path_points) const -{ - geometry_msgs::msg::Pose pose; - pose.position = interpolated_points[interpolated_idx]; - if (interpolated_idx > 0) { - pose.orientation = geometry_utils::getQuaternionFromPoints( - interpolated_points[interpolated_idx], interpolated_points[interpolated_idx - 1]); - } else if (interpolated_idx == 0 && interpolated_points.size() > 1) { - pose.orientation = geometry_utils::getQuaternionFromPoints( - interpolated_points[interpolated_idx + 1], interpolated_points[interpolated_idx]); - } - const auto opt_nearest_idx = motion_utils::findNearestIndex( - path_points, pose, std::numeric_limits::max(), - traj_param_.delta_yaw_threshold_for_closest_point); - const int nearest_idx = opt_nearest_idx ? *opt_nearest_idx : 0; - - const geometry_msgs::msg::Quaternion nearest_q = path_points[nearest_idx].pose.orientation; - Anchor anchor; - anchor.pose.position = interpolated_points[interpolated_idx]; - anchor.pose.orientation = nearest_q; - return anchor; -} - -int EBPathOptimizer::getStraightLineIdx( - const std::vector & interpolated_points, const int farthest_point_idx, - std::vector & debug_detected_straight_points) -{ - double prev_yaw = 0; - int straight_line_idx = farthest_point_idx; - for (int i = farthest_point_idx; i >= 0; i--) { - if (i < farthest_point_idx) { - const double yaw = - tier4_autoware_utils::calcAzimuthAngle(interpolated_points[i], interpolated_points[i + 1]); - const double delta_yaw = yaw - prev_yaw; - const double norm_delta_yaw = tier4_autoware_utils::normalizeRadian(delta_yaw); - if (std::fabs(norm_delta_yaw) > traj_param_.delta_yaw_threshold_for_straight) { - break; - } - straight_line_idx = i; - prev_yaw = yaw; - } else if (i == farthest_point_idx && farthest_point_idx >= 1) { - const double yaw = - tier4_autoware_utils::calcAzimuthAngle(interpolated_points[i - 1], interpolated_points[i]); - prev_yaw = yaw; - } - } - for (int i = straight_line_idx; i <= farthest_point_idx; i++) { - debug_detected_straight_points.push_back(interpolated_points[i]); - } - return straight_line_idx; -} - -EBPathOptimizer::Constrain EBPathOptimizer::getConstrainFromConstrainRectangle( - const geometry_msgs::msg::Point & interpolated_point, const ConstrainRectangle & constrain_range) -{ - Constrain constrain; - const double top_dx = constrain_range.top_left.x - constrain_range.top_right.x; - const double top_dy = constrain_range.top_left.y - constrain_range.top_right.y; - const double left_dx = constrain_range.top_left.x - constrain_range.bottom_left.x; - const double left_dy = constrain_range.top_left.y - constrain_range.bottom_left.y; - if ( - std::fabs(top_dx) < epsilon_ && std::fabs(top_dy) < epsilon_ && std::fabs(left_dx) < epsilon_ && - std::fabs(left_dy) < epsilon_) { - constrain.top_and_bottom.x_coef = 1; - constrain.top_and_bottom.y_coef = 1; - constrain.top_and_bottom.lower_bound = interpolated_point.x + interpolated_point.y; - constrain.top_and_bottom.upper_bound = interpolated_point.x + interpolated_point.y; - constrain.left_and_right.x_coef = -1; - constrain.left_and_right.y_coef = 1; - constrain.left_and_right.lower_bound = interpolated_point.y - interpolated_point.x; - constrain.left_and_right.upper_bound = interpolated_point.y - interpolated_point.x; - } else if (std::fabs(top_dx) < epsilon_) { - constrain.top_and_bottom.x_coef = 1; - constrain.top_and_bottom.y_coef = epsilon_; - constrain.top_and_bottom.lower_bound = interpolated_point.x; - constrain.top_and_bottom.upper_bound = interpolated_point.x; - constrain.left_and_right = - getConstrainLines(left_dx, left_dy, constrain_range.top_left, constrain_range.top_right); - } else if (std::fabs(top_dy) < epsilon_) { - constrain.top_and_bottom.x_coef = epsilon_; - constrain.top_and_bottom.y_coef = 1; - constrain.top_and_bottom.lower_bound = interpolated_point.y; - constrain.top_and_bottom.upper_bound = interpolated_point.y; - constrain.left_and_right = - getConstrainLines(left_dx, left_dy, constrain_range.top_left, constrain_range.top_right); - } else if (std::fabs(left_dx) < epsilon_) { - constrain.left_and_right.x_coef = 1; - constrain.left_and_right.y_coef = epsilon_; - constrain.left_and_right.lower_bound = interpolated_point.x; - constrain.left_and_right.upper_bound = interpolated_point.x; - constrain.top_and_bottom = - getConstrainLines(top_dx, top_dy, constrain_range.top_left, constrain_range.bottom_left); - } else if (std::fabs(left_dy) < epsilon_) { - constrain.left_and_right.x_coef = epsilon_; - constrain.left_and_right.y_coef = 1; - constrain.left_and_right.lower_bound = interpolated_point.y; - constrain.left_and_right.upper_bound = interpolated_point.y; - constrain.top_and_bottom = - getConstrainLines(top_dx, top_dy, constrain_range.top_left, constrain_range.bottom_left); - } else { - constrain.top_and_bottom = - getConstrainLines(top_dx, top_dy, constrain_range.top_left, constrain_range.bottom_left); - constrain.left_and_right = - getConstrainLines(left_dx, left_dy, constrain_range.top_left, constrain_range.top_right); - } - return constrain; -} - -EBPathOptimizer::ConstrainLines EBPathOptimizer::getConstrainLines( - const double dx, const double dy, const geometry_msgs::msg::Point & point, - const geometry_msgs::msg::Point & opposite_point) -{ - ConstrainLines constrain_point; - - const double slope = dy / dx; - const double intercept = point.y - slope * point.x; - const double intercept2 = opposite_point.y - slope * opposite_point.x; - constrain_point.x_coef = -1 * slope; - constrain_point.y_coef = 1; - if (intercept > intercept2) { - constrain_point.lower_bound = intercept2; - constrain_point.upper_bound = intercept; - } else { - constrain_point.lower_bound = intercept; - constrain_point.upper_bound = intercept2; - } - return constrain_point; -} - -ConstrainRectangle EBPathOptimizer::getConstrainRectangle( - const Anchor & anchor, const double clearance) const -{ - ConstrainRectangle constrain_range; - geometry_msgs::msg::Point top_left; - top_left.x = clearance; - top_left.y = clearance; - constrain_range.top_left = geometry_utils::transformToAbsoluteCoordinate2D(top_left, anchor.pose); - geometry_msgs::msg::Point top_right; - top_right.x = clearance; - top_right.y = -1 * clearance; - constrain_range.top_right = - geometry_utils::transformToAbsoluteCoordinate2D(top_right, anchor.pose); - geometry_msgs::msg::Point bottom_left; - bottom_left.x = -1 * clearance; - bottom_left.y = clearance; - constrain_range.bottom_left = - geometry_utils::transformToAbsoluteCoordinate2D(bottom_left, anchor.pose); - geometry_msgs::msg::Point bottom_right; - bottom_right.x = -1 * clearance; - bottom_right.y = -1 * clearance; - constrain_range.bottom_right = - geometry_utils::transformToAbsoluteCoordinate2D(bottom_right, anchor.pose); - return constrain_range; -} - -ConstrainRectangle EBPathOptimizer::getConstrainRectangle( - const Anchor & anchor, const double min_x, const double max_x, const double min_y, - const double max_y) const -{ - ConstrainRectangle rect; - rect.top_left = tier4_autoware_utils::calcOffsetPose(anchor.pose, max_x, max_y, 0.0).position; - rect.top_right = tier4_autoware_utils::calcOffsetPose(anchor.pose, max_x, min_y, 0.0).position; - rect.bottom_left = tier4_autoware_utils::calcOffsetPose(anchor.pose, min_x, max_y, 0.0).position; - rect.bottom_right = tier4_autoware_utils::calcOffsetPose(anchor.pose, min_x, min_y, 0.0).position; - return rect; -} diff --git a/planning/obstacle_avoidance_planner/src/eb_path_smoother.cpp b/planning/obstacle_avoidance_planner/src/eb_path_smoother.cpp new file mode 100644 index 0000000000000..807d113c6b699 --- /dev/null +++ b/planning/obstacle_avoidance_planner/src/eb_path_smoother.cpp @@ -0,0 +1,416 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "obstacle_avoidance_planner/eb_path_smoother.hpp" + +#include "motion_utils/motion_utils.hpp" +#include "obstacle_avoidance_planner/type_alias.hpp" +#include "obstacle_avoidance_planner/utils/geometry_utils.hpp" +#include "obstacle_avoidance_planner/utils/trajectory_utils.hpp" + +#include +#include +#include + +namespace +{ +Eigen::MatrixXd makePMatrix(const int num_points) +{ + // create P block matrix + Eigen::MatrixXd P_quarter = Eigen::MatrixXd::Zero(num_points, num_points); + for (int r = 0; r < num_points; ++r) { + for (int c = 0; c < num_points; ++c) { + if (r == c) { + if (r == 0 || r == num_points - 1) { + P_quarter(r, c) = 1.0; + } else if (r == 1 || r == num_points - 2) { + P_quarter(r, c) = 5.0; + } else { + P_quarter(r, c) = 6.0; + } + } else if (std::abs(c - r) == 1) { + if (r == 0 || r == num_points - 1) { + P_quarter(r, c) = -2.0; + } else if (c == 0 || c == num_points - 1) { + P_quarter(r, c) = -2.0; + } else { + P_quarter(r, c) = -4.0; + } + } else if (std::abs(c - r) == 2) { + P_quarter(r, c) = 1.0; + } else { + P_quarter(r, c) = 0.0; + } + } + } + + // create P matrix + Eigen::MatrixXd P = Eigen::MatrixXd::Zero(num_points * 2, num_points * 2); + P.block(0, 0, num_points, num_points) = P_quarter; + P.block(num_points, num_points, num_points, num_points) = P_quarter; + + return P; +} + +std::vector toStdVector(const Eigen::VectorXd & eigen_vec) +{ + return {eigen_vec.data(), eigen_vec.data() + eigen_vec.rows()}; +} +} // namespace + +namespace obstacle_avoidance_planner +{ +EBPathSmoother::EBParam::EBParam(rclcpp::Node * node) +{ + { // option + enable_warm_start = node->declare_parameter("eb.option.enable_warm_start"); + enable_optimization_validation = + node->declare_parameter("eb.option.enable_optimization_validation"); + } + + { // common + delta_arc_length = node->declare_parameter("eb.common.delta_arc_length"); + num_points = node->declare_parameter("eb.common.num_points"); + } + + { // clearance + num_joint_points = node->declare_parameter("eb.clearance.num_joint_points"); + clearance_for_fix = node->declare_parameter("eb.clearance.clearance_for_fix"); + clearance_for_joint = node->declare_parameter("eb.clearance.clearance_for_joint"); + clearance_for_smooth = node->declare_parameter("eb.clearance.clearance_for_smooth"); + } + + { // weight + smooth_weight = node->declare_parameter("eb.weight.smooth_weight"); + lat_error_weight = node->declare_parameter("eb.weight.lat_error_weight"); + } + + { // qp + qp_param.max_iteration = node->declare_parameter("eb.qp.max_iteration"); + qp_param.eps_abs = node->declare_parameter("eb.qp.eps_abs"); + qp_param.eps_rel = node->declare_parameter("eb.qp.eps_rel"); + } + + // validation + max_validation_error = node->declare_parameter("eb.validation.max_error"); +} + +void EBPathSmoother::EBParam::onParam(const std::vector & parameters) +{ + using tier4_autoware_utils::updateParam; + + { // option + updateParam(parameters, "eb.option.enable_warm_start", enable_warm_start); + updateParam( + parameters, "eb.option.enable_optimization_validation", enable_optimization_validation); + } + + { // common + updateParam(parameters, "eb.common.delta_arc_length", delta_arc_length); + updateParam(parameters, "eb.common.num_points", num_points); + } + + { // clearance + updateParam(parameters, "eb.clearance.num_joint_points", num_joint_points); + updateParam(parameters, "eb.clearance.clearance_for_fix", clearance_for_fix); + updateParam(parameters, "eb.clearance.clearance_for_joint", clearance_for_joint); + updateParam(parameters, "eb.clearance.clearance_for_smooth", clearance_for_smooth); + } + + { // weight + updateParam(parameters, "eb.weight.smooth_weight", smooth_weight); + updateParam(parameters, "eb.weight.lat_error_weight", lat_error_weight); + } + + { // qp + updateParam(parameters, "eb.qp.max_iteration", qp_param.max_iteration); + updateParam(parameters, "eb.qp.eps_abs", qp_param.eps_abs); + updateParam(parameters, "eb.qp.eps_rel", qp_param.eps_rel); + } +} + +EBPathSmoother::EBPathSmoother( + rclcpp::Node * node, const bool enable_debug_info, const EgoNearestParam ego_nearest_param, + const TrajectoryParam & traj_param, const std::shared_ptr time_keeper_ptr) +: enable_debug_info_(enable_debug_info), + ego_nearest_param_(ego_nearest_param), + traj_param_(traj_param), + time_keeper_ptr_(time_keeper_ptr), + logger_(node->get_logger().get_child("eb_path_smoother")) +{ + // eb param + eb_param_ = EBParam(node); + + // publisher + debug_eb_traj_pub_ = node->create_publisher("~/debug/eb_traj", 1); + debug_eb_fixed_traj_pub_ = node->create_publisher("~/debug/eb_fixed_traj", 1); +} + +void EBPathSmoother::onParam(const std::vector & parameters) +{ + eb_param_.onParam(parameters); +} + +void EBPathSmoother::initialize(const bool enable_debug_info, const TrajectoryParam & traj_param) +{ + enable_debug_info_ = enable_debug_info; + traj_param_ = traj_param; +} + +void EBPathSmoother::resetPreviousData() { prev_eb_traj_points_ptr_ = nullptr; } + +std::optional> +EBPathSmoother::getEBTrajectory(const PlannerData & planner_data) +{ + time_keeper_ptr_->tic(__func__); + + const auto & p = planner_data; + + // 1. crop trajectory + const double forward_traj_length = eb_param_.num_points * eb_param_.delta_arc_length; + const double backward_traj_length = traj_param_.output_backward_traj_length; + + const size_t ego_seg_idx = + trajectory_utils::findEgoSegmentIndex(p.traj_points, p.ego_pose, ego_nearest_param_); + const auto cropped_traj_points = trajectory_utils::cropPoints( + p.traj_points, p.ego_pose.position, ego_seg_idx, forward_traj_length, -backward_traj_length); + + // check if goal is contained in cropped_traj_points + const bool is_goal_contained = + geometry_utils::isSamePoint(cropped_traj_points.back(), planner_data.traj_points.back()); + + // 2. insert fixed point + // NOTE: This should be after cropping trajectory so that fixed point will not be cropped. + const auto traj_points_with_fixed_point = insertFixedPoint(cropped_traj_points); + + // 3. resample trajectory with delta_arc_length + const auto resampled_traj_points = [&]() { + // NOTE: If the interval of points is not constant, the optimization is sometimes unstable. + // Therefore, we do not resample a stop point here. + auto tmp_traj_points = trajectory_utils::resampleTrajectoryPointsWithoutStopPoint( + traj_points_with_fixed_point, eb_param_.delta_arc_length); + + // NOTE: The front point is previous optimized one, and the others are the input ones. + // There may be a lateral error between the points, which makes orientation unexpected. + // Therefore, the front pose is updated after resample. + tmp_traj_points.front().pose = traj_points_with_fixed_point.front().pose; + return tmp_traj_points; + }(); + + // 4. pad trajectory points + const auto [padded_traj_points, pad_start_idx] = getPaddedTrajectoryPoints(resampled_traj_points); + + // 5. update constraint for elastic band's QP + updateConstraint(p.header, padded_traj_points, is_goal_contained, pad_start_idx); + + // 6. get optimization result + const auto optimized_points = optimizeTrajectory(); + if (!optimized_points) { + RCLCPP_INFO_EXPRESSION( + logger_, enable_debug_info_, "return std::nullopt since smoothing failed"); + return std::nullopt; + } + + // 7. convert optimization result to trajectory + const auto eb_traj_points = + convertOptimizedPointsToTrajectory(*optimized_points, padded_traj_points, pad_start_idx); + if (!eb_traj_points) { + RCLCPP_WARN(logger_, "return std::nullopt since x or y error is too large"); + return std::nullopt; + } + + prev_eb_traj_points_ptr_ = std::make_shared>(*eb_traj_points); + + // 8. publish eb trajectory + const auto eb_traj = trajectory_utils::createTrajectory(p.header, *eb_traj_points); + debug_eb_traj_pub_->publish(eb_traj); + + time_keeper_ptr_->toc(__func__, " "); + return *eb_traj_points; +} + +std::vector EBPathSmoother::insertFixedPoint( + const std::vector & traj_points) const +{ + time_keeper_ptr_->tic(__func__); + + if (!prev_eb_traj_points_ptr_) { + return traj_points; + } + + auto traj_points_with_fixed_point = traj_points; + // replace the front pose with previous points + trajectory_utils::updateFrontPointForFix( + traj_points_with_fixed_point, *prev_eb_traj_points_ptr_, eb_param_.delta_arc_length, + ego_nearest_param_); + + time_keeper_ptr_->toc(__func__, " "); + return traj_points_with_fixed_point; +} + +std::tuple, size_t> EBPathSmoother::getPaddedTrajectoryPoints( + const std::vector & traj_points) const +{ + time_keeper_ptr_->tic(__func__); + + const size_t pad_start_idx = + std::min(static_cast(eb_param_.num_points), traj_points.size()); + + std::vector padded_traj_points; + for (size_t i = 0; i < static_cast(eb_param_.num_points); ++i) { + const size_t point_idx = i < pad_start_idx ? i : pad_start_idx - 1; + padded_traj_points.push_back(traj_points.at(point_idx)); + } + + time_keeper_ptr_->toc(__func__, " "); + return {padded_traj_points, pad_start_idx}; +} + +void EBPathSmoother::updateConstraint( + const std_msgs::msg::Header & header, const std::vector & traj_points, + const bool is_goal_contained, const int pad_start_idx) +{ + time_keeper_ptr_->tic(__func__); + + const auto & p = eb_param_; + + std::vector debug_fixed_traj_points; // for debug + + const Eigen::MatrixXd A = Eigen::MatrixXd::Identity(p.num_points, p.num_points); + std::vector upper_bound(p.num_points, 0.0); + std::vector lower_bound(p.num_points, 0.0); + for (size_t i = 0; i < static_cast(p.num_points); ++i) { + const double constraint_segment_length = [&]() { + if (i == 0) { + // NOTE: Only first point can be fixed since there is a lateral deviation + // between the two points. + // The front point is previous optimized one, and the others are the input ones. + return p.clearance_for_fix; + } + if (is_goal_contained) { + // NOTE: fix goal and its previous pose to keep goal orientation + if (p.num_points - 2 <= static_cast(i) || pad_start_idx - 2 <= static_cast(i)) { + return p.clearance_for_fix; + } + } + if (i < static_cast(p.num_joint_points) + 1) { // 1 is added since index 0 is fixed + // point + return p.clearance_for_joint; + } + return p.clearance_for_smooth; + }(); + + upper_bound.at(i) = constraint_segment_length; + lower_bound.at(i) = -constraint_segment_length; + + if (constraint_segment_length == 0.0) { + debug_fixed_traj_points.push_back(traj_points.at(i)); + } + } + + Eigen::VectorXd x_mat(2 * p.num_points); + Eigen::MatrixXd theta_mat = Eigen::MatrixXd::Zero(p.num_points, 2 * p.num_points); + for (size_t i = 0; i < static_cast(p.num_points); ++i) { + x_mat(i) = traj_points.at(i).pose.position.x; + x_mat(i + p.num_points) = traj_points.at(i).pose.position.y; + + const double yaw = tf2::getYaw(traj_points.at(i).pose.orientation); + theta_mat(i, i) = -std::sin(yaw); + theta_mat(i, i + p.num_points) = std::cos(yaw); + } + + // calculate P + const Eigen::MatrixXd raw_P_for_smooth = p.smooth_weight * makePMatrix(p.num_points); + const Eigen::MatrixXd P_for_smooth = theta_mat * raw_P_for_smooth * theta_mat.transpose(); + const Eigen::MatrixXd P_for_lat_error = + p.lat_error_weight * Eigen::MatrixXd::Identity(p.num_points, p.num_points); + const Eigen::MatrixXd P = P_for_smooth + P_for_lat_error; + + // calculate q + const Eigen::VectorXd raw_q_for_smooth = theta_mat * raw_P_for_smooth * x_mat; + const auto q = toStdVector(raw_q_for_smooth); + + if (p.enable_warm_start && osqp_solver_ptr_) { + osqp_solver_ptr_->updateP(P); + osqp_solver_ptr_->updateQ(q); + osqp_solver_ptr_->updateA(A); + osqp_solver_ptr_->updateBounds(lower_bound, upper_bound); + osqp_solver_ptr_->updateEpsRel(p.qp_param.eps_rel); + } else { + osqp_solver_ptr_ = std::make_unique( + P, A, q, lower_bound, upper_bound, p.qp_param.eps_abs); + osqp_solver_ptr_->updateEpsRel(p.qp_param.eps_rel); + osqp_solver_ptr_->updateEpsAbs(p.qp_param.eps_abs); + osqp_solver_ptr_->updateMaxIter(p.qp_param.max_iteration); + } + + // publish fixed trajectory + const auto eb_fixed_traj = trajectory_utils::createTrajectory(header, debug_fixed_traj_points); + debug_eb_fixed_traj_pub_->publish(eb_fixed_traj); + + time_keeper_ptr_->toc(__func__, " "); +} + +std::optional> EBPathSmoother::optimizeTrajectory() +{ + time_keeper_ptr_->tic(__func__); + + // solve QP + const auto result = osqp_solver_ptr_->optimize(); + const auto optimized_points = std::get<0>(result); + + const auto status = std::get<3>(result); + + // check status + if (status != 1) { + osqp_solver_ptr_->logUnsolvedStatus("[EB]"); + return std::nullopt; + } + + time_keeper_ptr_->toc(__func__, " "); + return optimized_points; +} + +std::optional> EBPathSmoother::convertOptimizedPointsToTrajectory( + const std::vector & optimized_points, const std::vector & traj_points, + const int pad_start_idx) const +{ + time_keeper_ptr_->tic(__func__); + + std::vector eb_traj_points; + + // update only x and y + for (size_t i = 0; i < static_cast(pad_start_idx); ++i) { + const double lat_offset = optimized_points.at(i); + + // validate optimization result + if (eb_param_.enable_optimization_validation) { + if (eb_param_.max_validation_error < std::abs(lat_offset)) { + return std::nullopt; + } + } + + auto eb_traj_point = traj_points.at(i); + eb_traj_point.pose = + tier4_autoware_utils::calcOffsetPose(eb_traj_point.pose, 0.0, lat_offset, 0.0); + eb_traj_points.push_back(eb_traj_point); + } + + // update orientation + motion_utils::insertOrientation(eb_traj_points, true); + + time_keeper_ptr_->toc(__func__, " "); + return eb_traj_points; +} +} // namespace obstacle_avoidance_planner diff --git a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp index bf62a10bbf8a5..dc95d7e1809a0 100644 --- a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp +++ b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp @@ -1,4 +1,4 @@ -// Copyright 2020 Tier IV, Inc. +// Copyright 2023 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -15,882 +15,948 @@ #include "obstacle_avoidance_planner/mpt_optimizer.hpp" #include "interpolation/spline_interpolation_points_2d.hpp" -#include "obstacle_avoidance_planner/utils/utils.hpp" -#include "obstacle_avoidance_planner/vehicle_model/vehicle_model_bicycle_kinematics.hpp" +#include "motion_utils/motion_utils.hpp" +#include "obstacle_avoidance_planner/utils/geometry_utils.hpp" +#include "obstacle_avoidance_planner/utils/trajectory_utils.hpp" #include "tf2/utils.h" -#include "tier4_autoware_utils/geometry/boost_geometry.hpp" - -#include "nav_msgs/msg/map_meta_data.hpp" - -#include "boost/assign/list_of.hpp" -#include "boost/optional.hpp" +#include "tier4_autoware_utils/tier4_autoware_utils.hpp" #include #include #include -#include +#include #include -#include +namespace obstacle_avoidance_planner +{ namespace { -geometry_msgs::msg::Pose convertRefPointsToPose(const ReferencePoint & ref_point) +std::tuple, std::vector> calcVehicleCirclesByUniformCircle( + const vehicle_info_util::VehicleInfo & vehicle_info, const size_t circle_num, + const double radius_ratio) { - geometry_msgs::msg::Pose pose; - pose.position = ref_point.p; - pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(ref_point.yaw); - return pose; + const double radius = std::hypot( + vehicle_info.vehicle_length_m / static_cast(circle_num) / 2.0, + vehicle_info.vehicle_width_m / 2.0) * + radius_ratio; + const std::vector radiuses(circle_num, radius); + + const double unit_lon_length = vehicle_info.vehicle_length_m / static_cast(circle_num); + std::vector longitudinal_offsets; + for (size_t i = 0; i < circle_num; ++i) { + longitudinal_offsets.push_back( + unit_lon_length / 2.0 + unit_lon_length * i - vehicle_info.rear_overhang_m); + } + + return {radiuses, longitudinal_offsets}; } -std::tuple extractBounds( - const std::vector & ref_points, const size_t l_idx, - const VehicleParam & vehicle_param_) +std::tuple, std::vector> calcVehicleCirclesByBicycleModel( + const vehicle_info_util::VehicleInfo & vehicle_info, const size_t circle_num, + const double rear_radius_ratio, const double front_radius_ratio) { - const double base_to_right = (vehicle_param_.wheel_tread / 2.0) + vehicle_param_.right_overhang; - const double base_to_left = (vehicle_param_.wheel_tread / 2.0) + vehicle_param_.left_overhang; - Eigen::VectorXd ub_vec(ref_points.size()); - Eigen::VectorXd lb_vec(ref_points.size()); - for (size_t i = 0; i < ref_points.size(); ++i) { - ub_vec(i) = ref_points.at(i).vehicle_bounds.at(l_idx).upper_bound + base_to_left; - lb_vec(i) = ref_points.at(i).vehicle_bounds.at(l_idx).lower_bound - base_to_right; + if (circle_num < 2) { + throw std::invalid_argument("circle_num is less than 2."); } - return {ub_vec, lb_vec}; + + // 1st circle (rear wheel) + const double rear_radius = vehicle_info.vehicle_width_m / 2.0 * rear_radius_ratio; + const double rear_lon_offset = 0.0; + + // 2nd circle (front wheel) + const double front_radius = + std::hypot( + vehicle_info.vehicle_length_m / static_cast(circle_num) / 2.0, + vehicle_info.vehicle_width_m / 2.0) * + front_radius_ratio; + + const double unit_lon_length = vehicle_info.vehicle_length_m / static_cast(circle_num); + const double front_lon_offset = + unit_lon_length / 2.0 + unit_lon_length * (circle_num - 1) - vehicle_info.rear_overhang_m; + + return {{rear_radius, front_radius}, {rear_lon_offset, front_lon_offset}}; } -/* -Bounds findWidestBounds(const BoundsCandidates & front_bounds_candidates) +std::tuple, std::vector> calcVehicleCirclesByFittingUniformCircle( + const vehicle_info_util::VehicleInfo & vehicle_info, const size_t circle_num) { - double max_width = std::numeric_limits::min(); - size_t max_width_index = 0; - if (front_bounds_candidates.size() != 1) { - for (size_t candidate_idx = 0; candidate_idx < front_bounds_candidates.size(); - ++candidate_idx) { - const auto & front_bounds_candidate = front_bounds_candidates.at(candidate_idx); - const double bound_width = - front_bounds_candidate.upper_bound - front_bounds_candidate.lower_bound; - if (max_width < bound_width) { - max_width_index = candidate_idx; - max_width = bound_width; - } - } + if (circle_num < 2) { + throw std::invalid_argument("circle_num is less than 2."); } - return front_bounds_candidates.at(max_width_index); -} -*/ -geometry_msgs::msg::Pose calcVehiclePose( - const ReferencePoint & ref_point, const double lat_error, const double yaw_error, - const double offset) -{ - geometry_msgs::msg::Pose pose; - pose.position.x = ref_point.p.x - std::sin(ref_point.yaw) * lat_error - - std::cos(ref_point.yaw + yaw_error) * offset; - pose.position.y = ref_point.p.y + std::cos(ref_point.yaw) * lat_error - - std::sin(ref_point.yaw + yaw_error) * offset; - pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(ref_point.yaw + yaw_error); - - return pose; + const double radius = vehicle_info.vehicle_width_m / 2.0; + std::vector radiuses(circle_num, radius); + + const double unit_lon_length = + vehicle_info.vehicle_length_m / static_cast(circle_num - 1); + std::vector longitudinal_offsets; + for (size_t i = 0; i < circle_num; ++i) { + longitudinal_offsets.push_back(unit_lon_length * i - vehicle_info.rear_overhang_m); + radiuses.push_back(radius); + } + + return {radiuses, longitudinal_offsets}; } -template -void trimPoints(std::vector & points) +std::tuple extractBounds( + const std::vector & ref_points, const size_t l_idx, const double offset) { - std::vector trimmed_points; - constexpr double epsilon = 1e-6; - - auto itr = points.begin(); - while (itr != points.end() - 1) { - bool is_overlapping = false; - if (itr != points.begin()) { - const auto & p_front = tier4_autoware_utils::getPoint(*itr); - const auto & p_back = tier4_autoware_utils::getPoint(*(itr + 1)); - - const double dx = p_front.x - p_back.x; - const double dy = p_front.y - p_back.y; - if (dx * dx + dy * dy < epsilon) { - is_overlapping = true; - } - } - if (is_overlapping) { - itr = points.erase(itr); - } else { - ++itr; - } + Eigen::VectorXd ub_vec(ref_points.size()); + Eigen::VectorXd lb_vec(ref_points.size()); + for (size_t i = 0; i < ref_points.size(); ++i) { + ub_vec(i) = ref_points.at(i).bounds_on_constraints.at(l_idx).upper_bound + offset; + lb_vec(i) = ref_points.at(i).bounds_on_constraints.at(l_idx).lower_bound - offset; } + return {ub_vec, lb_vec}; } -std::vector eigenVectorToStdVector(const Eigen::VectorXd & eigen_vec) +std::vector toStdVector(const Eigen::VectorXd & eigen_vec) { return {eigen_vec.data(), eigen_vec.data() + eigen_vec.rows()}; } -double calcLateralError( - const geometry_msgs::msg::Point & target_point, const geometry_msgs::msg::Pose ref_pose) +// NOTE: much faster than boost::geometry::intersection() +std::optional intersect( + const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2, + const geometry_msgs::msg::Point & p3, const geometry_msgs::msg::Point & p4) { - const double err_x = target_point.x - ref_pose.position.x; - const double err_y = target_point.y - ref_pose.position.y; - const double ref_yaw = tf2::getYaw(ref_pose.orientation); - const double lat_err = -std::sin(ref_yaw) * err_x + std::cos(ref_yaw) * err_y; - return lat_err; + // calculate intersection point + const double det = (p1.x - p2.x) * (p4.y - p3.y) - (p4.x - p3.x) * (p1.y - p2.y); + if (det == 0.0) { + return std::nullopt; + } + + const double t = ((p4.y - p3.y) * (p4.x - p2.x) + (p3.x - p4.x) * (p4.y - p2.y)) / det; + const double s = ((p2.y - p1.y) * (p4.x - p2.x) + (p1.x - p2.x) * (p4.y - p2.y)) / det; + if (t < 0 || 1 < t || s < 0 || 1 < s) { + return std::nullopt; + } + + geometry_msgs::msg::Point intersect_point; + intersect_point.x = t * p1.x + (1.0 - t) * p2.x; + intersect_point.y = t * p1.y + (1.0 - t) * p2.y; + return intersect_point; } -Eigen::Vector2d getState( - const geometry_msgs::msg::Pose & target_pose, const geometry_msgs::msg::Pose & ref_pose) +bool isLeft(const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Point & target_pos) { - const double lat_error = calcLateralError(target_pose.position, ref_pose); - const double yaw_error = tier4_autoware_utils::normalizeRadian( - tf2::getYaw(target_pose.orientation) - tf2::getYaw(ref_pose.orientation)); - Eigen::VectorXd kinematics = Eigen::VectorXd::Zero(2); - kinematics << lat_error, yaw_error; - return kinematics; + const double base_theta = tf2::getYaw(pose.orientation); + const double target_theta = tier4_autoware_utils::calcAzimuthAngle(pose.position, target_pos); + const double diff_theta = tier4_autoware_utils::normalizeRadian(target_theta - base_theta); + return diff_theta > 0; } -std::vector splineYawFromReferencePoints(const std::vector & ref_points) +// NOTE: Regarding boundary's sign, left is positive, and right is negative +double calcLateralDistToBounds( + const geometry_msgs::msg::Pose & pose, const std::vector & bound, + const double additional_offset, const bool is_left_bound = true) { - if (ref_points.size() == 1) { - return {ref_points.front().yaw}; - } + constexpr double max_lat_offset_for_left = 5.0; + constexpr double min_lat_offset_for_left = -5.0; - std::vector points; - for (const auto & ref_point : ref_points) { - points.push_back(ref_point.p); + const double max_lat_offset = is_left_bound ? max_lat_offset_for_left : -max_lat_offset_for_left; + const double min_lat_offset = is_left_bound ? min_lat_offset_for_left : -min_lat_offset_for_left; + const auto max_lat_offset_point = + tier4_autoware_utils::calcOffsetPose(pose, 0.0, max_lat_offset, 0.0).position; + const auto min_lat_offset_point = + tier4_autoware_utils::calcOffsetPose(pose, 0.0, min_lat_offset, 0.0).position; + + double closest_dist_to_bound = max_lat_offset; + for (size_t i = 0; i < bound.size() - 1; ++i) { + const auto intersect_point = + intersect(min_lat_offset_point, max_lat_offset_point, bound.at(i), bound.at(i + 1)); + if (intersect_point) { + const bool is_point_left = isLeft(pose, *intersect_point); + const double dist_to_bound = + tier4_autoware_utils::calcDistance2d(pose.position, *intersect_point) * + (is_point_left ? 1.0 : -1.0); + + closest_dist_to_bound = + is_left_bound ? std::min(dist_to_bound - additional_offset, closest_dist_to_bound) + : std::max(dist_to_bound + additional_offset, closest_dist_to_bound); + } } - return interpolation::splineYawFromPoints(points); + + return closest_dist_to_bound; } +} // namespace -size_t findNearestIndexWithSoftYawConstraints( - const std::vector & points, const geometry_msgs::msg::Pose & pose, - const double dist_threshold, const double yaw_threshold) +MPTOptimizer::MPTParam::MPTParam( + rclcpp::Node * node, const vehicle_info_util::VehicleInfo & vehicle_info) { - const auto points_with_yaw = points_utils::convertToPosesWithYawEstimation(points); + { // option + steer_limit_constraint = node->declare_parameter("mpt.option.steer_limit_constraint"); + enable_warm_start = node->declare_parameter("mpt.option.enable_warm_start"); + enable_manual_warm_start = node->declare_parameter("mpt.option.enable_manual_warm_start"); + enable_optimization_validation = + node->declare_parameter("mpt.option.enable_optimization_validation"); + mpt_visualize_sampling_num = node->declare_parameter("mpt.option.visualize_sampling_num"); + } - const auto nearest_idx_optional = - motion_utils::findNearestIndex(points_with_yaw, pose, dist_threshold, yaw_threshold); - return nearest_idx_optional ? *nearest_idx_optional - : motion_utils::findNearestIndex(points_with_yaw, pose.position); -} + { // common + num_points = node->declare_parameter("mpt.common.num_points"); + delta_arc_length = node->declare_parameter("mpt.common.delta_arc_length"); + } -boost::optional intersection( - const Eigen::Vector2d & start_point1, const Eigen::Vector2d & end_point1, - const Eigen::Vector2d & start_point2, const Eigen::Vector2d & end_point2) -{ - using Point = boost::geometry::model::d2::point_xy; - using Line = boost::geometry::model::linestring; + // kinematics + max_steer_rad = vehicle_info.max_steer_angle_rad; + + // NOTE: By default, optimization_center_offset will be vehicle_info.wheel_base * 0.8 + // The 0.8 scale is adopted as it performed the best. + constexpr double default_wheel_base_ratio = 0.8; + optimization_center_offset = node->declare_parameter( + "mpt.kinematics.optimization_center_offset", + vehicle_info.wheel_base_m * default_wheel_base_ratio); + + { // clearance + hard_clearance_from_road = + node->declare_parameter("mpt.clearance.hard_clearance_from_road"); + soft_clearance_from_road = + node->declare_parameter("mpt.clearance.soft_clearance_from_road"); + } - const Line line1 = - boost::assign::list_of(start_point1(0), start_point1(1))(end_point1(0), end_point1(1)); - const Line line2 = - boost::assign::list_of(start_point2(0), start_point2(1))(end_point2(0), end_point2(1)); + { // weight + soft_collision_free_weight = + node->declare_parameter("mpt.weight.soft_collision_free_weight"); + + lat_error_weight = node->declare_parameter("mpt.weight.lat_error_weight"); + yaw_error_weight = node->declare_parameter("mpt.weight.yaw_error_weight"); + yaw_error_rate_weight = node->declare_parameter("mpt.weight.yaw_error_rate_weight"); + steer_input_weight = node->declare_parameter("mpt.weight.steer_input_weight"); + steer_rate_weight = node->declare_parameter("mpt.weight.steer_rate_weight"); + + terminal_lat_error_weight = + node->declare_parameter("mpt.weight.terminal_lat_error_weight"); + terminal_yaw_error_weight = + node->declare_parameter("mpt.weight.terminal_yaw_error_weight"); + goal_lat_error_weight = node->declare_parameter("mpt.weight.goal_lat_error_weight"); + goal_yaw_error_weight = node->declare_parameter("mpt.weight.goal_yaw_error_weight"); + } - std::vector output; - boost::geometry::intersection(line1, line2, output); - if (output.empty()) { - return {}; + { // avoidance + max_avoidance_cost = node->declare_parameter("mpt.avoidance.max_avoidance_cost"); + avoidance_cost_margin = node->declare_parameter("mpt.avoidance.avoidance_cost_margin"); + avoidance_cost_band_length = + node->declare_parameter("mpt.avoidance.avoidance_cost_band_length"); + avoidance_cost_decrease_rate = + node->declare_parameter("mpt.avoidance.avoidance_cost_decrease_rate"); + + avoidance_lat_error_weight = + node->declare_parameter("mpt.avoidance.weight.lat_error_weight"); + avoidance_yaw_error_weight = + node->declare_parameter("mpt.avoidance.weight.yaw_error_weight"); + avoidance_steer_input_weight = + node->declare_parameter("mpt.avoidance.weight.steer_input_weight"); + } + + { // collision free constraints + l_inf_norm = node->declare_parameter("mpt.collision_free_constraints.option.l_inf_norm"); + soft_constraint = + node->declare_parameter("mpt.collision_free_constraints.option.soft_constraint"); + hard_constraint = + node->declare_parameter("mpt.collision_free_constraints.option.hard_constraint"); + } + + { // vehicle_circles + // NOTE: Vehicle shape for collision free constraints is considered as a set of circles + vehicle_circles_method = + node->declare_parameter("mpt.collision_free_constraints.vehicle_circles.method"); + + // uniform circles + vehicle_circles_uniform_circle_num = node->declare_parameter( + "mpt.collision_free_constraints.vehicle_circles.uniform_circle.num"); + vehicle_circles_uniform_circle_radius_ratio = node->declare_parameter( + "mpt.collision_free_constraints.vehicle_circles.uniform_circle.radius_ratio"); + + // bicycle model + vehicle_circles_bicycle_model_num = node->declare_parameter( + "mpt.collision_free_constraints.vehicle_circles.bicycle_model.num_for_" + "calculation"); + vehicle_circles_bicycle_model_rear_radius_ratio = node->declare_parameter( + "mpt.collision_free_constraints.vehicle_circles." + "bicycle_model.rear_radius_ratio"); + vehicle_circles_bicycle_model_front_radius_ratio = node->declare_parameter( + "mpt.collision_free_constraints.vehicle_circles." + "bicycle_model.front_radius_ratio"); + + // fitting uniform circles + vehicle_circles_fitting_uniform_circle_num = node->declare_parameter( + "mpt.collision_free_constraints.vehicle_circles.fitting_uniform_circle.num"); } - const Eigen::Vector2d output_point{output.front().x(), output.front().y()}; - return output_point; + { // validation + max_validation_lat_error = node->declare_parameter("mpt.validation.max_lat_error"); + max_validation_yaw_error = node->declare_parameter("mpt.validation.max_yaw_error"); + } } -double calcLateralDistToBound( - const Eigen::Vector2d & current_point, const Eigen::Vector2d & edge_point, - const std::vector & bound, const bool is_right_bound = false) +void MPTOptimizer::MPTParam::onParam(const std::vector & parameters) { - for (size_t i = 0; i < bound.size() - 1; ++i) { - const Eigen::Vector2d current_bound_point = {bound.at(i).x, bound.at(i).y}; - const Eigen::Vector2d next_bound_point = {bound.at(i + 1).x, bound.at(i + 1).y}; - - const auto intersects_point = - intersection(current_point, edge_point, current_bound_point, next_bound_point); - if (intersects_point) { - const double dist = (*intersects_point - current_point).norm(); - return is_right_bound ? -dist : dist; - } + using tier4_autoware_utils::updateParam; + + { // option + updateParam(parameters, "mpt.option.steer_limit_constraint", steer_limit_constraint); + updateParam(parameters, "mpt.option.enable_warm_start", enable_warm_start); + updateParam(parameters, "mpt.option.enable_manual_warm_start", enable_manual_warm_start); + updateParam( + parameters, "mpt.option.enable_optimization_validation", enable_optimization_validation); + updateParam(parameters, "mpt.option.visualize_sampling_num", mpt_visualize_sampling_num); + } + + // common + updateParam(parameters, "mpt.common.num_points", num_points); + updateParam(parameters, "mpt.common.delta_arc_length", delta_arc_length); + + // kinematics + updateParam( + parameters, "mpt.kinematics.optimization_center_offset", optimization_center_offset); + + // collision_free_constraints + updateParam(parameters, "mpt.collision_free_constraints.option.l_inf_norm", l_inf_norm); + updateParam( + parameters, "mpt.collision_free_constraints.option.soft_constraint", soft_constraint); + updateParam( + parameters, "mpt.collision_free_constraints.option.hard_constraint", hard_constraint); + + { // vehicle_circles + updateParam( + parameters, "mpt.collision_free_constraints.vehicle_circles.method", vehicle_circles_method); + + // uniform circles + updateParam( + parameters, "mpt.collision_free_constraints.vehicle_circles.uniform_circle.num", + vehicle_circles_uniform_circle_num); + updateParam( + parameters, "mpt.collision_free_constraints.vehicle_circles.uniform_circle.radius_ratio", + vehicle_circles_uniform_circle_radius_ratio); + + // bicycle model + updateParam( + parameters, + "mpt.collision_free_constraints.vehicle_circles.bicycle_model.num_for_calculation", + vehicle_circles_bicycle_model_num); + updateParam( + parameters, "mpt.collision_free_constraints.vehicle_circles.bicycle_model.rear_radius_ratio", + vehicle_circles_bicycle_model_rear_radius_ratio); + updateParam( + parameters, "mpt.collision_free_constraints.vehicle_circles.bicycle_model.front_radius_ratio", + vehicle_circles_bicycle_model_front_radius_ratio); + + // fitting uniform circles + updateParam( + parameters, "mpt.collision_free_constraints.vehicle_circles.fitting_uniform_circle.num", + vehicle_circles_fitting_uniform_circle_num); + } + + { // clearance + updateParam( + parameters, "mpt.clearance.hard_clearance_from_road", hard_clearance_from_road); + updateParam( + parameters, "mpt.clearance.soft_clearance_from_road", soft_clearance_from_road); } - return is_right_bound ? -5.0 : 5.0; + { // weight + updateParam( + parameters, "mpt.weight.soft_collision_free_weight", soft_collision_free_weight); + + updateParam(parameters, "mpt.weight.lat_error_weight", lat_error_weight); + updateParam(parameters, "mpt.weight.yaw_error_weight", yaw_error_weight); + updateParam(parameters, "mpt.weight.yaw_error_rate_weight", yaw_error_rate_weight); + updateParam(parameters, "mpt.weight.steer_input_weight", steer_input_weight); + updateParam(parameters, "mpt.weight.steer_rate_weight", steer_rate_weight); + + updateParam( + parameters, "mpt.weight.terminal_lat_error_weight", terminal_lat_error_weight); + updateParam( + parameters, "mpt.weight.terminal_yaw_error_weight", terminal_yaw_error_weight); + updateParam(parameters, "mpt.weight.goal_lat_error_weight", goal_lat_error_weight); + updateParam(parameters, "mpt.weight.goal_yaw_error_weight", goal_yaw_error_weight); + } + + { // avoidance + updateParam(parameters, "mpt.avoidance.max_avoidance_cost", max_avoidance_cost); + updateParam(parameters, "mpt.avoidance.avoidance_cost_margin", avoidance_cost_margin); + updateParam( + parameters, "mpt.avoidance.avoidance_cost_band_length", avoidance_cost_band_length); + updateParam( + parameters, "mpt.avoidance.avoidance_cost_decrease_rate", avoidance_cost_decrease_rate); + + updateParam( + parameters, "mpt.avoidance.weight.lat_error_weight", avoidance_lat_error_weight); + updateParam( + parameters, "mpt.avoidance.weight.yaw_error_weight", avoidance_yaw_error_weight); + updateParam( + parameters, "mpt.avoidance.weight.steer_input_weight", avoidance_steer_input_weight); + } + + { // validation + updateParam(parameters, "mpt.validation.max_lat_error", max_validation_lat_error); + updateParam(parameters, "mpt.validation.max_yaw_error", max_validation_yaw_error); + } } -} // namespace MPTOptimizer::MPTOptimizer( - const bool is_showing_debug_info, const TrajectoryParam & traj_param, - const VehicleParam & vehicle_param, const MPTParam & mpt_param) -: is_showing_debug_info_(is_showing_debug_info), + rclcpp::Node * node, const bool enable_debug_info, const EgoNearestParam ego_nearest_param, + const vehicle_info_util::VehicleInfo & vehicle_info, const TrajectoryParam & traj_param, + const std::shared_ptr debug_data_ptr, + const std::shared_ptr time_keeper_ptr) +: enable_debug_info_(enable_debug_info), + ego_nearest_param_(ego_nearest_param), + vehicle_info_(vehicle_info), traj_param_(traj_param), - vehicle_param_(vehicle_param), - mpt_param_(mpt_param), - vehicle_model_ptr_( - std::make_unique(vehicle_param_.wheelbase, mpt_param_.max_steer_rad)), - osqp_solver_ptr_(std::make_unique(osqp_epsilon_)) + debug_data_ptr_(debug_data_ptr), + time_keeper_ptr_(time_keeper_ptr), + logger_(node->get_logger().get_child("mpt_optimizer")) { + // initialize mpt param + mpt_param_ = MPTParam(node, vehicle_info); + updateVehicleCircles(); + debug_data_ptr_->mpt_visualize_sampling_num = mpt_param_.mpt_visualize_sampling_num; + + // state equation generator + state_equation_generator_ = + StateEquationGenerator(vehicle_info_.wheel_base_m, mpt_param_.max_steer_rad, time_keeper_ptr_); + + // osqp solver + osqp_solver_ptr_ = std::make_unique(osqp_epsilon_); + + // publisher + debug_fixed_traj_pub_ = node->create_publisher("~/debug/mpt_fixed_traj", 1); + debug_ref_traj_pub_ = node->create_publisher("~/debug/mpt_ref_traj", 1); + debug_mpt_traj_pub_ = node->create_publisher("~/debug/mpt_traj", 1); } -boost::optional MPTOptimizer::getModelPredictiveTrajectory( - const bool enable_avoidance, - const std::vector & smoothed_points, - const std::vector & path_points, - const std::vector & left_bound, - const std::vector & right_bound, - const std::unique_ptr & prev_trajs, - const geometry_msgs::msg::Pose & current_ego_pose, const double current_ego_vel, - DebugData & debug_data) +void MPTOptimizer::updateVehicleCircles() { - stop_watch_.tic(__func__); - - current_ego_pose_ = current_ego_pose; - current_ego_vel_ = current_ego_vel; - - if (smoothed_points.empty()) { - RCLCPP_INFO_EXPRESSION( - rclcpp::get_logger("mpt_optimizer"), is_showing_debug_info_, - "return boost::none since smoothed_points is empty"); - return boost::none; + const auto & p = mpt_param_; + + if (p.vehicle_circles_method == "uniform_circle") { + std::tie(vehicle_circle_radiuses_, vehicle_circle_longitudinal_offsets_) = + calcVehicleCirclesByUniformCircle( + vehicle_info_, p.vehicle_circles_uniform_circle_num, + p.vehicle_circles_uniform_circle_radius_ratio); + } else if (p.vehicle_circles_method == "bicycle_model") { + std::tie(vehicle_circle_radiuses_, vehicle_circle_longitudinal_offsets_) = + calcVehicleCirclesByBicycleModel( + vehicle_info_, p.vehicle_circles_bicycle_model_num, + p.vehicle_circles_bicycle_model_front_radius_ratio, + p.vehicle_circles_bicycle_model_rear_radius_ratio); + } else if (p.vehicle_circles_method == "fitting_uniform_circle") { + std::tie(vehicle_circle_radiuses_, vehicle_circle_longitudinal_offsets_) = + calcVehicleCirclesByFittingUniformCircle( + vehicle_info_, p.vehicle_circles_fitting_uniform_circle_num); + } else { + throw std::invalid_argument("mpt_param_.vehicle_circles_method is invalid."); } - std::vector full_ref_points = - getReferencePoints(smoothed_points, left_bound, right_bound, prev_trajs, debug_data); - if (full_ref_points.empty()) { - RCLCPP_INFO_EXPRESSION( - rclcpp::get_logger("mpt_optimizer"), is_showing_debug_info_, - "return boost::none since ref_points is empty"); - return boost::none; - } else if (full_ref_points.size() == 1) { - RCLCPP_INFO_EXPRESSION( - rclcpp::get_logger("mpt_optimizer"), is_showing_debug_info_, - "return boost::none since ref_points.size() == 1"); - return boost::none; - } + debug_data_ptr_->vehicle_circle_radiuses = vehicle_circle_radiuses_; + debug_data_ptr_->vehicle_circle_longitudinal_offsets = vehicle_circle_longitudinal_offsets_; +} - debug_data.mpt_fixed_traj = getMPTFixedPoints(full_ref_points); +void MPTOptimizer::initialize(const bool enable_debug_info, const TrajectoryParam & traj_param) +{ + enable_debug_info_ = enable_debug_info; + traj_param_ = traj_param; +} - std::vector fixed_ref_points; - std::vector non_fixed_ref_points; - bool is_fixing_ref_points = true; - for (size_t i = 0; i < full_ref_points.size(); ++i) { - if (i == full_ref_points.size() - 1) { - if (!full_ref_points.at(i).fix_kinematic_state) { - is_fixing_ref_points = false; - } - } else if ( - // fix first three points - full_ref_points.at(i).fix_kinematic_state && full_ref_points.at(i + 1).fix_kinematic_state && - (i + 2 < full_ref_points.size() && full_ref_points.at(i + 2).fix_kinematic_state) && - (i + 3 < full_ref_points.size() && full_ref_points.at(i + 3).fix_kinematic_state)) { - } else { - is_fixing_ref_points = false; - } +void MPTOptimizer::resetPreviousData() { prev_ref_points_ptr_ = nullptr; } - if (is_fixing_ref_points) { - fixed_ref_points.push_back(full_ref_points.at(i)); - } else { - non_fixed_ref_points.push_back(full_ref_points.at(i)); - } - } +void MPTOptimizer::onParam(const std::vector & parameters) +{ + mpt_param_.onParam(parameters); + updateVehicleCircles(); + debug_data_ptr_->mpt_visualize_sampling_num = mpt_param_.mpt_visualize_sampling_num; +} - // calculate B and W matrices - const auto mpt_matrix = generateMPTMatrix(non_fixed_ref_points, debug_data); +std::optional> MPTOptimizer::getModelPredictiveTrajectory( + const PlannerData & planner_data, const std::vector & smoothed_points) +{ + time_keeper_ptr_->tic(__func__); - // calculate Q and R matrices - const auto val_matrix = generateValueMatrix(non_fixed_ref_points, path_points, debug_data); + const auto & p = planner_data; + const auto & traj_points = p.traj_points; - const auto optimized_control_variables = executeOptimization( - prev_trajs, enable_avoidance, mpt_matrix, val_matrix, non_fixed_ref_points, debug_data); - if (!optimized_control_variables) { + // 1. calculate reference points + auto ref_points = calcReferencePoints(planner_data, smoothed_points); + if (ref_points.size() < 2) { RCLCPP_INFO_EXPRESSION( - rclcpp::get_logger("mpt_optimizer"), is_showing_debug_info_, - "return boost::none since could not solve qp"); - return boost::none; + logger_, enable_debug_info_, "return std::nullopt since ref_points size is less than 2."); + return std::nullopt; } - const auto mpt_points = getMPTPoints( - fixed_ref_points, non_fixed_ref_points, optimized_control_variables.get(), mpt_matrix, - debug_data); - - // NOTE: Sometimes optimization failed without failed status. - // Therefore, we have to check if optimization was solved correctly by the result. - constexpr double max_lateral_deviation = 3.0; - for (const double lateral_error : debug_data.lateral_errors) { - if (max_lateral_deviation < std::abs(lateral_error)) { - RCLCPP_ERROR( - rclcpp::get_logger("mpt_optimizer"), - "return boost::none since lateral deviation is too large."); - return boost::none; - } - } - constexpr double max_yaw_deviation = 50.0 / 180 * 3.14; - for (const double yaw_error : debug_data.yaw_errors) { - if (max_yaw_deviation < std::abs(yaw_error)) { - RCLCPP_ERROR( - rclcpp::get_logger("mpt_optimizer"), - "return boost::none since yaw deviation is too large."); - return boost::none; - } - } + // 2. calculate B and W matrices where x = B u + W + const auto mpt_mat = state_equation_generator_.calcMatrix(ref_points); - auto full_optimized_ref_points = fixed_ref_points; - full_optimized_ref_points.insert( - full_optimized_ref_points.end(), non_fixed_ref_points.begin(), non_fixed_ref_points.end()); + // 3. calculate Q and R matrices where J(x, u) = x^t Q x + u^t R u + const auto val_mat = calcValueMatrix(ref_points, traj_points); - debug_data.msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) << " [ms]\n"; + // 4. get objective matrix + const auto obj_mat = calcObjectiveMatrix(mpt_mat, val_mat, ref_points); - MPTTrajs mpt_trajs; - mpt_trajs.mpt = mpt_points; - mpt_trajs.ref_points = full_optimized_ref_points; - return mpt_trajs; -} + // 5. get constraints matrix + const auto const_mat = calcConstraintMatrix(mpt_mat, ref_points); -std::vector MPTOptimizer::getReferencePoints( - const std::vector & smoothed_points, - const std::vector & left_bound, - const std::vector & right_bound, - const std::unique_ptr & prev_trajs, DebugData & debug_data) const -{ - stop_watch_.tic(__func__); - - const auto ref_points = [&]() { - auto ref_points = [&]() { - // TODO(murooka) consider better algorithm to calculate fixed/non-fixed reference points - const auto fixed_ref_points = getFixedReferencePoints(smoothed_points, prev_trajs); - - // if no fixed_ref_points - if (fixed_ref_points.empty()) { - // interpolate and crop backward - const auto interpolated_points = interpolation_utils::getInterpolatedPoints( - smoothed_points, mpt_param_.delta_arc_length_for_mpt_points); - const auto interpolated_points_with_yaw = - points_utils::convertToPosesWithYawEstimation(interpolated_points); - const auto cropped_interpolated_points_with_yaw = points_utils::clipBackwardPoints( - interpolated_points_with_yaw, current_ego_pose_, traj_param_.backward_fixing_distance, - mpt_param_.delta_arc_length_for_mpt_points, - traj_param_.delta_yaw_threshold_for_closest_point); - const auto cropped_interpolated_points = - points_utils::convertToPoints(cropped_interpolated_points_with_yaw); - - return points_utils::convertToReferencePoints(cropped_interpolated_points); - } + // 6. optimize steer angles + const auto optimized_steer_angles = calcOptimizedSteerAngles(ref_points, obj_mat, const_mat); + if (!optimized_steer_angles) { + RCLCPP_INFO_EXPRESSION( + logger_, enable_debug_info_, "return std::nullopt since could not solve qp"); + return std::nullopt; + } - // calc non fixed traj points - // const auto fixed_ref_points_with_yaw = points_utils::convertToPosesWithYawEstimation( - // points_utils::convertToPoints(fixed_ref_points)); - const auto fixed_ref_points_with_yaw = points_utils::convertToPoses(fixed_ref_points); - const auto seg_idx_optional = motion_utils::findNearestSegmentIndex( - smoothed_points, fixed_ref_points_with_yaw.back(), std::numeric_limits::max(), - traj_param_.delta_yaw_threshold_for_closest_point); - - if (!seg_idx_optional) { - RCLCPP_INFO_EXPRESSION( - rclcpp::get_logger("mpt_optimizer"), is_showing_debug_info_, - "cannot find nearest segment index in getReferencePoints"); - return std::vector{}; - } - const auto seg_idx = *seg_idx_optional; - const auto non_fixed_traj_points = - std::vector{ - smoothed_points.begin() + seg_idx, smoothed_points.end()}; - - const double offset = motion_utils::calcLongitudinalOffsetToSegment( - non_fixed_traj_points, 0, fixed_ref_points.back().p) + - mpt_param_.delta_arc_length_for_mpt_points; - const auto non_fixed_interpolated_traj_points = interpolation_utils::getInterpolatedPoints( - non_fixed_traj_points, mpt_param_.delta_arc_length_for_mpt_points, offset); - const auto non_fixed_ref_points = - points_utils::convertToReferencePoints(non_fixed_interpolated_traj_points); - - // make ref points - auto ref_points = fixed_ref_points; - ref_points.insert(ref_points.end(), non_fixed_ref_points.begin(), non_fixed_ref_points.end()); - - return ref_points; - }(); + // 7. convert to points with validation + const auto mpt_traj_points = calcMPTPoints(ref_points, *optimized_steer_angles, mpt_mat); + if (!mpt_traj_points) { + RCLCPP_WARN(logger_, "return std::nullopt since lateral or yaw error is too large."); + return std::nullopt; + } - if (ref_points.empty()) { - return std::vector{}; - } + // 8. publish trajectories for debug + publishDebugTrajectories(p.header, ref_points, *mpt_traj_points); - // set some information to reference points considering fix kinematics - trimPoints(ref_points); - calcOrientation(ref_points); - calcCurvature(ref_points); - calcArcLength(ref_points); - calcPlanningFromEgo( - ref_points); // NOTE: fix_kinematic_state will be updated when planning from ego - - // crop trajectory with margin to calculate vehicle bounds at the end point - constexpr double tmp_ref_points_margin = 20.0; - const double ref_length_with_margin = - traj_param_.num_sampling_points * mpt_param_.delta_arc_length_for_mpt_points + - tmp_ref_points_margin; - ref_points = points_utils::clipForwardPoints(ref_points, 0, ref_length_with_margin); - if (ref_points.empty()) { - return std::vector{}; - } + time_keeper_ptr_->toc(__func__, " "); + + debug_data_ptr_->ref_points = ref_points; + prev_ref_points_ptr_ = std::make_shared>(ref_points); - // set bounds information - calcBounds(ref_points, left_bound, right_bound, debug_data); - calcVehicleBounds(ref_points, debug_data); + return *mpt_traj_points; +} - // set extra information (alpha and has_object_collision) - // NOTE: This must be after bounds calculation. - calcExtraPoints(ref_points, prev_trajs); +std::vector MPTOptimizer::calcReferencePoints( + const PlannerData & planner_data, const std::vector & smoothed_points) const +{ + time_keeper_ptr_->tic(__func__); - const double ref_length = - traj_param_.num_sampling_points * mpt_param_.delta_arc_length_for_mpt_points; - ref_points = points_utils::clipForwardPoints(ref_points, 0, ref_length); + const auto & p = planner_data; - // bounds information is assigned to debug data after truncating reference points - debug_data.ref_points = ref_points; + const double forward_traj_length = mpt_param_.num_points * mpt_param_.delta_arc_length; + const double backward_traj_length = traj_param_.output_backward_traj_length; - return ref_points; + // 1. resample and convert smoothed points type from trajectory points to reference points + time_keeper_ptr_->tic("resampleReferencePoints"); + auto ref_points = [&]() { + const auto resampled_smoothed_points = + trajectory_utils::resampleTrajectoryPointsWithoutStopPoint( + smoothed_points, mpt_param_.delta_arc_length); + return trajectory_utils::convertToReferencePoints(resampled_smoothed_points); }(); - if (ref_points.empty()) { - return std::vector{}; + time_keeper_ptr_->toc("resampleReferencePoints", " "); + + // 2. crop forward and backward with margin, and calculate spline interpolation + // NOTE: Margin is added to calculate orientation, curvature, etc precisely. + // Start point may change. Spline calculation is required. + constexpr double tmp_margin = 10.0; + size_t ego_seg_idx = + trajectory_utils::findEgoSegmentIndex(ref_points, p.ego_pose, ego_nearest_param_); + ref_points = trajectory_utils::cropPoints( + ref_points, p.ego_pose.position, ego_seg_idx, forward_traj_length + tmp_margin, + -backward_traj_length - tmp_margin); + SplineInterpolationPoints2d ref_points_spline(ref_points); + ego_seg_idx = trajectory_utils::findEgoSegmentIndex(ref_points, p.ego_pose, ego_nearest_param_); + + // 3. calculate orientation and curvature + updateOrientation(ref_points, ref_points_spline); + updateCurvature(ref_points, ref_points_spline); + + // 4. crop backward + // NOTE: Start point may change. Spline calculation is required. + ref_points = trajectory_utils::cropPoints( + ref_points, p.ego_pose.position, ego_seg_idx, forward_traj_length + tmp_margin, + -backward_traj_length); + ref_points_spline = SplineInterpolationPoints2d(ref_points); + ego_seg_idx = trajectory_utils::findEgoSegmentIndex(ref_points, p.ego_pose, ego_nearest_param_); + + // 5. update fixed points, and resample + // NOTE: This must be after backward cropping. + // New start point may be added and resampled. Spline calculation is required. + updateFixedPoint(ref_points); + ref_points_spline = SplineInterpolationPoints2d(ref_points); + + // 6. update bounds + // NOTE: After this, resample must not be called since bounds are not interpolated. + updateBounds(ref_points, p.left_bound, p.right_bound); + updateVehicleBounds(ref_points, ref_points_spline); + + // 7. update delta arc length + updateDeltaArcLength(ref_points); + + // 8. update extra information (alpha and beta) + // NOTE: This must be after calculation of bounds and delta arc length + updateExtraPoints(ref_points); + + // 9. crop forward + // ref_points = trajectory_utils::cropForwardPoints( + // ref_points, p.ego_pose.position, ego_seg_idx, forward_traj_length); + if (static_cast(mpt_param_.num_points) < ref_points.size()) { + ref_points.resize(mpt_param_.num_points); } - debug_data.msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) - << " [ms]\n"; + time_keeper_ptr_->toc(__func__, " "); + return ref_points; } -void MPTOptimizer::calcPlanningFromEgo(std::vector & ref_points) const +void MPTOptimizer::updateOrientation( + std::vector & ref_points, + const SplineInterpolationPoints2d & ref_points_spline) const { - // if plan from ego - constexpr double epsilon = 1e-04; - const double trajectory_length = motion_utils::calcArcLength(ref_points); - - const bool plan_from_ego = mpt_param_.plan_from_ego && std::abs(current_ego_vel_) < epsilon && - ref_points.size() > 1 && - trajectory_length < mpt_param_.max_plan_from_ego_length; - if (plan_from_ego) { - for (auto & ref_point : ref_points) { - ref_point.fix_kinematic_state = boost::none; - } - - /* - // interpolate and crop backward - const auto interpolated_points = interpolation_utils::getInterpolatedPoints( - points, - mpt_param_.delta_arc_length_for_mpt_points); const auto cropped_interpolated_points = - points_utils::clipBackwardPoints( interpolated_points, current_pose_.position, - traj_param_.backward_fixing_distance, mpt_param_.delta_arc_length_for_mpt_points); - - auto cropped_ref_points = - points_utils::convertToReferencePoints(cropped_interpolated_points); - */ - - // assign fix kinematics - const size_t nearest_ref_idx = motion_utils::findFirstNearestIndexWithSoftConstraints( - points_utils::convertToPosesWithYawEstimation(points_utils::convertToPoints(ref_points)), - current_ego_pose_, traj_param_.ego_nearest_dist_threshold, - traj_param_.ego_nearest_yaw_threshold); - - // calculate cropped_ref_points.at(nearest_ref_idx) with yaw - const geometry_msgs::msg::Pose nearest_ref_pose = [&]() -> geometry_msgs::msg::Pose { - geometry_msgs::msg::Pose pose; - pose.position = ref_points.at(nearest_ref_idx).p; - - const size_t prev_nearest_ref_idx = - (nearest_ref_idx == ref_points.size() - 1) ? nearest_ref_idx - 1 : nearest_ref_idx; - pose.orientation = geometry_utils::getQuaternionFromPoints( - ref_points.at(prev_nearest_ref_idx + 1).p, ref_points.at(prev_nearest_ref_idx).p); - return pose; - }(); + const auto yaw_vec = ref_points_spline.getSplineInterpolatedYaws(); + for (size_t i = 0; i < ref_points.size(); ++i) { + ref_points.at(i).pose.orientation = + tier4_autoware_utils::createQuaternionFromYaw(yaw_vec.at(i)); + } +} - ref_points.at(nearest_ref_idx).fix_kinematic_state = - getState(current_ego_pose_, nearest_ref_pose); - ref_points.at(nearest_ref_idx).plan_from_ego = true; +void MPTOptimizer::updateCurvature( + std::vector & ref_points, + const SplineInterpolationPoints2d & ref_points_spline) const +{ + const auto curvature_vec = ref_points_spline.getSplineInterpolatedCurvatures(); + for (size_t i = 0; i < ref_points.size(); ++i) { + ref_points.at(i).curvature = curvature_vec.at(i); } } -std::vector MPTOptimizer::getFixedReferencePoints( - const std::vector & points, - const std::unique_ptr & prev_trajs) const +void MPTOptimizer::updateFixedPoint(std::vector & ref_points) const { - if ( - !prev_trajs || prev_trajs->model_predictive_trajectory.empty() || - prev_trajs->mpt_ref_points.empty() || - prev_trajs->model_predictive_trajectory.size() != prev_trajs->mpt_ref_points.size()) { - return std::vector(); + time_keeper_ptr_->tic(__func__); + + if (!prev_ref_points_ptr_) { + // no fixed point + return; } - if (!mpt_param_.fix_points_around_ego) { - return std::vector(); + // replace the front pose and curvature with previous reference points + const auto idx = trajectory_utils::updateFrontPointForFix( + ref_points, *prev_ref_points_ptr_, mpt_param_.delta_arc_length, ego_nearest_param_); + + // NOTE: memorize front point to be fixed before resampling + const auto front_point = ref_points.front(); + + if (idx && *idx != 0) { + // In order to fix the front "orientation" defined by two front points, insert the previous + // fixed point. + ref_points.insert(ref_points.begin(), prev_ref_points_ptr_->at(static_cast(*idx) - 1)); + + // resample to make ref_points' interval constant. + // NOTE: Only pose, velocity and curvature will be interpolated. + ref_points = trajectory_utils::resampleReferencePoints(ref_points, mpt_param_.delta_arc_length); + + // update pose which is previous one, and fixed kinematic state + // NOTE: There may be a lateral error between the previous and input points. + // Therefore, the pose for fix should not be resampled. + const auto & prev_ref_front_point = prev_ref_points_ptr_->at(*idx); + const auto & prev_ref_prev_front_point = prev_ref_points_ptr_->at(static_cast(*idx) - 1); + + ref_points.front().pose = prev_ref_prev_front_point.pose; + ref_points.front().fixed_kinematic_state = prev_ref_prev_front_point.optimized_kinematic_state; + ref_points.at(1).pose = prev_ref_front_point.pose; + ref_points.at(1).fixed_kinematic_state = prev_ref_front_point.optimized_kinematic_state; + } else { + // resample to make ref_points' interval constant. + // NOTE: Only pose, velocity and curvature will be interpolated. + ref_points = trajectory_utils::resampleReferencePoints(ref_points, mpt_param_.delta_arc_length); + + ref_points.front().pose = front_point.pose; + ref_points.front().curvature = front_point.curvature; + ref_points.front().fixed_kinematic_state = front_point.optimized_kinematic_state; } - const auto & prev_ref_points = prev_trajs->mpt_ref_points; - const int nearest_prev_ref_idx = - static_cast(motion_utils::findFirstNearestIndexWithSoftConstraints( - points_utils::convertToPosesWithYawEstimation(points_utils::convertToPoints(prev_ref_points)), - current_ego_pose_, traj_param_.ego_nearest_dist_threshold, - traj_param_.ego_nearest_yaw_threshold)); + time_keeper_ptr_->toc(__func__, " "); +} - // calculate begin_prev_ref_idx - const int begin_prev_ref_idx = [&]() { - const int backward_fixing_num = - traj_param_.backward_fixing_distance / mpt_param_.delta_arc_length_for_mpt_points; +void MPTOptimizer::updateDeltaArcLength(std::vector & ref_points) const +{ + for (size_t i = 0; i < ref_points.size(); i++) { + ref_points.at(i).delta_arc_length = + (i == ref_points.size() - 1) + ? 0.0 + : tier4_autoware_utils::calcDistance2d(ref_points.at(i + 1), ref_points.at(i)); + } +} - return std::max(0, nearest_prev_ref_idx - backward_fixing_num); - }(); +void MPTOptimizer::updateExtraPoints(std::vector & ref_points) const +{ + // alpha + for (size_t i = 0; i < ref_points.size(); ++i) { + const auto front_wheel_pos = + trajectory_utils::getNearestPosition(ref_points, i, vehicle_info_.wheel_base_m); - // calculate end_prev_ref_idx - const int end_prev_ref_idx = [&]() { - const double forward_fixed_length = std::max( - current_ego_vel_ * traj_param_.forward_fixing_min_time, - traj_param_.forward_fixing_min_distance); + const bool are_too_close_points = + tier4_autoware_utils::calcDistance2d(front_wheel_pos, ref_points.at(i).pose.position) < 1e-03; + const auto front_wheel_yaw = + are_too_close_points + ? ref_points.at(i).getYaw() + : tier4_autoware_utils::calcAzimuthAngle(ref_points.at(i).pose.position, front_wheel_pos); + ref_points.at(i).alpha = + tier4_autoware_utils::normalizeRadian(front_wheel_yaw - ref_points.at(i).getYaw()); + } - const int forward_fixing_num = - forward_fixed_length / mpt_param_.delta_arc_length_for_mpt_points; - return std::min( - static_cast(prev_ref_points.size()) - 1, nearest_prev_ref_idx + forward_fixing_num); - }(); + { // avoidance + // calculate one-step avoidance const + for (size_t i = 0; i < ref_points.size(); ++i) { + const auto normalized_avoidance_cost = calcNormalizedAvoidanceCost(ref_points.at(i)); + if (normalized_avoidance_cost) { + const int max_length_idx = + std::floor(mpt_param_.avoidance_cost_band_length / mpt_param_.delta_arc_length); + for (int j = -max_length_idx; j <= max_length_idx; ++j) { + const int k = i + j; + if (0 <= k && k < static_cast(ref_points.size())) { + ref_points.at(k).normalized_avoidance_cost = *normalized_avoidance_cost; + } + } + } + } - bool points_reaching_prev_points_flag = false; - std::vector fixed_ref_points; - for (size_t i = begin_prev_ref_idx; i <= static_cast(end_prev_ref_idx); ++i) { - const auto & prev_ref_point = prev_ref_points.at(i); + /* + // update avoidance cost between longitudinally close obstacles + constexpr double max_longitudinal_length_to_fill_drivable_area = 50; + const int edge_fill_index = std::ceil(max_longitudinal_length_to_fill_drivable_area / + mpt_param_.delta_arc_length / 2); const auto copied_ref_points = ref_points; for (size_t i = 0; + i < ref_points.size(); ++i) { const double base_normalized_avoidance_cost = + ref_points.at(i).normalized_avoidance_cost; for (int j = -edge_fill_index; j <= edge_fill_index; + ++j) { const int k = i + j; if (k < 0 || ref_points.size() - 1 <= k) { continue; + } + ref_points.at(i).normalized_avoidance_cost = + std::max(ref_points.at(i).normalized_avoidance_cost, + copied_ref_points.at(k).normalized_avoidance_cost); + } + } + */ - if (!points_reaching_prev_points_flag) { - if (motion_utils::calcSignedArcLength(points, 0, prev_ref_point.p) < 0) { - continue; + // update spread avoidance cost + for (int i = 0; i < static_cast(ref_points.size()); ++i) { + const double base_normalized_avoidance_cost = ref_points.at(i).normalized_avoidance_cost; + if (0 < base_normalized_avoidance_cost) { + const int edge_decrease_idx = std::floor( + ref_points.at(i).normalized_avoidance_cost / mpt_param_.avoidance_cost_decrease_rate); + for (int j = -edge_decrease_idx; j <= edge_decrease_idx; ++j) { + const int k = i + j; + if (0 <= k && k < static_cast(ref_points.size())) { + const double normalized_avoidance_cost = std::max( + base_normalized_avoidance_cost - + std::abs(j) * mpt_param_.avoidance_cost_decrease_rate, + ref_points.at(k).normalized_avoidance_cost); + ref_points.at(k).normalized_avoidance_cost = + std::clamp(normalized_avoidance_cost, 0.0, 1.0); + } + } } - points_reaching_prev_points_flag = true; } - ReferencePoint fixed_ref_point; - fixed_ref_point = prev_ref_point; - fixed_ref_point.fix_kinematic_state = prev_ref_point.optimized_kinematic_state; + // take over previous avoidance cost + const double max_dist_threshold = mpt_param_.delta_arc_length / 2.0; + if (prev_ref_points_ptr_ && !prev_ref_points_ptr_->empty()) { + for (int i = 0; i < static_cast(ref_points.size()); ++i) { + const size_t prev_idx = trajectory_utils::findEgoIndex( + *prev_ref_points_ptr_, tier4_autoware_utils::getPose(ref_points.at(i)), + ego_nearest_param_); + + const double dist_to_prev = tier4_autoware_utils::calcDistance2d( + ref_points.at(i), prev_ref_points_ptr_->at(prev_idx)); + if (max_dist_threshold < dist_to_prev) { + continue; + } - fixed_ref_points.push_back(fixed_ref_point); - if (mpt_param_.is_fixed_point_single) { - break; + ref_points.at(i).normalized_avoidance_cost = std::max( + prev_ref_points_ptr_->at(prev_idx).normalized_avoidance_cost, + ref_points.at(i).normalized_avoidance_cost); + } } } - - return fixed_ref_points; } -std::vector MPTOptimizer::getMPTFixedPoints( - const std::vector & ref_points) const +void MPTOptimizer::updateBounds( + std::vector & ref_points, + const std::vector & left_bound, + const std::vector & right_bound) const { - std::vector mpt_fixed_traj; - for (size_t i = 0; i < ref_points.size(); ++i) { - const auto & ref_point = ref_points.at(i); + time_keeper_ptr_->tic(__func__); + + const double soft_road_clearance = + mpt_param_.soft_clearance_from_road + vehicle_info_.vehicle_width_m / 2.0; + + // calculate distance to left/right bound on each reference point + for (auto & ref_point : ref_points) { + const double dist_to_left_bound = + calcLateralDistToBounds(ref_point.pose, left_bound, soft_road_clearance, true); + const double dist_to_right_bound = + calcLateralDistToBounds(ref_point.pose, right_bound, soft_road_clearance, false); + ref_point.bounds = Bounds{dist_to_right_bound, dist_to_left_bound}; + } - if (ref_point.fix_kinematic_state) { - const double lat_error = ref_point.fix_kinematic_state.get()(0); - const double yaw_error = ref_point.fix_kinematic_state.get()(1); + /* + // TODO(murooka) deal with filling data between obstacles + // fill between obstacles + constexpr double max_longitudinal_length_to_fill_drivable_area = 20; + const int edge_fill_index = std::ceil(max_longitudinal_length_to_fill_drivable_area / + mpt_param_.delta_arc_length / 2); for (int i = 0; i < ref_points.size(); ++i) { for (int j = + -edge_fill_index; j <= edge_fill_index; ++j) { const int k = i + j; if (k < 0 || ref_points.size() + - 1 <= k) { continue; + } - autoware_auto_planning_msgs::msg::TrajectoryPoint fixed_traj_point; - fixed_traj_point.pose = calcVehiclePose(ref_point, lat_error, yaw_error, 0.0); - mpt_fixed_traj.push_back(fixed_traj_point); + const auto normalized_avoidance_cost = calcNormalizedAvoidanceCost(ref_points.at(k)); + if (normalized_avoidance_cost) { + } } } + */ - return mpt_fixed_traj; + time_keeper_ptr_->toc(__func__, " "); + return; } -// predict equation: x = Bex u + Wex (u includes x_0) -// cost function: J = x' Qex x + u' Rex u -MPTOptimizer::MPTMatrix MPTOptimizer::generateMPTMatrix( - const std::vector & ref_points, DebugData & debug_data) const +void MPTOptimizer::updateVehicleBounds( + std::vector & ref_points, + const SplineInterpolationPoints2d & ref_points_spline) const { - if (ref_points.empty()) { - return MPTMatrix{}; - } + time_keeper_ptr_->tic(__func__); - stop_watch_.tic(__func__); + for (size_t p_idx = 0; p_idx < ref_points.size(); ++p_idx) { + const auto & ref_point = ref_points.at(p_idx); + // NOTE: This clear is required. + // It seems they sometimes already have previous values. + ref_points.at(p_idx).bounds_on_constraints.clear(); + ref_points.at(p_idx).beta.clear(); - // NOTE: center offset of vehicle is always 0 in this algorithm. - // vehicle_model_ptr_->updateCenterOffset(0.0); + for (const double lon_offset : vehicle_circle_longitudinal_offsets_) { + const auto collision_check_pose = + ref_points_spline.getSplineInterpolatedPose(p_idx, lon_offset); + const double collision_check_yaw = tf2::getYaw(collision_check_pose.orientation); - const size_t N_ref = ref_points.size(); - const size_t D_x = vehicle_model_ptr_->getDimX(); - const size_t D_u = vehicle_model_ptr_->getDimU(); - const size_t D_v = D_x + D_u * (N_ref - 1); + // calculate beta + const double beta = ref_point.getYaw() - collision_check_yaw; + ref_points.at(p_idx).beta.push_back(beta); - Eigen::MatrixXd Bex = Eigen::MatrixXd::Zero(D_x * N_ref, D_v); - Eigen::VectorXd Wex = Eigen::VectorXd::Zero(D_x * N_ref); + // calculate vehicle_bounds_pose + const double tmp_yaw = std::atan2( + collision_check_pose.position.y - ref_point.pose.position.y, + collision_check_pose.position.x - ref_point.pose.position.x); + const double offset_y = + -tier4_autoware_utils::calcDistance2d(ref_point, collision_check_pose) * + std::sin(tmp_yaw - collision_check_yaw); - Eigen::MatrixXd Ad(D_x, D_x); - Eigen::MatrixXd Bd(D_x, D_u); - Eigen::MatrixXd Wd(D_x, 1); + const auto vehicle_bounds_pose = + tier4_autoware_utils::calcOffsetPose(collision_check_pose, 0.0, offset_y, 0.0); - // predict kinematics for N_ref times - for (size_t i = 0; i < N_ref; ++i) { - if (i == 0) { - Bex.block(0, 0, D_x, D_x) = Eigen::MatrixXd::Identity(D_x, D_x); - continue; - } + // interpolate bounds + const auto bounds = [&]() { + const double collision_check_s = ref_points_spline.getAccumulatedLength(p_idx) + lon_offset; + const size_t collision_check_idx = ref_points_spline.getOffsetIndex(p_idx, lon_offset); - const int idx_x_i = i * D_x; - const int idx_x_i_prev = (i - 1) * D_x; - const int idx_u_i_prev = (i - 1) * D_u; + const size_t prev_idx = std::clamp( + collision_check_idx - 1, static_cast(0), + static_cast(ref_points_spline.getSize() - 2)); + const size_t next_idx = prev_idx + 1; - const double ds = [&]() { - if (N_ref == 1) { - return 0.0; - } - const size_t prev_idx = (i < N_ref - 1) ? i + 1 : i; - return ref_points.at(prev_idx).s - ref_points.at(prev_idx - 1).s; - }(); + const auto & prev_bounds = ref_points.at(prev_idx).bounds; + const auto & next_bounds = ref_points.at(next_idx).bounds; - // get discrete kinematics matrix A, B, W - const double ref_k = ref_points.at(std::max(0, static_cast(i) - 1)).k; - vehicle_model_ptr_->setCurvature(ref_k); - vehicle_model_ptr_->calculateStateEquationMatrix(Ad, Bd, Wd, ds); + const double prev_s = ref_points_spline.getAccumulatedLength(prev_idx); + const double next_s = ref_points_spline.getAccumulatedLength(next_idx); - Bex.block(idx_x_i, 0, D_x, D_x) = Ad * Bex.block(idx_x_i_prev, 0, D_x, D_x); - Bex.block(idx_x_i, D_x + idx_u_i_prev, D_x, D_u) = Bd; + const double ratio = std::clamp((collision_check_s - prev_s) / (next_s - prev_s), 0.0, 1.0); - for (size_t j = 0; j < i - 1; ++j) { - size_t idx_u_j = j * D_u; - Bex.block(idx_x_i, D_x + idx_u_j, D_x, D_u) = - Ad * Bex.block(idx_x_i_prev, D_x + idx_u_j, D_x, D_u); - } + auto bounds = Bounds::lerp(prev_bounds, next_bounds, ratio); + bounds.translate(offset_y); + return bounds; + }(); - Wex.segment(idx_x_i, D_x) = Ad * Wex.block(idx_x_i_prev, 0, D_x, 1) + Wd; + ref_points.at(p_idx).bounds_on_constraints.push_back(bounds); + ref_points.at(p_idx).pose_on_constraints.push_back(vehicle_bounds_pose); + } } - MPTMatrix m; - m.Bex = Bex; - m.Wex = Wex; - - debug_data.msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) - << " [ms]\n"; - return m; + time_keeper_ptr_->toc(__func__, " "); } -MPTOptimizer::ValueMatrix MPTOptimizer::generateValueMatrix( +// cost function: J = x' Q x + u' R u +MPTOptimizer::ValueMatrix MPTOptimizer::calcValueMatrix( const std::vector & ref_points, - const std::vector & path_points, - DebugData & debug_data) const + const std::vector & traj_points) const { - if (ref_points.empty()) { - return ValueMatrix{}; - } - - stop_watch_.tic(__func__); + time_keeper_ptr_->tic(__func__); - const size_t D_x = vehicle_model_ptr_->getDimX(); - const size_t D_u = vehicle_model_ptr_->getDimU(); + const size_t D_x = state_equation_generator_.getDimX(); + const size_t D_u = state_equation_generator_.getDimU(); const size_t N_ref = ref_points.size(); - const size_t D_v = D_x + (N_ref - 1) * D_u; - const bool is_containing_path_terminal_point = points_utils::isNearLastPathPoint( - ref_points.back(), path_points, traj_param_.max_dist_for_extending_end_point, - traj_param_.delta_yaw_threshold_for_closest_point); + const bool is_goal_contained = geometry_utils::isSamePoint(ref_points.back(), traj_points.back()); // update Q - Eigen::SparseMatrix Qex_sparse_mat(D_x * N_ref, D_x * N_ref); - std::vector> Qex_triplet_vec; + Eigen::SparseMatrix Q_sparse_mat(D_x * N_ref, D_x * N_ref); + std::vector> Q_triplet_vec; for (size_t i = 0; i < N_ref; ++i) { - // this is for plan_from_ego - // const bool near_kinematic_state_while_planning_from_ego = [&]() { - // const size_t min_idx = static_cast(std::max(0, static_cast(i) - 20)); - // const size_t max_idx = std::min(ref_points.size() - 1, i + 20); - // for (size_t j = min_idx; j <= max_idx; ++j) { - // if (ref_points.at(j).plan_from_ego && ref_points.at(j).fix_kinematic_state) { - // return true; - // } - // } - // return false; - // }(); - const auto adaptive_error_weight = [&]() -> std::array { - if (ref_points.at(i).near_objects) { - return { - mpt_param_.obstacle_avoid_lat_error_weight, mpt_param_.obstacle_avoid_yaw_error_weight}; - } else if (i == N_ref - 1 && is_containing_path_terminal_point) { - return { - mpt_param_.terminal_path_lat_error_weight, mpt_param_.terminal_path_yaw_error_weight}; - } else if (i == N_ref - 1) { + // for terminal point + if (i == N_ref - 1) { + if (is_goal_contained) { + return {mpt_param_.goal_lat_error_weight, mpt_param_.goal_yaw_error_weight}; + } return {mpt_param_.terminal_lat_error_weight, mpt_param_.terminal_yaw_error_weight}; } - // NOTE: may be better to add decreasing weights in a narrow and sharp curve - // else if (std::abs(ref_points[i].k) > 0.3) { - // return {0.0, 0.0}; - // } - + // for avoidance + if (0 < ref_points.at(i).normalized_avoidance_cost) { + const double lat_error_weight = interpolation::lerp( + mpt_param_.lat_error_weight, mpt_param_.avoidance_lat_error_weight, + ref_points.at(i).normalized_avoidance_cost); + const double yaw_error_weight = interpolation::lerp( + mpt_param_.yaw_error_weight, mpt_param_.avoidance_yaw_error_weight, + ref_points.at(i).normalized_avoidance_cost); + return {lat_error_weight, yaw_error_weight}; + } + // normal case return {mpt_param_.lat_error_weight, mpt_param_.yaw_error_weight}; }(); const double adaptive_lat_error_weight = adaptive_error_weight.at(0); const double adaptive_yaw_error_weight = adaptive_error_weight.at(1); - Qex_triplet_vec.push_back(Eigen::Triplet(i * D_x, i * D_x, adaptive_lat_error_weight)); - Qex_triplet_vec.push_back( + Q_triplet_vec.push_back(Eigen::Triplet(i * D_x, i * D_x, adaptive_lat_error_weight)); + Q_triplet_vec.push_back( Eigen::Triplet(i * D_x + 1, i * D_x + 1, adaptive_yaw_error_weight)); } - Qex_sparse_mat.setFromTriplets(Qex_triplet_vec.begin(), Qex_triplet_vec.end()); + Q_sparse_mat.setFromTriplets(Q_triplet_vec.begin(), Q_triplet_vec.end()); // update R - Eigen::SparseMatrix Rex_sparse_mat(D_v, D_v); - std::vector> Rex_triplet_vec; + Eigen::SparseMatrix R_sparse_mat(D_v, D_v); + std::vector> R_triplet_vec; for (size_t i = 0; i < N_ref - 1; ++i) { - const double adaptive_steer_weight = ref_points.at(i).near_objects - ? mpt_param_.obstacle_avoid_steer_input_weight - : mpt_param_.steer_input_weight; - Rex_triplet_vec.push_back( + const double adaptive_steer_weight = interpolation::lerp( + mpt_param_.steer_input_weight, mpt_param_.avoidance_steer_input_weight, + ref_points.at(i).normalized_avoidance_cost); + R_triplet_vec.push_back( Eigen::Triplet(D_x + D_u * i, D_x + D_u * i, adaptive_steer_weight)); } - addSteerWeightR(Rex_triplet_vec, ref_points); + addSteerWeightR(R_triplet_vec, ref_points); - Rex_sparse_mat.setFromTriplets(Rex_triplet_vec.begin(), Rex_triplet_vec.end()); + R_sparse_mat.setFromTriplets(R_triplet_vec.begin(), R_triplet_vec.end()); ValueMatrix m; - m.Qex = Qex_sparse_mat; - m.Rex = Rex_sparse_mat; + m.Q = Q_sparse_mat; + m.R = R_sparse_mat; - debug_data.msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) - << " [ms]\n"; + time_keeper_ptr_->toc(__func__, " "); return m; } -boost::optional MPTOptimizer::executeOptimization( - const std::unique_ptr & prev_trajs, const bool enable_avoidance, - const MPTMatrix & mpt_mat, const ValueMatrix & val_mat, - const std::vector & ref_points, DebugData & debug_data) -{ - if (ref_points.empty()) { - return Eigen::VectorXd{}; - } - - stop_watch_.tic(__func__); - - const size_t N_ref = ref_points.size(); - - // get matrix - ObjectiveMatrix obj_m = getObjectiveMatrix(mpt_mat, val_mat, ref_points, debug_data); - ConstraintMatrix const_m = getConstraintMatrix(enable_avoidance, mpt_mat, ref_points, debug_data); - - // manual warm start - Eigen::VectorXd u0 = Eigen::VectorXd::Zero(obj_m.gradient.size()); - - if (mpt_param_.enable_manual_warm_start) { - const size_t D_x = vehicle_model_ptr_->getDimX(); - - if (prev_trajs && prev_trajs->mpt_ref_points.size() > 1) { - geometry_msgs::msg::Pose ref_front_point; - ref_front_point.position = ref_points.front().p; - ref_front_point.orientation = - tier4_autoware_utils::createQuaternionFromYaw(ref_points.front().yaw); - - const size_t front_idx = findNearestIndexWithSoftYawConstraints( - points_utils::convertToPoints(prev_trajs->mpt_ref_points), ref_front_point, - traj_param_.ego_nearest_dist_threshold, traj_param_.ego_nearest_yaw_threshold); - - // set initial state - u0(0) = prev_trajs->mpt_ref_points.at(front_idx).optimized_kinematic_state(0); - u0(1) = prev_trajs->mpt_ref_points.at(front_idx).optimized_kinematic_state(1); - - // set steer angle - for (size_t i = 0; i + 1 < N_ref; ++i) { - const size_t prev_target_idx = - std::min(front_idx + i, prev_trajs->mpt_ref_points.size() - 1); - u0(D_x + i) = prev_trajs->mpt_ref_points.at(prev_target_idx).optimized_input; - } - } - } - - const Eigen::MatrixXd & H = obj_m.hessian; - const Eigen::MatrixXd & A = const_m.linear; - std::vector f; - std::vector upper_bound; - std::vector lower_bound; - - if (mpt_param_.enable_manual_warm_start) { - f = eigenVectorToStdVector(obj_m.gradient + H * u0); - Eigen::VectorXd A_times_u0 = A * u0; - upper_bound = eigenVectorToStdVector(const_m.upper_bound - A_times_u0); - lower_bound = eigenVectorToStdVector(const_m.lower_bound - A_times_u0); - } else { - f = eigenVectorToStdVector(obj_m.gradient); - upper_bound = eigenVectorToStdVector(const_m.upper_bound); - lower_bound = eigenVectorToStdVector(const_m.lower_bound); - } - - // initialize or update solver with warm start - stop_watch_.tic("initOsqp"); - autoware::common::osqp::CSC_Matrix P_csc = autoware::common::osqp::calCSCMatrixTrapezoidal(H); - autoware::common::osqp::CSC_Matrix A_csc = autoware::common::osqp::calCSCMatrix(A); - if (mpt_param_.enable_warm_start && prev_mat_n == H.rows() && prev_mat_m == A.rows()) { - RCLCPP_INFO_EXPRESSION( - rclcpp::get_logger("mpt_optimizer"), is_showing_debug_info_, "warm start"); - - osqp_solver_ptr_->updateCscP(P_csc); - osqp_solver_ptr_->updateQ(f); - osqp_solver_ptr_->updateCscA(A_csc); - osqp_solver_ptr_->updateL(lower_bound); - osqp_solver_ptr_->updateU(upper_bound); - } else { - RCLCPP_INFO_EXPRESSION( - rclcpp::get_logger("mpt_optimizer"), is_showing_debug_info_, "no warm start"); - - osqp_solver_ptr_ = std::make_unique( - // obj_m.hessian, const_m.linear, obj_m.gradient, const_m.lower_bound, const_m.upper_bound, - P_csc, A_csc, f, lower_bound, upper_bound, osqp_epsilon_); - } - prev_mat_n = H.rows(); - prev_mat_m = A.rows(); - - debug_data.msg_stream << " " - << "initOsqp" - << ":= " << stop_watch_.toc("initOsqp") << " [ms]\n"; - - // solve - stop_watch_.tic("solveOsqp"); - const auto result = osqp_solver_ptr_->optimize(); - debug_data.msg_stream << " " - << "solveOsqp" - << ":= " << stop_watch_.toc("solveOsqp") << " [ms]\n"; - - // check solution status - const int solution_status = std::get<3>(result); - if (solution_status != 1) { - utils::logOSQPSolutionStatus(solution_status, "MPT: "); - return boost::none; - } - - // print iteration - const int iteration_status = std::get<4>(result); - RCLCPP_INFO_EXPRESSION( - rclcpp::get_logger("mpt_optimizer"), is_showing_debug_info_, "iteration: %d", iteration_status); - - // get result - std::vector result_vec = std::get<0>(result); - - const size_t DIM_U = vehicle_model_ptr_->getDimU(); - const size_t DIM_X = vehicle_model_ptr_->getDimX(); - const Eigen::VectorXd optimized_control_variables = - Eigen::Map(&result_vec[0], DIM_X + (N_ref - 1) * DIM_U); - - debug_data.msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) - << " [ms]\n"; - - const Eigen::VectorXd optimized_control_variables_with_offset = - mpt_param_.enable_manual_warm_start - ? optimized_control_variables + u0.segment(0, DIM_X + (N_ref - 1) * DIM_U) - : optimized_control_variables; - - return optimized_control_variables_with_offset; -} - -MPTOptimizer::ObjectiveMatrix MPTOptimizer::getObjectiveMatrix( - const MPTMatrix & mpt_mat, const ValueMatrix & val_mat, - [[maybe_unused]] const std::vector & ref_points, DebugData & debug_data) const +MPTOptimizer::ObjectiveMatrix MPTOptimizer::calcObjectiveMatrix( + const StateEquationGenerator::Matrix & mpt_mat, const ValueMatrix & val_mat, + const std::vector & ref_points) const { - stop_watch_.tic(__func__); + time_keeper_ptr_->tic(__func__); - const size_t D_x = vehicle_model_ptr_->getDimX(); - const size_t D_u = vehicle_model_ptr_->getDimU(); const size_t N_ref = ref_points.size(); - + const size_t D_x = state_equation_generator_.getDimX(); + const size_t D_u = state_equation_generator_.getDimU(); const size_t D_xn = D_x * N_ref; const size_t D_v = D_x + (N_ref - 1) * D_u; + const size_t N_slack = getNumberOfSlackVariables(); // generate T matrix and vector to shift optimization center - // define Z as time-series vector of shifted deviation error - // Z = sparse_T_mat * (Bex * U + Wex) + T_vec + // NOTE: Z is defined as time-series vector of shifted deviation + // error where Z = sparse_T_mat * (B * U + W) + T_vec Eigen::SparseMatrix sparse_T_mat(D_xn, D_xn); Eigen::VectorXd T_vec = Eigen::VectorXd::Zero(D_xn); std::vector> triplet_T_vec; @@ -907,212 +973,158 @@ MPTOptimizer::ObjectiveMatrix MPTOptimizer::getObjectiveMatrix( } sparse_T_mat.setFromTriplets(triplet_T_vec.begin(), triplet_T_vec.end()); - const Eigen::MatrixXd B = sparse_T_mat * mpt_mat.Bex; - const Eigen::MatrixXd QB = val_mat.Qex * B; - const Eigen::MatrixXd R = val_mat.Rex; + const Eigen::MatrixXd B = sparse_T_mat * mpt_mat.B; + const Eigen::MatrixXd QB = val_mat.Q * B; + const Eigen::MatrixXd R = val_mat.R; - // min J(v) = min (v'Hv + v'f) + // calculate H, and extend it for slack variables + // NOTE: min J(v) = min (v'Hv + v'g) Eigen::MatrixXd H = Eigen::MatrixXd::Zero(D_v, D_v); H.triangularView() = B.transpose() * QB + R; H.triangularView() = H.transpose(); - // Eigen::VectorXd f = ((sparse_T_mat * mpt_mat.Wex + T_vec).transpose() * QB).transpose(); - Eigen::VectorXd f = (sparse_T_mat * mpt_mat.Wex + T_vec).transpose() * QB; - - const size_t N_avoid = mpt_param_.vehicle_circle_longitudinal_offsets.size(); - const size_t N_first_slack = [&]() -> size_t { - if (mpt_param_.soft_constraint) { - if (mpt_param_.l_inf_norm) { - return 1; - } - return N_avoid; - } - return 0; - }(); - const size_t N_second_slack = [&]() -> size_t { - if (mpt_param_.two_step_soft_constraint) { - return N_first_slack; - } - return 0; - }(); - - // number of slack variables for one step - const size_t N_slack = N_first_slack + N_second_slack; + Eigen::MatrixXd extended_H = Eigen::MatrixXd::Zero(D_v + N_ref * N_slack, D_v + N_ref * N_slack); + extended_H.block(0, 0, D_v, D_v) = H; - // extend H for slack variables - Eigen::MatrixXd full_H = Eigen::MatrixXd::Zero(D_v + N_ref * N_slack, D_v + N_ref * N_slack); - full_H.block(0, 0, D_v, D_v) = H; - - // extend f for slack variables - Eigen::VectorXd full_f(D_v + N_ref * N_slack); + // calculate g, and extend it for slack variables + Eigen::VectorXd g = (sparse_T_mat * mpt_mat.W + T_vec).transpose() * QB; + /* + Eigen::VectorXd extended_g(D_v + N_ref * N_slack); - full_f.segment(0, D_v) = f; - if (N_first_slack > 0) { - full_f.segment(D_v, N_ref * N_first_slack) = - mpt_param_.soft_avoidance_weight * Eigen::VectorXd::Ones(N_ref * N_first_slack); - } - if (N_second_slack > 0) { - full_f.segment(D_v + N_ref * N_first_slack, N_ref * N_second_slack) = - mpt_param_.soft_second_avoidance_weight * Eigen::VectorXd::Ones(N_ref * N_second_slack); + extended_g.segment(0, D_v) = g; + if (N_slack > 0) { + extended_g.segment(D_v, N_ref * N_slack) = + mpt_param_.soft_collision_free_weight * Eigen::VectorXd::Ones(N_ref * N_slack); } + */ + Eigen::VectorXd extended_g(D_v + N_ref * N_slack); + extended_g << g, mpt_param_.soft_collision_free_weight * Eigen::VectorXd::Ones(N_ref * N_slack); ObjectiveMatrix obj_matrix; - obj_matrix.hessian = full_H; - obj_matrix.gradient = full_f; - - debug_data.msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) - << " [ms]\n"; + obj_matrix.hessian = extended_H; + obj_matrix.gradient = extended_g; + time_keeper_ptr_->toc(__func__, " "); return obj_matrix; } -// Set constraint: lb <= Ax <= ub +// Constraint: lb <= A u <= ub // decision variable -// x := [u0, ..., uN-1 | z00, ..., z0N-1 | z10, ..., z1N-1 | z20, ..., z2N-1] -// \in \mathbb{R}^{N * (N_vehicle_circle + 1)} -MPTOptimizer::ConstraintMatrix MPTOptimizer::getConstraintMatrix( - [[maybe_unused]] const bool enable_avoidance, const MPTMatrix & mpt_mat, - const std::vector & ref_points, [[maybe_unused]] DebugData & debug_data) const +// u := [initial state, steer angles, soft variables] +MPTOptimizer::ConstraintMatrix MPTOptimizer::calcConstraintMatrix( + const StateEquationGenerator::Matrix & mpt_mat, + const std::vector & ref_points) const { - stop_watch_.tic(__func__); + time_keeper_ptr_->tic(__func__); - // NOTE: currently, add additional length to soft bounds approximately - // for soft second and hard bounds - const size_t D_x = vehicle_model_ptr_->getDimX(); - const size_t D_u = vehicle_model_ptr_->getDimU(); + const size_t D_x = state_equation_generator_.getDimX(); + const size_t D_u = state_equation_generator_.getDimU(); const size_t N_ref = ref_points.size(); - const size_t N_u = (N_ref - 1) * D_u; const size_t D_v = D_x + N_u; + const size_t N_collision_check = vehicle_circle_longitudinal_offsets_.size(); - const size_t N_avoid = mpt_param_.vehicle_circle_longitudinal_offsets.size(); - - // number of slack variables for one step - const size_t N_first_slack = [&]() -> size_t { - if (mpt_param_.soft_constraint) { - if (mpt_param_.l_inf_norm) { - return 1; - } - return N_avoid; - } - return 0; - }(); - const size_t N_second_slack = [&]() -> size_t { - if (mpt_param_.soft_constraint && mpt_param_.two_step_soft_constraint) { - return N_first_slack; - } - return 0; - }(); - - // number of all slack variables is N_ref * N_slack - const size_t N_slack = N_first_slack + N_second_slack; - const size_t N_soft = mpt_param_.two_step_soft_constraint ? 2 : 1; - - const size_t A_cols = [&] { - if (mpt_param_.soft_constraint) { - return D_v + N_ref * N_slack; // initial_state + steer + soft - } - return D_v; // initial state + steer - }(); + // NOTE: The number of one-step slack variables. + // The number of all slack variables will be N_ref * N_slack. + const size_t N_slack = getNumberOfSlackVariables(); // calculate indices of fixed points std::vector fixed_points_indices; for (size_t i = 0; i < N_ref; ++i) { - if (ref_points.at(i).fix_kinematic_state) { + if (ref_points.at(i).fixed_kinematic_state) { fixed_points_indices.push_back(i); } } - // calculate rows of A + // calculate rows and cols of A size_t A_rows = 0; if (mpt_param_.soft_constraint) { - // 3 means slack variable constraints to be between lower and upper bounds, and positive. - A_rows += 3 * N_ref * N_avoid * N_soft; + // NOTE: 3 means expecting slack variable constraints to be larger than lower bound, + // smaller than upper bound, and positive. + A_rows += 3 * N_ref * N_collision_check; } if (mpt_param_.hard_constraint) { - A_rows += N_ref * N_avoid; + A_rows += N_ref * N_collision_check; } A_rows += fixed_points_indices.size() * D_x; if (mpt_param_.steer_limit_constraint) { A_rows += N_u; } + const size_t A_cols = [&] { + if (mpt_param_.soft_constraint) { + return D_v + N_ref * N_slack; // initial state + steer angles + soft variables + } + return D_v; // initial state + steer angles + }(); + Eigen::MatrixXd A = Eigen::MatrixXd::Zero(A_rows, A_cols); Eigen::VectorXd lb = Eigen::VectorXd::Constant(A_rows, -autoware::common::osqp::INF); Eigen::VectorXd ub = Eigen::VectorXd::Constant(A_rows, autoware::common::osqp::INF); size_t A_rows_end = 0; // CX = C(Bv + w) + C \in R^{N_ref, N_ref * D_x} - for (size_t l_idx = 0; l_idx < N_avoid; ++l_idx) { - // create C := [1 | l | O] + for (size_t l_idx = 0; l_idx < N_collision_check; ++l_idx) { + // create C := [cos(beta) | l cos(beta)] Eigen::SparseMatrix C_sparse_mat(N_ref, N_ref * D_x); std::vector> C_triplet_vec; Eigen::VectorXd C_vec = Eigen::VectorXd::Zero(N_ref); // calculate C mat and vec for (size_t i = 0; i < N_ref; ++i) { - const double beta = ref_points.at(i).beta.at(l_idx).get(); - const double avoid_offset = mpt_param_.vehicle_circle_longitudinal_offsets.at(l_idx); + const double beta = *ref_points.at(i).beta.at(l_idx); + const double lon_offset = vehicle_circle_longitudinal_offsets_.at(l_idx); C_triplet_vec.push_back(Eigen::Triplet(i, i * D_x, 1.0 * std::cos(beta))); - C_triplet_vec.push_back( - Eigen::Triplet(i, i * D_x + 1, avoid_offset * std::cos(beta))); - C_vec(i) = avoid_offset * std::sin(beta); + C_triplet_vec.push_back(Eigen::Triplet(i, i * D_x + 1, lon_offset * std::cos(beta))); + C_vec(i) = lon_offset * std::sin(beta); } C_sparse_mat.setFromTriplets(C_triplet_vec.begin(), C_triplet_vec.end()); // calculate CB, and CW - const Eigen::MatrixXd CB = C_sparse_mat * mpt_mat.Bex; - const Eigen::VectorXd CW = C_sparse_mat * mpt_mat.Wex + C_vec; + const Eigen::MatrixXd CB = C_sparse_mat * mpt_mat.B; + const Eigen::VectorXd CW = C_sparse_mat * mpt_mat.W + C_vec; // calculate bounds - const auto & [part_ub, part_lb] = extractBounds(ref_points, l_idx, vehicle_param_); + const double bounds_offset = + vehicle_info_.vehicle_width_m / 2.0 - vehicle_circle_radiuses_.at(l_idx); + const auto & [part_ub, part_lb] = extractBounds(ref_points, l_idx, bounds_offset); // soft constraints if (mpt_param_.soft_constraint) { size_t A_offset_cols = D_v; - for (size_t s_idx = 0; s_idx < N_soft; ++s_idx) { - const size_t A_blk_rows = 3 * N_ref; - - // A := [C * Bex | O | ... | O | I | O | ... - // -C * Bex | O | ... | O | I | O | ... - // O | O | ... | O | I | O | ... ] - Eigen::MatrixXd A_blk = Eigen::MatrixXd::Zero(A_blk_rows, A_cols); - A_blk.block(0, 0, N_ref, D_v) = CB; - A_blk.block(N_ref, 0, N_ref, D_v) = -CB; - - size_t local_A_offset_cols = A_offset_cols; - if (!mpt_param_.l_inf_norm) { - local_A_offset_cols += N_ref * l_idx; - } - A_blk.block(0, local_A_offset_cols, N_ref, N_ref) = Eigen::MatrixXd::Identity(N_ref, N_ref); - A_blk.block(N_ref, local_A_offset_cols, N_ref, N_ref) = - Eigen::MatrixXd::Identity(N_ref, N_ref); - A_blk.block(2 * N_ref, local_A_offset_cols, N_ref, N_ref) = - Eigen::MatrixXd::Identity(N_ref, N_ref); - - // lb := [lower_bound - CW - // CW - upper_bound - // O ] - Eigen::VectorXd lb_blk = Eigen::VectorXd::Zero(A_blk_rows); - lb_blk.segment(0, N_ref) = -CW + part_lb; - lb_blk.segment(N_ref, N_ref) = CW - part_ub; - - if (s_idx == 1) { - // add additional clearance - const double diff_clearance = - mpt_param_.soft_second_clearance_from_road - mpt_param_.soft_clearance_from_road; - lb_blk.segment(0, N_ref) -= Eigen::MatrixXd::Constant(N_ref, 1, diff_clearance); - lb_blk.segment(N_ref, N_ref) -= Eigen::MatrixXd::Constant(N_ref, 1, diff_clearance); - } - - A_offset_cols += N_ref * N_first_slack; + const size_t A_blk_rows = 3 * N_ref; - A.block(A_rows_end, 0, A_blk_rows, A_cols) = A_blk; - lb.segment(A_rows_end, A_blk_rows) = lb_blk; + // A := [C * B | O | ... | O | I | O | ... + // -C * B | O | ... | O | I | O | ... + // O | O | ... | O | I | O | ... ] + Eigen::MatrixXd A_blk = Eigen::MatrixXd::Zero(A_blk_rows, A_cols); + A_blk.block(0, 0, N_ref, D_v) = CB; + A_blk.block(N_ref, 0, N_ref, D_v) = -CB; - A_rows_end += A_blk_rows; + size_t local_A_offset_cols = A_offset_cols; + if (!mpt_param_.l_inf_norm) { + local_A_offset_cols += N_ref * l_idx; } + A_blk.block(0, local_A_offset_cols, N_ref, N_ref) = Eigen::MatrixXd::Identity(N_ref, N_ref); + A_blk.block(N_ref, local_A_offset_cols, N_ref, N_ref) = + Eigen::MatrixXd::Identity(N_ref, N_ref); + A_blk.block(2 * N_ref, local_A_offset_cols, N_ref, N_ref) = + Eigen::MatrixXd::Identity(N_ref, N_ref); + + // lb := [lower_bound - CW + // CW - upper_bound + // O ] + Eigen::VectorXd lb_blk = Eigen::VectorXd::Zero(A_blk_rows); + lb_blk.segment(0, N_ref) = -CW + part_lb; + lb_blk.segment(N_ref, N_ref) = CW - part_ub; + + A_offset_cols += N_ref * N_slack; + + A.block(A_rows_end, 0, A_blk_rows, A_cols) = A_blk; + lb.segment(A_rows_end, A_blk_rows) = lb_blk; + + A_rows_end += A_blk_rows; } // hard constraints @@ -1131,25 +1143,33 @@ MPTOptimizer::ConstraintMatrix MPTOptimizer::getConstraintMatrix( } // fixed points constraint - // CX = C(B v + w) where C extracts fixed points - if (fixed_points_indices.size() > 0) { - for (const size_t i : fixed_points_indices) { - A.block(A_rows_end, 0, D_x, D_v) = mpt_mat.Bex.block(i * D_x, 0, D_x, D_v); + // X = B v + w where point is fixed + for (const size_t i : fixed_points_indices) { + A.block(A_rows_end, 0, D_x, D_v) = mpt_mat.B.block(i * D_x, 0, D_x, D_v); - lb.segment(A_rows_end, D_x) = - ref_points[i].fix_kinematic_state.get() - mpt_mat.Wex.segment(i * D_x, D_x); - ub.segment(A_rows_end, D_x) = - ref_points[i].fix_kinematic_state.get() - mpt_mat.Wex.segment(i * D_x, D_x); + lb.segment(A_rows_end, D_x) = + ref_points[i].fixed_kinematic_state->toEigenVector() - mpt_mat.W.segment(i * D_x, D_x); + ub.segment(A_rows_end, D_x) = + ref_points[i].fixed_kinematic_state->toEigenVector() - mpt_mat.W.segment(i * D_x, D_x); - A_rows_end += D_x; - } + A_rows_end += D_x; } - // steer max limit + // steer limit if (mpt_param_.steer_limit_constraint) { A.block(A_rows_end, D_x, N_u, N_u) = Eigen::MatrixXd::Identity(N_u, N_u); - lb.segment(A_rows_end, N_u) = Eigen::MatrixXd::Constant(N_u, 1, -mpt_param_.max_steer_rad); - ub.segment(A_rows_end, N_u) = Eigen::MatrixXd::Constant(N_u, 1, mpt_param_.max_steer_rad); + + // TODO(murooka) use curvature by stabling optimization + // Currently, when using curvature, the optimization result is weird with sample_map. + // lb.segment(A_rows_end, N_u) = Eigen::MatrixXd::Constant(N_u, 1, -mpt_param_.max_steer_rad); + // ub.segment(A_rows_end, N_u) = Eigen::MatrixXd::Constant(N_u, 1, mpt_param_.max_steer_rad); + + for (size_t i = 0; i < N_u; ++i) { + const double ref_steer_angle = + std::atan2(vehicle_info_.wheel_base_m * ref_points.at(i).curvature, 1.0); + lb(A_rows_end + i) = ref_steer_angle - mpt_param_.max_steer_rad; + ub(A_rows_end + i) = ref_steer_angle + mpt_param_.max_steer_rad; + } A_rows_end += N_u; } @@ -1159,384 +1179,312 @@ MPTOptimizer::ConstraintMatrix MPTOptimizer::getConstraintMatrix( constraint_matrix.lower_bound = lb; constraint_matrix.upper_bound = ub; - debug_data.msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) - << " [ms]\n"; + time_keeper_ptr_->toc(__func__, " "); return constraint_matrix; } -std::vector MPTOptimizer::getMPTPoints( - std::vector & fixed_ref_points, - std::vector & non_fixed_ref_points, const Eigen::VectorXd & Uex, - const MPTMatrix & mpt_mat, DebugData & debug_data) +void MPTOptimizer::addSteerWeightR( + std::vector> & R_triplet_vec, + const std::vector & ref_points) const { - const size_t D_x = vehicle_model_ptr_->getDimX(); - const size_t D_u = vehicle_model_ptr_->getDimU(); - const size_t N_ref = static_cast(Uex.rows() - D_x) + 1; - - stop_watch_.tic(__func__); - - std::vector lat_error_vec; - std::vector yaw_error_vec; - for (size_t i = 0; i < fixed_ref_points.size(); ++i) { - const auto & ref_point = fixed_ref_points.at(i); + const size_t D_x = state_equation_generator_.getDimX(); + const size_t D_u = state_equation_generator_.getDimU(); + const size_t N_ref = ref_points.size(); + const size_t D_v = D_x + (N_ref - 1) * D_u; - lat_error_vec.push_back(ref_point.fix_kinematic_state.get()(0)); - yaw_error_vec.push_back(ref_point.fix_kinematic_state.get()(1)); + // add steering rate : weight for (u(i) - u(i-1))^2 + for (size_t i = D_x; i < D_v - 1; ++i) { + R_triplet_vec.push_back(Eigen::Triplet(i, i, mpt_param_.steer_rate_weight)); + R_triplet_vec.push_back(Eigen::Triplet(i + 1, i, -mpt_param_.steer_rate_weight)); + R_triplet_vec.push_back(Eigen::Triplet(i, i + 1, -mpt_param_.steer_rate_weight)); + R_triplet_vec.push_back(Eigen::Triplet(i + 1, i + 1, mpt_param_.steer_rate_weight)); } +} - const size_t N_kinematic_state = vehicle_model_ptr_->getDimX(); - const Eigen::VectorXd Xex = mpt_mat.Bex * Uex + mpt_mat.Wex; - - for (size_t i = 0; i < non_fixed_ref_points.size(); ++i) { - lat_error_vec.push_back(Xex(i * N_kinematic_state)); - yaw_error_vec.push_back(Xex(i * N_kinematic_state + 1)); - } +std::optional MPTOptimizer::calcOptimizedSteerAngles( + const std::vector & ref_points, const ObjectiveMatrix & obj_mat, + const ConstraintMatrix & const_mat) +{ + time_keeper_ptr_->tic(__func__); - // calculate trajectory from optimization result - std::vector traj_points; - debug_data.vehicle_circles_pose.resize(lat_error_vec.size()); - for (size_t i = 0; i < lat_error_vec.size(); ++i) { - auto & ref_point = (i < fixed_ref_points.size()) - ? fixed_ref_points.at(i) - : non_fixed_ref_points.at(i - fixed_ref_points.size()); - const double lat_error = lat_error_vec.at(i); - const double yaw_error = yaw_error_vec.at(i); - - geometry_msgs::msg::Pose ref_pose; - ref_pose.position = ref_point.p; - ref_pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(ref_point.yaw); - debug_data.mpt_ref_poses.push_back(ref_pose); - debug_data.lateral_errors.push_back(lat_error); - debug_data.yaw_errors.push_back(yaw_error); - - ref_point.optimized_kinematic_state << lat_error_vec.at(i), yaw_error_vec.at(i); - if (i >= fixed_ref_points.size()) { - const size_t j = i - fixed_ref_points.size(); - if (j == N_ref - 1) { - ref_point.optimized_input = 0.0; - } else { - ref_point.optimized_input = Uex(D_x + j * D_u); + const size_t D_x = state_equation_generator_.getDimX(); + const size_t D_u = state_equation_generator_.getDimU(); + const size_t N_ref = ref_points.size(); + const size_t D_v = D_x + (N_ref - 1) * D_u; + const size_t N_slack = getNumberOfSlackVariables(); + const size_t D_un = D_v + N_ref * N_slack; + + // for manual warm start, calculate initial solution + const auto u0 = [&]() -> std::optional { + if (mpt_param_.enable_manual_warm_start) { + if (prev_ref_points_ptr_ && 1 < prev_ref_points_ptr_->size()) { + return calcInitialSolutionForManualWarmStart(ref_points, *prev_ref_points_ptr_); } } + return std::nullopt; + }(); - autoware_auto_planning_msgs::msg::TrajectoryPoint traj_point; - traj_point.pose = calcVehiclePose(ref_point, lat_error, yaw_error, 0.0); - - traj_points.push_back(traj_point); - - { // for debug visualization - const double base_x = ref_point.p.x - std::sin(ref_point.yaw) * lat_error; - const double base_y = ref_point.p.y + std::cos(ref_point.yaw) * lat_error; - - // NOTE: coordinate of vehicle_circle_longitudinal_offsets is back wheel center - for (const double d : mpt_param_.vehicle_circle_longitudinal_offsets) { - geometry_msgs::msg::Pose vehicle_circle_pose; + // for manual start, update objective and constraint matrix + const auto [updated_obj_mat, updated_const_mat] = + updateMatrixForManualWarmStart(obj_mat, const_mat, u0); + + // calculate matrices for qp + const Eigen::MatrixXd & H = updated_obj_mat.hessian; + const Eigen::MatrixXd & A = updated_const_mat.linear; + const auto f = toStdVector(updated_obj_mat.gradient); + const auto upper_bound = toStdVector(updated_const_mat.upper_bound); + const auto lower_bound = toStdVector(updated_const_mat.lower_bound); + + // initialize or update solver according to warm start + time_keeper_ptr_->tic("initOsqp"); + + const autoware::common::osqp::CSC_Matrix P_csc = + autoware::common::osqp::calCSCMatrixTrapezoidal(H); + const autoware::common::osqp::CSC_Matrix A_csc = autoware::common::osqp::calCSCMatrix(A); + if (mpt_param_.enable_warm_start && prev_mat_n_ == H.rows() && prev_mat_m_ == A.rows()) { + RCLCPP_INFO_EXPRESSION(logger_, enable_debug_info_, "warm start"); + osqp_solver_ptr_->updateCscP(P_csc); + osqp_solver_ptr_->updateQ(f); + osqp_solver_ptr_->updateCscA(A_csc); + osqp_solver_ptr_->updateL(lower_bound); + osqp_solver_ptr_->updateU(upper_bound); + } else { + RCLCPP_INFO_EXPRESSION(logger_, enable_debug_info_, "no warm start"); + osqp_solver_ptr_ = std::make_unique( + P_csc, A_csc, f, lower_bound, upper_bound, osqp_epsilon_); + } + prev_mat_n_ = H.rows(); + prev_mat_m_ = A.rows(); - vehicle_circle_pose.position.x = base_x + d * std::cos(ref_point.yaw + yaw_error); - vehicle_circle_pose.position.y = base_y + d * std::sin(ref_point.yaw + yaw_error); + time_keeper_ptr_->toc("initOsqp", " "); - vehicle_circle_pose.orientation = - tier4_autoware_utils::createQuaternionFromYaw(ref_point.yaw + ref_point.alpha); + // solve qp + time_keeper_ptr_->tic("solveOsqp"); + const auto result = osqp_solver_ptr_->optimize(); + time_keeper_ptr_->toc("solveOsqp", " "); - debug_data.vehicle_circles_pose.at(i).push_back(vehicle_circle_pose); - } - } + // check solution status + const int solution_status = std::get<3>(result); + if (solution_status != 1) { + osqp_solver_ptr_->logUnsolvedStatus("[MPT]"); + return std::nullopt; } - // NOTE: If generated trajectory's orientation is not so smooth or kinematically infeasible, - // recalculate orientation here for (size_t i = 0; i < lat_error_vec.size(); ++i) { - // auto & ref_point = (i < fixed_ref_points.size()) - // ? fixed_ref_points.at(i) - // : non_fixed_ref_points.at(i - fixed_ref_points.size()); - // - // if (i > 0 && traj_points.size() > 1) { - // traj_points.at(i).pose.orientation = geometry_utils::getQuaternionFromPoints( - // traj_points.at(i).pose.position, traj_points.at(i - 1).pose.position); - // } else if (traj_points.size() > 1) { - // traj_points.at(i).pose.orientation = geometry_utils::getQuaternionFromPoints( - // traj_points.at(i + 1).pose.position, traj_points.at(i).pose.position); - // } else { - // traj_points.at(i).pose.orientation = - // tier4_autoware_utils::createQuaternionFromYaw(ref_point.yaw); - // } - // } - - debug_data.msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) - << " [ms]\n"; + // print iteration + const int iteration_status = std::get<4>(result); + RCLCPP_INFO_EXPRESSION(logger_, enable_debug_info_, "iteration: %d", iteration_status); - return traj_points; -} + // get optimization result + auto optimization_result = + std::get<0>(result); // NOTE: const cannot be added due to the next operation. + const Eigen::VectorXd optimized_steer_angles = + Eigen::Map(&optimization_result[0], D_un); -void MPTOptimizer::calcOrientation(std::vector & ref_points) const -{ - const auto yaw_angles = splineYawFromReferencePoints(ref_points); - for (size_t i = 0; i < ref_points.size(); ++i) { - if (ref_points.at(i).fix_kinematic_state) { - continue; - } + time_keeper_ptr_->toc(__func__, " "); - ref_points.at(i).yaw = yaw_angles.at(i); + if (u0) { // manual warm start + return static_cast(optimized_steer_angles + *u0); } + return optimized_steer_angles; } -void MPTOptimizer::calcCurvature(std::vector & ref_points) const +Eigen::VectorXd MPTOptimizer::calcInitialSolutionForManualWarmStart( + const std::vector & ref_points, + const std::vector & prev_ref_points) const { - const size_t num_points = static_cast(ref_points.size()); - - // calculate curvature by circle fitting from three points - size_t max_smoothing_num = static_cast(std::floor(0.5 * (num_points - 1))); - size_t L = - std::min(static_cast(mpt_param_.num_curvature_sampling_points), max_smoothing_num); - auto curvatures = points_utils::calcCurvature( - ref_points, static_cast(mpt_param_.num_curvature_sampling_points)); - for (size_t i = L; i < num_points - L; ++i) { - if (!ref_points.at(i).fix_kinematic_state) { - ref_points.at(i).k = curvatures.at(i); - } + const size_t D_x = state_equation_generator_.getDimX(); + const size_t D_u = state_equation_generator_.getDimU(); + const size_t N_ref = ref_points.size(); + const size_t N_u = (N_ref - 1) * D_u; + const size_t D_v = D_x + N_u; + const size_t N_slack = getNumberOfSlackVariables(); + const size_t D_un = D_v + N_ref * N_slack; + + Eigen::VectorXd u0 = Eigen::VectorXd::Zero(D_un); + + const size_t nearest_idx = motion_utils::findFirstNearestIndexWithSoftConstraints( + prev_ref_points, ref_points.front().pose, ego_nearest_param_.dist_threshold, + ego_nearest_param_.yaw_threshold); + + // set previous lateral and yaw deviation + u0(0) = prev_ref_points.at(nearest_idx).optimized_kinematic_state.lat; + u0(1) = prev_ref_points.at(nearest_idx).optimized_kinematic_state.yaw; + + // set previous steer angles + for (size_t i = 0; i < N_u; ++i) { + const size_t prev_target_idx = std::min(nearest_idx + i, prev_ref_points.size() - 1); + u0(D_x + i) = prev_ref_points.at(prev_target_idx).optimized_input; } - // first and last curvature is copied from next value - for (size_t i = 0; i < std::min(L, num_points); ++i) { - if (!ref_points.at(i).fix_kinematic_state) { - ref_points.at(i).k = ref_points.at(std::min(L, num_points - 1)).k; - } - if (!ref_points.at(num_points - i - 1).fix_kinematic_state) { - ref_points.at(num_points - i - 1).k = - ref_points.at(std::max(static_cast(num_points) - static_cast(L) - 1, 0)).k; + + // set previous slack variables + for (size_t i = 0; i < N_ref; ++i) { + const auto & slack_variables = ref_points.at(i).slack_variables; + if (slack_variables) { + for (size_t j = 0; j < slack_variables->size(); ++j) { + u0(D_v + i * N_slack + j) = slack_variables->at(j); + } } } + + return u0; } -void MPTOptimizer::calcArcLength(std::vector & ref_points) const +std::pair +MPTOptimizer::updateMatrixForManualWarmStart( + const ObjectiveMatrix & obj_mat, const ConstraintMatrix & const_mat, + const std::optional & u0) const { - for (size_t i = 0; i < ref_points.size(); i++) { - if (i > 0) { - geometry_msgs::msg::Point a, b; - a = ref_points.at(i).p; - b = ref_points.at(i - 1).p; - ref_points.at(i).s = ref_points.at(i - 1).s + tier4_autoware_utils::calcDistance2d(a, b); - } + if (!u0) { + // not manual warm start + return {obj_mat, const_mat}; } -} -void MPTOptimizer::calcExtraPoints( - std::vector & ref_points, const std::unique_ptr & prev_trajs) const -{ - for (size_t i = 0; i < ref_points.size(); ++i) { - // alpha - const double front_wheel_s = - ref_points.at(i).s + - vehicle_param_.wheelbase; // TODO(murooka) use offset instead of wheelbase? - const int front_wheel_nearest_idx = points_utils::getNearestIdx(ref_points, front_wheel_s, i); - const auto front_wheel_pos = ref_points.at(front_wheel_nearest_idx).p; + const Eigen::MatrixXd & H = obj_mat.hessian; + const Eigen::MatrixXd & A = const_mat.linear; - const bool are_too_close_points = - tier4_autoware_utils::calcDistance2d(front_wheel_pos, ref_points.at(i).p) < 1e-03; - const auto front_wheel_yaw = are_too_close_points ? ref_points.at(i).yaw - : tier4_autoware_utils::calcAzimuthAngle( - ref_points.at(i).p, front_wheel_pos); - ref_points.at(i).alpha = - tier4_autoware_utils::normalizeRadian(front_wheel_yaw - ref_points.at(i).yaw); + auto updated_obj_mat = obj_mat; + auto updated_const_mat = const_mat; - // near objects - ref_points.at(i).near_objects = [&]() { - const int avoidance_check_steps = - mpt_param_.near_objects_length / mpt_param_.delta_arc_length_for_mpt_points; + Eigen::VectorXd & f = updated_obj_mat.gradient; + Eigen::VectorXd & ub = updated_const_mat.upper_bound; + Eigen::VectorXd & lb = updated_const_mat.lower_bound; - const int avoidance_check_begin_idx = - std::max(0, static_cast(i) - avoidance_check_steps); - const int avoidance_check_end_idx = - std::min(static_cast(ref_points.size()), static_cast(i) + avoidance_check_steps); + // update gradient + f += H * *u0; - for (int a_idx = avoidance_check_begin_idx; a_idx < avoidance_check_end_idx; ++a_idx) { - if (ref_points.at(a_idx).vehicle_bounds.at(0).hasCollisionWithObject()) { - return true; - } - } - return false; - }(); + // update upper_bound and lower_bound + const Eigen::VectorXd A_times_u0 = A * *u0; + ub -= A_times_u0; + lb -= A_times_u0; - // The point are considered to be near the object if nearest previous ref point is near the - // object. - if (prev_trajs && !prev_trajs->mpt_ref_points.empty()) { - const auto & prev_ref_points = prev_trajs->mpt_ref_points; - - const auto ref_points_with_yaw = - points_utils::convertToPosesWithYawEstimation(points_utils::convertToPoints(ref_points)); - const size_t prev_idx = findNearestIndexWithSoftYawConstraints( - points_utils::convertToPoints(prev_ref_points), ref_points_with_yaw.at(i), - traj_param_.delta_dist_threshold_for_closest_point, - traj_param_.delta_yaw_threshold_for_closest_point); - const double dist_to_nearest_prev_ref = - tier4_autoware_utils::calcDistance2d(prev_ref_points.at(prev_idx), ref_points.at(i)); - if (dist_to_nearest_prev_ref < 1.0 && prev_ref_points.at(prev_idx).near_objects) { - ref_points.at(i).near_objects = true; - } - } - } + return {updated_obj_mat, updated_const_mat}; } -void MPTOptimizer::addSteerWeightR( - std::vector> & Rex_triplet_vec, - const std::vector & ref_points) const +std::optional> MPTOptimizer::calcMPTPoints( + std::vector & ref_points, const Eigen::VectorXd & U, + const StateEquationGenerator::Matrix & mpt_mat) const { - const size_t D_x = vehicle_model_ptr_->getDimX(); - const size_t D_u = vehicle_model_ptr_->getDimU(); - const size_t N_ref = ref_points.size(); - const size_t N_u = (N_ref - 1) * D_u; - const size_t D_v = D_x + N_u; - - // add steering rate : weight for (u(i) - u(i-1))^2 - for (size_t i = D_x; i < D_v - 1; ++i) { - Rex_triplet_vec.push_back(Eigen::Triplet(i, i, mpt_param_.steer_rate_weight)); - Rex_triplet_vec.push_back(Eigen::Triplet(i + 1, i, -mpt_param_.steer_rate_weight)); - Rex_triplet_vec.push_back(Eigen::Triplet(i, i + 1, -mpt_param_.steer_rate_weight)); - Rex_triplet_vec.push_back(Eigen::Triplet(i + 1, i + 1, mpt_param_.steer_rate_weight)); - } -} + time_keeper_ptr_->tic(__func__); -void MPTOptimizer::calcBounds( - std::vector & ref_points, - const std::vector & left_bound, - const std::vector & right_bound, DebugData & debug_data) const -{ - stop_watch_.tic(__func__); + const size_t D_x = state_equation_generator_.getDimX(); + const size_t D_u = state_equation_generator_.getDimU(); + const size_t N_ref = ref_points.size(); + const size_t D_v = D_x + (N_ref - 1) * D_u; + const size_t N_slack = getNumberOfSlackVariables(); - if (left_bound.empty() || right_bound.empty()) { - std::cerr << "[ObstacleAvoidancePlanner]: Boundary is empty when calculating bounds" - << std::endl; - return; - } + const Eigen::VectorXd steer_angles = U.segment(0, D_v); + const Eigen::VectorXd slack_variables = U.segment(D_v, N_ref * N_slack); - /* - const double min_soft_road_clearance = vehicle_param_.width / 2.0 + - mpt_param_.soft_clearance_from_road + - mpt_param_.extra_desired_clearance_from_road; - */ - const double base_to_right = (vehicle_param_.wheel_tread / 2.0) + vehicle_param_.right_overhang; - const double base_to_left = (vehicle_param_.wheel_tread / 2.0) + vehicle_param_.left_overhang; - - // search bounds candidate for each ref points - debug_data.bounds_candidates.clear(); - for (size_t i = 0; i < ref_points.size() - 1; ++i) { - const auto curr_ref_position = convertRefPointsToPose(ref_points.at(i)).position; - const auto next_ref_position = convertRefPointsToPose(ref_points.at(i + 1)).position; - const Eigen::Vector2d current_ref_point = {curr_ref_position.x, curr_ref_position.y}; - const Eigen::Vector2d next_ref_point = {next_ref_position.x, next_ref_position.y}; - const Eigen::Vector2d current_to_next_vec = next_ref_point - current_ref_point; - const Eigen::Vector2d left_vertical_vec = {-current_to_next_vec(1), current_to_next_vec(0)}; - const Eigen::Vector2d right_vertical_vec = {current_to_next_vec(1), -current_to_next_vec(0)}; - - const Eigen::Vector2d left_point = current_ref_point + left_vertical_vec.normalized() * 5.0; - const Eigen::Vector2d right_point = current_ref_point + right_vertical_vec.normalized() * 5.0; - const double lat_dist_to_left_bound = std::min( - calcLateralDistToBound(current_ref_point, left_point, left_bound) - base_to_left, 5.0); - const double lat_dist_to_right_bound = std::max( - calcLateralDistToBound(current_ref_point, right_point, right_bound, true) + base_to_right, - -5.0); - - ref_points.at(i).bounds = Bounds{ - lat_dist_to_right_bound, lat_dist_to_left_bound, CollisionType::NO_COLLISION, - CollisionType::NO_COLLISION}; - debug_data.bounds_candidates.push_back(ref_points.at(i).bounds); - } + // predict time-series states from optimized control inputs + const Eigen::VectorXd X = state_equation_generator_.predict(mpt_mat, steer_angles); - // Terminal Boundary - const double lat_dist_to_left_bound = - -motion_utils::calcLateralOffset( - left_bound, convertRefPointsToPose(ref_points.back()).position) - - base_to_left; - const double lat_dist_to_right_bound = - -motion_utils::calcLateralOffset( - right_bound, convertRefPointsToPose(ref_points.back()).position) + - base_to_right; - ref_points.back().bounds = Bounds{ - lat_dist_to_right_bound, lat_dist_to_left_bound, CollisionType::NO_COLLISION, - CollisionType::NO_COLLISION}; - debug_data.bounds_candidates.push_back(ref_points.back().bounds); - - debug_data.msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) - << " [ms]\n"; - return; -} + // calculate trajectory points from optimization result + std::vector traj_points; + for (size_t i = 0; i < N_ref; ++i) { + auto & ref_point = ref_points.at(i); -void MPTOptimizer::calcVehicleBounds( - std::vector & ref_points, DebugData & debug_data) const -{ - stop_watch_.tic(__func__); + const double lat_error = X(i * D_x); + const double yaw_error = X(i * D_x + 1); - if (ref_points.size() == 1) { - for ([[maybe_unused]] const double d : mpt_param_.vehicle_circle_longitudinal_offsets) { - ref_points.at(0).vehicle_bounds.push_back(ref_points.at(0).bounds); - ref_points.at(0).beta.push_back(0.0); + // validate optimization result + if (mpt_param_.enable_optimization_validation) { + if ( + mpt_param_.max_validation_lat_error < std::abs(lat_error) || + mpt_param_.max_validation_yaw_error < std::abs(yaw_error)) { + return std::nullopt; + } } - return; - } - SplineInterpolationPoints2d ref_points_spline_interpolation( - points_utils::convertToPoints(ref_points)); - - for (size_t p_idx = 0; p_idx < ref_points.size(); ++p_idx) { - const auto & ref_point = ref_points.at(p_idx); - ref_points.at(p_idx).vehicle_bounds.clear(); - ref_points.at(p_idx).beta.clear(); + // memorize optimization result (optimized_kinematic_state and optimized_input) + ref_point.optimized_kinematic_state = KinematicState{lat_error, yaw_error}; + if (i == N_ref - 1) { + ref_point.optimized_input = 0.0; + } else { + ref_point.optimized_input = steer_angles(D_x + i * D_u); + } - for (const double d : mpt_param_.vehicle_circle_longitudinal_offsets) { - geometry_msgs::msg::Pose avoid_traj_pose; - avoid_traj_pose.position = - ref_points_spline_interpolation.getSplineInterpolatedPoint(p_idx, d); - const double vehicle_bounds_pose_yaw = - ref_points_spline_interpolation.getSplineInterpolatedYaw(p_idx, d); - avoid_traj_pose.orientation = - tier4_autoware_utils::createQuaternionFromYaw(vehicle_bounds_pose_yaw); - - const double avoid_yaw = std::atan2( - avoid_traj_pose.position.y - ref_point.p.y, avoid_traj_pose.position.x - ref_point.p.x); - const double beta = ref_point.yaw - vehicle_bounds_pose_yaw; - ref_points.at(p_idx).beta.push_back(beta); + std::vector tmp_slack_variables; + for (size_t j = 0; j < N_slack; ++j) { + tmp_slack_variables.push_back(slack_variables(i * N_slack + j)); + } + ref_point.slack_variables = tmp_slack_variables; - const double offset_y = -tier4_autoware_utils::calcDistance2d(ref_point, avoid_traj_pose) * - std::sin(avoid_yaw - vehicle_bounds_pose_yaw); + // update pose and velocity + TrajectoryPoint traj_point; + traj_point.pose = ref_point.offsetDeviation(lat_error, yaw_error); + traj_point.longitudinal_velocity_mps = ref_point.longitudinal_velocity_mps; - const auto vehicle_bounds_pose = - tier4_autoware_utils::calcOffsetPose(avoid_traj_pose, 0.0, offset_y, 0.0); + traj_points.push_back(traj_point); + } - // interpolate bounds - const double avoid_s = ref_points_spline_interpolation.getAccumulatedLength(p_idx) + d; - for (size_t cp_idx = 0; cp_idx < ref_points.size(); ++cp_idx) { - const double current_s = ref_points_spline_interpolation.getAccumulatedLength(cp_idx); - if (avoid_s <= current_s) { - double prev_avoid_idx; - if (cp_idx == 0) { - prev_avoid_idx = cp_idx; - } else { - prev_avoid_idx = cp_idx - 1; - } + time_keeper_ptr_->toc(__func__, " "); + return traj_points; +} - const double prev_s = - ref_points_spline_interpolation.getAccumulatedLength(prev_avoid_idx); - const double next_s = - ref_points_spline_interpolation.getAccumulatedLength(prev_avoid_idx + 1); - const double ratio = (avoid_s - prev_s) / (next_s - prev_s); +void MPTOptimizer::publishDebugTrajectories( + const std_msgs::msg::Header & header, const std::vector & ref_points, + const std::vector & mpt_traj_points) const +{ + // reference points + const auto ref_traj = trajectory_utils::createTrajectory( + header, trajectory_utils::convertToTrajectoryPoints(ref_points)); + debug_ref_traj_pub_->publish(ref_traj); + + // fixed reference points + const auto fixed_traj_points = extractFixedPoints(ref_points); + const auto fixed_traj = trajectory_utils::createTrajectory(header, fixed_traj_points); + debug_fixed_traj_pub_->publish(fixed_traj); + + // mpt points + const auto mpt_traj = trajectory_utils::createTrajectory(header, mpt_traj_points); + debug_mpt_traj_pub_->publish(mpt_traj); +} - const auto prev_bounds = ref_points.at(prev_avoid_idx).bounds; - const auto next_bounds = ref_points.at(prev_avoid_idx + 1).bounds; +std::vector MPTOptimizer::extractFixedPoints( + const std::vector & ref_points) const +{ + std::vector fixed_traj_points; + for (const auto & ref_point : ref_points) { + if (ref_point.fixed_kinematic_state) { + TrajectoryPoint fixed_traj_point; + fixed_traj_point.pose = ref_point.offsetDeviation( + ref_point.fixed_kinematic_state->lat, ref_point.fixed_kinematic_state->yaw); + fixed_traj_points.push_back(fixed_traj_point); + } + } - auto bounds = Bounds::lerp(prev_bounds, next_bounds, ratio); - bounds.translate(offset_y); + return fixed_traj_points; +} - ref_points.at(p_idx).vehicle_bounds.push_back(bounds); - break; - } +double MPTOptimizer::getTrajectoryLength() const +{ + const double forward_traj_length = mpt_param_.num_points * mpt_param_.delta_arc_length; + const double backward_traj_length = traj_param_.output_backward_traj_length; + return forward_traj_length + backward_traj_length; +} - if (cp_idx == ref_points.size() - 1) { - ref_points.at(p_idx).vehicle_bounds.push_back(ref_points.back().bounds); - } - } +int MPTOptimizer::getNumberOfPoints() const { return mpt_param_.num_points; } - ref_points.at(p_idx).vehicle_bounds_poses.push_back(vehicle_bounds_pose); +size_t MPTOptimizer::getNumberOfSlackVariables() const +{ + if (mpt_param_.soft_constraint) { + if (mpt_param_.l_inf_norm) { + return 1; } + return vehicle_circle_longitudinal_offsets_.size(); } + return 0; +} - debug_data.msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) - << " [ms]\n"; +std::optional MPTOptimizer::calcNormalizedAvoidanceCost( + const ReferencePoint & ref_point) const +{ + const double negative_avoidance_cost = std::min( + -ref_point.bounds.lower_bound - mpt_param_.avoidance_cost_margin, + ref_point.bounds.upper_bound - mpt_param_.avoidance_cost_margin); + if (0 <= negative_avoidance_cost) { + return {}; + } + return std::clamp(-negative_avoidance_cost / mpt_param_.max_avoidance_cost, 0.0, 1.0); } +} // namespace obstacle_avoidance_planner diff --git a/planning/obstacle_avoidance_planner/src/node.cpp b/planning/obstacle_avoidance_planner/src/node.cpp index bee261ef532e0..e387cbbbc2b1e 100644 --- a/planning/obstacle_avoidance_planner/src/node.cpp +++ b/planning/obstacle_avoidance_planner/src/node.cpp @@ -1,4 +1,4 @@ -// Copyright 2020 Tier IV, Inc. +// Copyright 2023 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -15,581 +15,117 @@ #include "obstacle_avoidance_planner/node.hpp" #include "interpolation/spline_interpolation_points_2d.hpp" -#include "motion_utils/motion_utils.hpp" -#include "obstacle_avoidance_planner/utils/debug_utils.hpp" -#include "obstacle_avoidance_planner/utils/utils.hpp" +#include "obstacle_avoidance_planner/debug_marker.hpp" +#include "obstacle_avoidance_planner/utils/geometry_utils.hpp" +#include "obstacle_avoidance_planner/utils/trajectory_utils.hpp" #include "rclcpp/time.hpp" -#include "tf2/utils.h" -#include "tier4_autoware_utils/ros/update_param.hpp" -#include "vehicle_info_util/vehicle_info_util.hpp" -#include "geometry_msgs/msg/transform_stamped.hpp" -#include "geometry_msgs/msg/twist_stamped.hpp" -#include "std_msgs/msg/bool.hpp" -#include "visualization_msgs/msg/marker_array.hpp" - -#include #include #include -#include -#include -#include - -namespace -{ -std::tuple, std::vector> calcVehicleCirclesInfo( - const VehicleParam & vehicle_param, const size_t circle_num, const double rear_radius_ratio, - const double front_radius_ratio) -{ - std::vector longitudinal_offsets; - std::vector radiuses; - const double lateral_offset = - abs(vehicle_param.right_overhang - vehicle_param.left_overhang) / 2.0; - - { // 1st circle (rear) - const double radius = (vehicle_param.width / 2.0 + lateral_offset) * rear_radius_ratio; - - longitudinal_offsets.push_back(-vehicle_param.rear_overhang); - radiuses.push_back(radius); - } - - { // 2nd circle (front) - const double radius = - (std::hypot( - vehicle_param.length / static_cast(circle_num) / 2.0, - (vehicle_param.width / 2.0 + lateral_offset)) * - front_radius_ratio); - - const double unit_lon_length = vehicle_param.length / static_cast(circle_num); - const double longitudinal_offset = - unit_lon_length / 2.0 + unit_lon_length * (circle_num - 1) - vehicle_param.rear_overhang; - - longitudinal_offsets.push_back(longitudinal_offset); - radiuses.push_back(radius); - } - - return {radiuses, longitudinal_offsets}; -} - -std::tuple, std::vector> calcVehicleCirclesInfo( - const VehicleParam & vehicle_param, const size_t circle_num, const double radius_ratio) -{ - std::vector longitudinal_offsets; - std::vector radiuses; - const double lateral_offset = - abs(vehicle_param.right_overhang - vehicle_param.left_overhang) / 2.0; - - const double radius = std::hypot( - vehicle_param.length / static_cast(circle_num) / 2.0, - (vehicle_param.width / 2.0 + lateral_offset)) * - radius_ratio; - const double unit_lon_length = vehicle_param.length / static_cast(circle_num); - - for (size_t i = 0; i < circle_num; ++i) { - longitudinal_offsets.push_back( - unit_lon_length / 2.0 + unit_lon_length * i - vehicle_param.rear_overhang); - radiuses.push_back(radius); - } - - return {radiuses, longitudinal_offsets}; -} - -std::tuple, std::vector> calcVehicleCirclesInfo( - const VehicleParam & vehicle_param, size_t circle_num) -{ - circle_num = std::max(circle_num, static_cast(2)); - - std::vector longitudinal_offsets; - std::vector radiuses; - const double lateral_offset = - abs(vehicle_param.right_overhang - vehicle_param.left_overhang) / 2.0; - - const double radius = (vehicle_param.width / 2.0 + lateral_offset); - const double unit_lon_length = vehicle_param.length / static_cast(circle_num - 1); - - for (size_t i = 0; i < circle_num; ++i) { - longitudinal_offsets.push_back(unit_lon_length * i - vehicle_param.rear_overhang); - radiuses.push_back(radius); - } - - return {radiuses, longitudinal_offsets}; -} -std::tuple, std::vector> calcVehicleCirclesInfoByBicycleModel( - const VehicleParam & vehicle_param, const size_t circle_num, const double rear_radius_ratio, - const double front_radius_ratio) +namespace obstacle_avoidance_planner { - std::vector longitudinal_offsets; - std::vector radiuses; - const double lateral_offset = - abs(vehicle_param.right_overhang - vehicle_param.left_overhang) / 2.0; - - { // 1st circle (rear wheel) - const double radius = (vehicle_param.width / 2.0 + lateral_offset) * rear_radius_ratio; - longitudinal_offsets.push_back(0.0); - radiuses.push_back(radius); - } - - { // 2nd circle (front wheel) - const double radius = std::hypot( - vehicle_param.length / static_cast(circle_num) / 2.0, - (vehicle_param.width / 2.0 + lateral_offset)) * - front_radius_ratio; - - const double unit_lon_length = vehicle_param.length / static_cast(circle_num); - const double longitudinal_offset = - unit_lon_length / 2.0 + unit_lon_length * (circle_num - 1) - vehicle_param.rear_overhang; - - longitudinal_offsets.push_back(longitudinal_offset); - radiuses.push_back(radius); - } - - return {radiuses, longitudinal_offsets}; -} - -[[maybe_unused]] void fillYawInTrajectoryPoint(std::vector & traj_points) +namespace { - std::vector points; - for (const auto & traj_point : traj_points) { - points.push_back(traj_point.pose.position); - } - const auto yaw_vec = interpolation::splineYawFromPoints(points); - - for (size_t i = 0; i < traj_points.size(); ++i) { - traj_points.at(i).pose.orientation = - tier4_autoware_utils::createQuaternionFromYaw(yaw_vec.at(i)); - } -} - template -[[maybe_unused]] size_t findNearestIndexWithSoftYawConstraints( - const std::vector & points, const geometry_msgs::msg::Pose & pose, const double dist_threshold, - const double yaw_threshold) -{ - const auto nearest_idx_optional = - motion_utils::findNearestIndex(points, pose, dist_threshold, yaw_threshold); - return nearest_idx_optional ? *nearest_idx_optional - : motion_utils::findNearestIndex(points, pose.position); -} - -template <> -[[maybe_unused]] size_t findNearestIndexWithSoftYawConstraints( - const std::vector & points, const geometry_msgs::msg::Pose & pose, - const double dist_threshold, const double yaw_threshold) +std::vector concatVectors(const std::vector & prev_vector, const std::vector & next_vector) { - const auto points_with_yaw = points_utils::convertToPosesWithYawEstimation(points); - - return findNearestIndexWithSoftYawConstraints( - points_with_yaw, pose, dist_threshold, yaw_threshold); + std::vector concatenated_vector; + concatenated_vector.insert(concatenated_vector.end(), prev_vector.begin(), prev_vector.end()); + concatenated_vector.insert(concatenated_vector.end(), next_vector.begin(), next_vector.end()); + return concatenated_vector; } -template -[[maybe_unused]] size_t findNearestSegmentIndexWithSoftYawConstraints( - const std::vector & points, const geometry_msgs::msg::Pose & pose, const double dist_threshold, - const double yaw_threshold) +StringStamped createStringStamped(const rclcpp::Time & now, const std::string & data) { - const auto nearest_idx_optional = - motion_utils::findNearestSegmentIndex(points, pose, dist_threshold, yaw_threshold); - return nearest_idx_optional ? *nearest_idx_optional - : motion_utils::findNearestSegmentIndex(points, pose.position); + StringStamped msg; + msg.stamp = now; + msg.data = data; + return msg; } -template <> -[[maybe_unused]] size_t findNearestSegmentIndexWithSoftYawConstraints( - const std::vector & points, const geometry_msgs::msg::Pose & pose, - const double dist_threshold, const double yaw_threshold) +void setZeroVelocityAfterStopPoint(std::vector & traj_points) { - const auto points_with_yaw = points_utils::convertToPosesWithYawEstimation(points); - - return findNearestSegmentIndexWithSoftYawConstraints( - points_with_yaw, pose, dist_threshold, yaw_threshold); -} - -template -size_t searchExtendedZeroVelocityIndex( - const std::vector & fine_points, const std::vector & vel_points, - const double yaw_threshold) -{ - const auto opt_zero_vel_idx = motion_utils::searchZeroVelocityIndex(vel_points); - const size_t zero_vel_idx = opt_zero_vel_idx ? opt_zero_vel_idx.get() : vel_points.size() - 1; - return findNearestIndexWithSoftYawConstraints( - fine_points, vel_points.at(zero_vel_idx).pose, std::numeric_limits::max(), - yaw_threshold); + const auto opt_zero_vel_idx = motion_utils::searchZeroVelocityIndex(traj_points); + if (opt_zero_vel_idx) { + for (size_t i = opt_zero_vel_idx.get(); i < traj_points.size(); ++i) { + traj_points.at(i).longitudinal_velocity_mps = 0.0; + } + } } -Trajectory createTrajectory( - const std::vector & traj_points, const std_msgs::msg::Header & header) +bool hasZeroVelocity(const TrajectoryPoint & traj_point) { - auto traj = motion_utils::convertToTrajectory(traj_points); - traj.header = header; - - return traj; -} - -std::vector resampleTrajectoryPoints( - const std::vector & traj_points, const double interval) -{ - const auto traj = motion_utils::convertToTrajectory(traj_points); - const auto resampled_traj = motion_utils::resampleTrajectory(traj, interval); - - // convert Trajectory to std::vector - std::vector resampled_traj_points; - for (const auto & point : resampled_traj.points) { - resampled_traj_points.push_back(point); - } - - return resampled_traj_points; + constexpr double zero_vel = 0.0001; + return std::abs(traj_point.longitudinal_velocity_mps) < zero_vel; } } // namespace ObstacleAvoidancePlanner::ObstacleAvoidancePlanner(const rclcpp::NodeOptions & node_options) : Node("obstacle_avoidance_planner", node_options), - logger_ros_clock_(RCL_ROS_TIME), - eb_solved_count_(0) + vehicle_info_(vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo()), + debug_data_ptr_(std::make_shared()), + time_keeper_ptr_(std::make_shared()) { - rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_ROS_TIME); - - // qos - rclcpp::QoS durable_qos{1}; - durable_qos.transient_local(); - - // publisher to other nodes + // interface publisher traj_pub_ = create_publisher("~/output/path", 1); + virtual_wall_pub_ = create_publisher("~/virtual_wall", 1); - // debug publisher - debug_eb_traj_pub_ = create_publisher("~/debug/eb_trajectory", durable_qos); - debug_extended_fixed_traj_pub_ = create_publisher("~/debug/extended_fixed_traj", 1); - debug_extended_non_fixed_traj_pub_ = - create_publisher("~/debug/extended_non_fixed_traj", 1); - debug_mpt_fixed_traj_pub_ = create_publisher("~/debug/mpt_fixed_traj", 1); - debug_mpt_ref_traj_pub_ = create_publisher("~/debug/mpt_ref_traj", 1); - debug_mpt_traj_pub_ = create_publisher("~/debug/mpt_traj", 1); - debug_markers_pub_ = - create_publisher("~/debug/marker", durable_qos); - debug_wall_markers_pub_ = - create_publisher("~/debug/virtual_wall", durable_qos); - debug_clearance_map_pub_ = create_publisher("~/debug/clearance_map", durable_qos); - debug_object_clearance_map_pub_ = - create_publisher("~/debug/object_clearance_map", durable_qos); - debug_area_with_objects_pub_ = - create_publisher("~/debug/area_with_objects", durable_qos); - debug_msg_pub_ = - create_publisher("~/debug/calculation_time", 1); - - // subscriber + // interface subscriber path_sub_ = create_subscription( - "~/input/path", rclcpp::QoS{1}, - std::bind(&ObstacleAvoidancePlanner::onPath, this, std::placeholders::_1)); + "~/input/path", 1, std::bind(&ObstacleAvoidancePlanner::onPath, this, std::placeholders::_1)); odom_sub_ = create_subscription( - "/localization/kinematic_state", rclcpp::QoS{1}, - std::bind(&ObstacleAvoidancePlanner::onOdometry, this, std::placeholders::_1)); - objects_sub_ = create_subscription( - "~/input/objects", rclcpp::QoS{10}, - std::bind(&ObstacleAvoidancePlanner::onObjects, this, std::placeholders::_1)); - is_avoidance_sub_ = create_subscription( - "/planning/scenario_planning/lane_driving/obstacle_avoidance_approval", rclcpp::QoS{10}, - std::bind(&ObstacleAvoidancePlanner::onEnableAvoidance, this, std::placeholders::_1)); - - const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); - { // vehicle param - vehicle_param_ = VehicleParam{}; - vehicle_param_.width = vehicle_info.vehicle_width_m; - vehicle_param_.length = vehicle_info.vehicle_length_m; - vehicle_param_.wheelbase = vehicle_info.wheel_base_m; - vehicle_param_.rear_overhang = vehicle_info.rear_overhang_m; - vehicle_param_.front_overhang = vehicle_info.front_overhang_m; - vehicle_param_.right_overhang = vehicle_info.right_overhang_m; - vehicle_param_.left_overhang = vehicle_info.left_overhang_m; - vehicle_param_.wheel_tread = vehicle_info.wheel_tread_m; - } - - { // option parameter - is_publishing_debug_visualization_marker_ = - declare_parameter("option.is_publishing_debug_visualization_marker"); - is_publishing_clearance_map_ = declare_parameter("option.is_publishing_clearance_map"); - is_publishing_object_clearance_map_ = - declare_parameter("option.is_publishing_object_clearance_map"); - is_publishing_area_with_objects_ = - declare_parameter("option.is_publishing_area_with_objects"); - - is_showing_debug_info_ = declare_parameter("option.is_showing_debug_info"); - is_showing_calculation_time_ = declare_parameter("option.is_showing_calculation_time"); - is_stopping_if_outside_drivable_area_ = - declare_parameter("option.is_stopping_if_outside_drivable_area"); - - enable_avoidance_ = declare_parameter("option.enable_avoidance"); - enable_pre_smoothing_ = declare_parameter("option.enable_pre_smoothing"); - skip_optimization_ = declare_parameter("option.skip_optimization"); - reset_prev_optimization_ = declare_parameter("option.reset_prev_optimization"); - is_considering_footprint_edges_ = - declare_parameter("option.is_considering_footprint_edges"); - - vehicle_stop_margin_outside_drivable_area_ = - declare_parameter("common.vehicle_stop_margin_outside_drivable_area"); - } - - { // trajectory parameter - traj_param_ = TrajectoryParam{}; - - // common - traj_param_.num_sampling_points = declare_parameter("common.num_sampling_points"); - traj_param_.trajectory_length = declare_parameter("common.trajectory_length"); - traj_param_.forward_fixing_min_distance = - declare_parameter("common.forward_fixing_min_distance"); - traj_param_.forward_fixing_min_time = - declare_parameter("common.forward_fixing_min_time"); - traj_param_.backward_fixing_distance = - declare_parameter("common.backward_fixing_distance"); - traj_param_.delta_arc_length_for_trajectory = - declare_parameter("common.delta_arc_length_for_trajectory"); - - traj_param_.delta_dist_threshold_for_closest_point = - declare_parameter("common.delta_dist_threshold_for_closest_point"); - traj_param_.delta_yaw_threshold_for_closest_point = - declare_parameter("common.delta_yaw_threshold_for_closest_point"); - traj_param_.delta_yaw_threshold_for_straight = - declare_parameter("common.delta_yaw_threshold_for_straight"); - - traj_param_.num_fix_points_for_extending = - declare_parameter("common.num_fix_points_for_extending"); - traj_param_.max_dist_for_extending_end_point = - declare_parameter("common.max_dist_for_extending_end_point"); - traj_param_.non_fixed_trajectory_length = - declare_parameter("common.non_fixed_trajectory_length"); - - traj_param_.enable_clipping_fixed_traj = - declare_parameter("common.enable_clipping_fixed_traj"); - - // object - traj_param_.max_avoiding_ego_velocity_ms = - declare_parameter("object.max_avoiding_ego_velocity_ms"); - traj_param_.max_avoiding_objects_velocity_ms = - declare_parameter("object.max_avoiding_objects_velocity_ms"); - traj_param_.is_avoiding_unknown = - declare_parameter("object.avoiding_object_type.unknown", true); - traj_param_.is_avoiding_car = declare_parameter("object.avoiding_object_type.car", true); - traj_param_.is_avoiding_truck = - declare_parameter("object.avoiding_object_type.truck", true); - traj_param_.is_avoiding_bus = declare_parameter("object.avoiding_object_type.bus", true); - traj_param_.is_avoiding_bicycle = - declare_parameter("object.avoiding_object_type.bicycle", true); - traj_param_.is_avoiding_motorbike = - declare_parameter("object.avoiding_object_type.motorbike", true); - traj_param_.is_avoiding_pedestrian = - declare_parameter("object.avoiding_object_type.pedestrian", true); - traj_param_.is_avoiding_animal = - declare_parameter("object.avoiding_object_type.animal", true); - - // ego nearest search - traj_param_.ego_nearest_dist_threshold = - declare_parameter("ego_nearest_dist_threshold"); - traj_param_.ego_nearest_yaw_threshold = declare_parameter("ego_nearest_yaw_threshold"); - } - - { // elastic band parameter - eb_param_ = EBParam{}; - - // common - eb_param_.num_joint_buffer_points = - declare_parameter("advanced.eb.common.num_joint_buffer_points"); - eb_param_.num_offset_for_begin_idx = - declare_parameter("advanced.eb.common.num_offset_for_begin_idx"); - eb_param_.delta_arc_length_for_eb = - declare_parameter("advanced.eb.common.delta_arc_length_for_eb"); - eb_param_.num_sampling_points_for_eb = - declare_parameter("advanced.eb.common.num_sampling_points_for_eb"); - - // clearance - eb_param_.clearance_for_straight_line = - declare_parameter("advanced.eb.clearance.clearance_for_straight_line"); - eb_param_.clearance_for_joint = - declare_parameter("advanced.eb.clearance.clearance_for_joint"); - eb_param_.clearance_for_only_smoothing = - declare_parameter("advanced.eb.clearance.clearance_for_only_smoothing"); - - // qp - eb_param_.qp_param.max_iteration = declare_parameter("advanced.eb.qp.max_iteration"); - eb_param_.qp_param.eps_abs = declare_parameter("advanced.eb.qp.eps_abs"); - eb_param_.qp_param.eps_rel = declare_parameter("advanced.eb.qp.eps_rel"); - - // other - eb_param_.clearance_for_fixing = 0.0; - } - - { // mpt param - mpt_param_ = MPTParam{}; - - // option - // TODO(murooka) implement plan_from_ego - mpt_param_.plan_from_ego = declare_parameter("mpt.option.plan_from_ego"); - mpt_param_.max_plan_from_ego_length = - declare_parameter("mpt.option.max_plan_from_ego_length"); - mpt_param_.steer_limit_constraint = - declare_parameter("mpt.option.steer_limit_constraint"); - mpt_param_.fix_points_around_ego = declare_parameter("mpt.option.fix_points_around_ego"); - mpt_param_.enable_warm_start = declare_parameter("mpt.option.enable_warm_start"); - mpt_param_.enable_manual_warm_start = - declare_parameter("mpt.option.enable_manual_warm_start"); - mpt_visualize_sampling_num_ = declare_parameter("mpt.option.visualize_sampling_num"); - mpt_param_.is_fixed_point_single = declare_parameter("mpt.option.is_fixed_point_single"); - - // common - mpt_param_.num_curvature_sampling_points = - declare_parameter("mpt.common.num_curvature_sampling_points"); - - mpt_param_.delta_arc_length_for_mpt_points = - declare_parameter("mpt.common.delta_arc_length_for_mpt_points"); - - // kinematics - mpt_param_.max_steer_rad = vehicle_info.max_steer_angle_rad; - - // By default, optimization_center_offset will be vehicle_info.wheel_base * 0.8 - // The 0.8 scale is adopted as it performed the best. - constexpr double default_wheelbase_ratio = 0.8; - mpt_param_.optimization_center_offset = declare_parameter( - "mpt.kinematics.optimization_center_offset", - vehicle_param_.wheelbase * default_wheelbase_ratio); - - // bounds search - mpt_param_.bounds_search_widths = - declare_parameter>("advanced.mpt.bounds_search_widths"); - - // collision free constraints - mpt_param_.l_inf_norm = - declare_parameter("advanced.mpt.collision_free_constraints.option.l_inf_norm"); - mpt_param_.soft_constraint = - declare_parameter("advanced.mpt.collision_free_constraints.option.soft_constraint"); - mpt_param_.hard_constraint = - declare_parameter("advanced.mpt.collision_free_constraints.option.hard_constraint"); - - // TODO(murooka) implement two-step soft constraint - mpt_param_.two_step_soft_constraint = false; - // mpt_param_.two_step_soft_constraint = - // declare_parameter("advanced.mpt.collision_free_constraints.option.two_step_soft_constraint"); - - { // vehicle_circles - // NOTE: Vehicle shape for collision free constraints is considered as a set of circles - vehicle_circle_method_ = declare_parameter( - "advanced.mpt.collision_free_constraints.vehicle_circles.method"); - - if (vehicle_circle_method_ == "uniform_circle") { - vehicle_circle_num_for_calculation_ = declare_parameter( - "advanced.mpt.collision_free_constraints.vehicle_circles.uniform_circle.num"); - vehicle_circle_radius_ratios_.push_back(declare_parameter( - "advanced.mpt.collision_free_constraints.vehicle_circles.uniform_circle.radius_ratio")); - - std::tie( - mpt_param_.vehicle_circle_radiuses, mpt_param_.vehicle_circle_longitudinal_offsets) = - calcVehicleCirclesInfo( - vehicle_param_, vehicle_circle_num_for_calculation_, - vehicle_circle_radius_ratios_.front()); - } else if (vehicle_circle_method_ == "fitting_uniform_circle") { - vehicle_circle_num_for_calculation_ = declare_parameter( - "advanced.mpt.collision_free_constraints.vehicle_circles.fitting_uniform_circle.num"); - - std::tie( - mpt_param_.vehicle_circle_radiuses, mpt_param_.vehicle_circle_longitudinal_offsets) = - calcVehicleCirclesInfo(vehicle_param_, vehicle_circle_num_for_calculation_); - } else if (vehicle_circle_method_ == "rear_drive") { - vehicle_circle_num_for_calculation_ = declare_parameter( - "advanced.mpt.collision_free_constraints.vehicle_circles.rear_drive.num_for_calculation"); - - vehicle_circle_radius_ratios_.push_back(declare_parameter( - "advanced.mpt.collision_free_constraints.vehicle_circles.rear_drive.rear_radius_ratio")); - vehicle_circle_radius_ratios_.push_back(declare_parameter( - "advanced.mpt.collision_free_constraints.vehicle_circles.rear_drive.front_radius_ratio")); - - std::tie( - mpt_param_.vehicle_circle_radiuses, mpt_param_.vehicle_circle_longitudinal_offsets) = - calcVehicleCirclesInfo( - vehicle_param_, vehicle_circle_num_for_calculation_, - vehicle_circle_radius_ratios_.front(), vehicle_circle_radius_ratios_.back()); - } else if (vehicle_circle_method_ == "bicycle_model") { - vehicle_circle_num_for_calculation_ = declare_parameter( - "advanced.mpt.collision_free_constraints.vehicle_circles.bicycle_model.num_for_" - "calculation"); - - vehicle_circle_radius_ratios_.push_back( - declare_parameter("advanced.mpt.collision_free_constraints.vehicle_circles." - "bicycle_model.rear_radius_ratio")); - vehicle_circle_radius_ratios_.push_back( - declare_parameter("advanced.mpt.collision_free_constraints.vehicle_circles." - "bicycle_model.front_radius_ratio")); - - std::tie( - mpt_param_.vehicle_circle_radiuses, mpt_param_.vehicle_circle_longitudinal_offsets) = - calcVehicleCirclesInfoByBicycleModel( - vehicle_param_, vehicle_circle_num_for_calculation_, - vehicle_circle_radius_ratios_.front(), vehicle_circle_radius_ratios_.back()); - } else { - throw std::invalid_argument( - "advanced.mpt.collision_free_constraints.vehicle_circles.num parameter is invalid."); - } - } - - // clearance - mpt_param_.hard_clearance_from_road = - declare_parameter("advanced.mpt.clearance.hard_clearance_from_road"); - mpt_param_.soft_clearance_from_road = - declare_parameter("advanced.mpt.clearance.soft_clearance_from_road"); - mpt_param_.soft_second_clearance_from_road = - declare_parameter("advanced.mpt.clearance.soft_second_clearance_from_road"); - mpt_param_.extra_desired_clearance_from_road = - declare_parameter("advanced.mpt.clearance.extra_desired_clearance_from_road"); - mpt_param_.clearance_from_object = - declare_parameter("advanced.mpt.clearance.clearance_from_object"); - - // weight - mpt_param_.soft_avoidance_weight = - declare_parameter("advanced.mpt.weight.soft_avoidance_weight"); - mpt_param_.soft_second_avoidance_weight = - declare_parameter("advanced.mpt.weight.soft_second_avoidance_weight"); - - mpt_param_.lat_error_weight = declare_parameter("advanced.mpt.weight.lat_error_weight"); - mpt_param_.yaw_error_weight = declare_parameter("advanced.mpt.weight.yaw_error_weight"); - mpt_param_.yaw_error_rate_weight = - declare_parameter("advanced.mpt.weight.yaw_error_rate_weight"); - mpt_param_.steer_input_weight = - declare_parameter("advanced.mpt.weight.steer_input_weight"); - mpt_param_.steer_rate_weight = - declare_parameter("advanced.mpt.weight.steer_rate_weight"); - - mpt_param_.obstacle_avoid_lat_error_weight = - declare_parameter("advanced.mpt.weight.obstacle_avoid_lat_error_weight"); - mpt_param_.obstacle_avoid_yaw_error_weight = - declare_parameter("advanced.mpt.weight.obstacle_avoid_yaw_error_weight"); - mpt_param_.obstacle_avoid_steer_input_weight = - declare_parameter("advanced.mpt.weight.obstacle_avoid_steer_input_weight"); - mpt_param_.near_objects_length = - declare_parameter("advanced.mpt.weight.near_objects_length"); - - mpt_param_.terminal_lat_error_weight = - declare_parameter("advanced.mpt.weight.terminal_lat_error_weight"); - mpt_param_.terminal_yaw_error_weight = - declare_parameter("advanced.mpt.weight.terminal_yaw_error_weight"); - mpt_param_.terminal_path_lat_error_weight = - declare_parameter("advanced.mpt.weight.terminal_path_lat_error_weight"); - mpt_param_.terminal_path_yaw_error_weight = - declare_parameter("advanced.mpt.weight.terminal_path_yaw_error_weight"); - } + "~/input/odometry", 1, [this](const Odometry::SharedPtr msg) { ego_state_ptr_ = msg; }); - { // replan - max_path_shape_change_dist_for_replan_ = - declare_parameter("replan.max_path_shape_change_dist"); - max_ego_moving_dist_for_replan_ = - declare_parameter("replan.max_ego_moving_dist_for_replan"); - max_delta_time_sec_for_replan_ = - declare_parameter("replan.max_delta_time_sec_for_replan"); - } - - // TODO(murooka) tune this param when avoiding with obstacle_avoidance_planner - traj_param_.center_line_width = vehicle_param_.width; + // debug publisher + debug_extended_traj_pub_ = create_publisher("~/debug/extended_traj", 1); + debug_markers_pub_ = create_publisher("~/debug/marker", 1); + debug_calculation_time_pub_ = create_publisher("~/debug/calculation_time", 1); + + { // parameters + // parameter for option + enable_outside_drivable_area_stop_ = + declare_parameter("option.enable_outside_drivable_area_stop"); + enable_smoothing_ = declare_parameter("option.enable_smoothing"); + enable_skip_optimization_ = declare_parameter("option.enable_skip_optimization"); + enable_reset_prev_optimization_ = + declare_parameter("option.enable_reset_prev_optimization"); + use_footprint_polygon_for_outside_drivable_area_check_ = + declare_parameter("option.use_footprint_polygon_for_outside_drivable_area_check"); + + // parameter for debug marker + enable_pub_debug_marker_ = declare_parameter("option.debug.enable_pub_debug_marker"); + + // parameter for debug info + enable_debug_info_ = declare_parameter("option.debug.enable_debug_info"); + time_keeper_ptr_->enable_calculation_time_info = + declare_parameter("option.debug.enable_calculation_time_info"); + + // parameters for ego nearest search + ego_nearest_param_ = EgoNearestParam(this); + + // parameters for trajectory + traj_param_ = TrajectoryParam(this); + } + + // create core algorithm pointers with parameter declaration + replan_checker_ptr_ = std::make_shared(this, ego_nearest_param_); + eb_path_smoother_ptr_ = std::make_shared( + this, enable_debug_info_, ego_nearest_param_, traj_param_, time_keeper_ptr_); + mpt_optimizer_ptr_ = std::make_shared( + this, enable_debug_info_, ego_nearest_param_, vehicle_info_, traj_param_, debug_data_ptr_, + time_keeper_ptr_); + + // reset planners + // NOTE: This function must be called after core algorithms (e.g. mpt_optimizer_) have been + // initialized. + initializePlanning(); // set parameter callback + // NOTE: This function must be called after core algorithms (e.g. mpt_optimizer_) have been + // initialized. set_param_res_ = this->add_on_set_parameters_callback( std::bind(&ObstacleAvoidancePlanner::onParam, this, std::placeholders::_1)); - - resetPlanning(); } rcl_interfaces::msg::SetParametersResult ObstacleAvoidancePlanner::onParam( @@ -597,286 +133,39 @@ rcl_interfaces::msg::SetParametersResult ObstacleAvoidancePlanner::onParam( { using tier4_autoware_utils::updateParam; - { // option parameter - updateParam( - parameters, "option.is_publishing_debug_visualization_marker", - is_publishing_debug_visualization_marker_); - updateParam( - parameters, "option.is_publishing_clearance_map", is_publishing_clearance_map_); - updateParam( - parameters, "option.is_publishing_object_clearance_map", is_publishing_object_clearance_map_); - updateParam( - parameters, "option.is_publishing_area_with_objects", is_publishing_area_with_objects_); - - updateParam(parameters, "option.is_showing_debug_info", is_showing_debug_info_); - updateParam( - parameters, "option.is_showing_calculation_time", is_showing_calculation_time_); - updateParam( - parameters, "option.is_stopping_if_outside_drivable_area", - is_stopping_if_outside_drivable_area_); - - updateParam(parameters, "option.enable_avoidance", enable_avoidance_); - updateParam(parameters, "option.enable_pre_smoothing", enable_pre_smoothing_); - updateParam(parameters, "option.skip_optimization", skip_optimization_); - updateParam(parameters, "option.reset_prev_optimization", reset_prev_optimization_); - updateParam( - parameters, "option.is_considering_footprint_edges", is_considering_footprint_edges_); - - updateParam( - parameters, "common.vehicle_stop_margin_outside_drivable_area", - vehicle_stop_margin_outside_drivable_area_); - } + // parameters for option + updateParam( + parameters, "option.enable_outside_drivable_area_stop", enable_outside_drivable_area_stop_); + updateParam(parameters, "option.enable_smoothing", enable_smoothing_); + updateParam(parameters, "option.enable_skip_optimization", enable_skip_optimization_); + updateParam( + parameters, "option.enable_reset_prev_optimization", enable_reset_prev_optimization_); + updateParam( + parameters, "option.use_footprint_polygon_for_outside_drivable_area_check", + use_footprint_polygon_for_outside_drivable_area_check_); - { // trajectory parameter - // common - updateParam(parameters, "common.num_sampling_points", traj_param_.num_sampling_points); - updateParam(parameters, "common.trajectory_length", traj_param_.trajectory_length); - updateParam( - parameters, "common.forward_fixing_min_distance", traj_param_.forward_fixing_min_distance); - updateParam( - parameters, "common.forward_fixing_min_time", traj_param_.forward_fixing_min_time); - updateParam( - parameters, "common.backward_fixing_distance", traj_param_.backward_fixing_distance); - updateParam( - parameters, "common.delta_arc_length_for_trajectory", - traj_param_.delta_arc_length_for_trajectory); - - updateParam( - parameters, "common.delta_dist_threshold_for_closest_point", - traj_param_.delta_dist_threshold_for_closest_point); - updateParam( - parameters, "common.delta_yaw_threshold_for_closest_point", - traj_param_.delta_yaw_threshold_for_closest_point); - updateParam( - parameters, "common.delta_yaw_threshold_for_straight", - traj_param_.delta_yaw_threshold_for_straight); - updateParam( - parameters, "common.num_fix_points_for_extending", traj_param_.num_fix_points_for_extending); - updateParam( - parameters, "common.max_dist_for_extending_end_point", - traj_param_.max_dist_for_extending_end_point); - - // object - updateParam( - parameters, "object.max_avoiding_ego_velocity_ms", traj_param_.max_avoiding_ego_velocity_ms); - updateParam( - parameters, "object.max_avoiding_objects_velocity_ms", - traj_param_.max_avoiding_objects_velocity_ms); - updateParam( - parameters, "object.avoiding_object_type.unknown", traj_param_.is_avoiding_unknown); - updateParam(parameters, "object.avoiding_object_type.car", traj_param_.is_avoiding_car); - updateParam( - parameters, "object.avoiding_object_type.truck", traj_param_.is_avoiding_truck); - updateParam(parameters, "object.avoiding_object_type.bus", traj_param_.is_avoiding_bus); - updateParam( - parameters, "object.avoiding_object_type.bicycle", traj_param_.is_avoiding_bicycle); - updateParam( - parameters, "object.avoiding_object_type.motorbike", traj_param_.is_avoiding_motorbike); - updateParam( - parameters, "object.avoiding_object_type.pedestrian", traj_param_.is_avoiding_pedestrian); - updateParam( - parameters, "object.avoiding_object_type.animal", traj_param_.is_avoiding_animal); - } + // parameters for debug marker + updateParam(parameters, "option.debug.enable_pub_debug_marker", enable_pub_debug_marker_); - { // elastic band parameter - // common - updateParam( - parameters, "advanced.eb.common.num_joint_buffer_points", eb_param_.num_joint_buffer_points); - updateParam( - parameters, "advanced.eb.common.num_offset_for_begin_idx", - eb_param_.num_offset_for_begin_idx); - updateParam( - parameters, "advanced.eb.common.delta_arc_length_for_eb", eb_param_.delta_arc_length_for_eb); - updateParam( - parameters, "advanced.eb.common.num_sampling_points_for_eb", - eb_param_.num_sampling_points_for_eb); - - // clearance - updateParam( - parameters, "advanced.eb.clearance.clearance_for_straight_line", - eb_param_.clearance_for_straight_line); - updateParam( - parameters, "advanced.eb.clearance.clearance_for_joint", eb_param_.clearance_for_joint); - updateParam( - parameters, "advanced.eb.clearance.clearance_for_only_smoothing", - eb_param_.clearance_for_only_smoothing); - - // qp - updateParam(parameters, "advanced.eb.qp.max_iteration", eb_param_.qp_param.max_iteration); - updateParam(parameters, "advanced.eb.qp.eps_abs", eb_param_.qp_param.eps_abs); - updateParam(parameters, "advanced.eb.qp.eps_rel", eb_param_.qp_param.eps_rel); - } + // parameters for debug info + updateParam(parameters, "option.debug.enable_debug_info", enable_debug_info_); + updateParam( + parameters, "option.debug.enable_calculation_time_info", + time_keeper_ptr_->enable_calculation_time_info); - { // mpt param - // option - updateParam(parameters, "mpt.option.plan_from_ego", mpt_param_.plan_from_ego); - updateParam( - parameters, "mpt.option.max_plan_from_ego_length", mpt_param_.max_plan_from_ego_length); - updateParam( - parameters, "mpt.option.steer_limit_constraint", mpt_param_.steer_limit_constraint); - updateParam( - parameters, "mpt.option.fix_points_around_ego", mpt_param_.fix_points_around_ego); - updateParam(parameters, "mpt.option.enable_warm_start", mpt_param_.enable_warm_start); - updateParam( - parameters, "mpt.option.enable_manual_warm_start", mpt_param_.enable_manual_warm_start); - updateParam(parameters, "mpt.option.visualize_sampling_num", mpt_visualize_sampling_num_); - updateParam( - parameters, "mpt.option.option.is_fixed_point_single", mpt_param_.is_fixed_point_single); - - // common - updateParam( - parameters, "mpt.common.num_curvature_sampling_points", - mpt_param_.num_curvature_sampling_points); - - updateParam( - parameters, "mpt.common.delta_arc_length_for_mpt_points", - mpt_param_.delta_arc_length_for_mpt_points); - - // kinematics - updateParam( - parameters, "mpt.kinematics.optimization_center_offset", - mpt_param_.optimization_center_offset); - - // collision_free_constraints - updateParam( - parameters, "advanced.mpt.collision_free_constraints.option.l_inf_norm", - mpt_param_.l_inf_norm); - // updateParam( - // parameters, "advanced.mpt.collision_free_constraints.option.two_step_soft_constraint", - // mpt_param_.two_step_soft_constraint); - updateParam( - parameters, "advanced.mpt.collision_free_constraints.option.soft_constraint", - mpt_param_.soft_constraint); - updateParam( - parameters, "advanced.mpt.collision_free_constraints.option.hard_constraint", - mpt_param_.hard_constraint); - - { // vehicle_circles - // NOTE: Changing method is not supported - // updateParam( - // parameters, "advanced.mpt.collision_free_constraints.vehicle_circles.method", - // vehicle_circle_method_); - - if (vehicle_circle_method_ == "uniform_circle") { - updateParam( - parameters, "advanced.mpt.collision_free_constraints.vehicle_circles.uniform_circle.num", - vehicle_circle_num_for_calculation_); - updateParam( - parameters, - "advanced.mpt.collision_free_constraints.vehicle_circles.uniform_circle.radius_ratio", - vehicle_circle_radius_ratios_.front()); - - std::tie( - mpt_param_.vehicle_circle_radiuses, mpt_param_.vehicle_circle_longitudinal_offsets) = - calcVehicleCirclesInfo( - vehicle_param_, vehicle_circle_num_for_calculation_, - vehicle_circle_radius_ratios_.front()); - } else if (vehicle_circle_method_ == "fitting_uniform_circle") { - updateParam( - parameters, - "advanced.mpt.collision_free_constraints.vehicle_circles.fitting_uniform_circle.num", - vehicle_circle_num_for_calculation_); - - std::tie( - mpt_param_.vehicle_circle_radiuses, mpt_param_.vehicle_circle_longitudinal_offsets) = - calcVehicleCirclesInfo(vehicle_param_, vehicle_circle_num_for_calculation_); - } else if (vehicle_circle_method_ == "rear_drive") { - updateParam( - parameters, - "advanced.mpt.collision_free_constraints.vehicle_circles.rear_drive.num_for_calculation", - vehicle_circle_num_for_calculation_); - - updateParam( - parameters, - "advanced.mpt.collision_free_constraints.vehicle_circles.rear_drive.rear_radius_ratio", - vehicle_circle_radius_ratios_.front()); - - updateParam( - parameters, - "advanced.mpt.collision_free_constraints.vehicle_circles.rear_drive.front_radius_ratio", - vehicle_circle_radius_ratios_.back()); - - std::tie( - mpt_param_.vehicle_circle_radiuses, mpt_param_.vehicle_circle_longitudinal_offsets) = - calcVehicleCirclesInfo( - vehicle_param_, vehicle_circle_num_for_calculation_, - vehicle_circle_radius_ratios_.front(), vehicle_circle_radius_ratios_.back()); - } else { - throw std::invalid_argument( - "advanced.mpt.collision_free_constraints.vehicle_circles.num parameter is invalid."); - } - } + // parameters for ego nearest search + ego_nearest_param_.onParam(parameters); - // clearance - updateParam( - parameters, "advanced.mpt.clearance.hard_clearance_from_road", - mpt_param_.hard_clearance_from_road); - updateParam( - parameters, "advanced.mpt.clearance.soft_clearance_from_road", - mpt_param_.soft_clearance_from_road); - updateParam( - parameters, "advanced.mpt.clearance.soft_second_clearance_from_road", - mpt_param_.soft_second_clearance_from_road); - updateParam( - parameters, "advanced.mpt.clearance.extra_desired_clearance_from_road", - mpt_param_.extra_desired_clearance_from_road); - updateParam( - parameters, "advanced.mpt.clearance.clearance_from_object", mpt_param_.clearance_from_object); - - // weight - updateParam( - parameters, "advanced.mpt.weight.soft_avoidance_weight", mpt_param_.soft_avoidance_weight); - updateParam( - parameters, "advanced.mpt.weight.soft_second_avoidance_weight", - mpt_param_.soft_second_avoidance_weight); - - updateParam( - parameters, "advanced.mpt.weight.lat_error_weight", mpt_param_.lat_error_weight); - updateParam( - parameters, "advanced.mpt.weight.yaw_error_weight", mpt_param_.yaw_error_weight); - updateParam( - parameters, "advanced.mpt.weight.yaw_error_rate_weight", mpt_param_.yaw_error_rate_weight); - updateParam( - parameters, "advanced.mpt.weight.steer_input_weight", mpt_param_.steer_input_weight); - updateParam( - parameters, "advanced.mpt.weight.steer_rate_weight", mpt_param_.steer_rate_weight); - - updateParam( - parameters, "advanced.mpt.weight.obstacle_avoid_lat_error_weight", - mpt_param_.obstacle_avoid_lat_error_weight); - updateParam( - parameters, "advanced.mpt.weight.obstacle_avoid_yaw_error_weight", - mpt_param_.obstacle_avoid_yaw_error_weight); - updateParam( - parameters, "advanced.mpt.weight.obstacle_avoid_steer_input_weight", - mpt_param_.obstacle_avoid_steer_input_weight); - updateParam( - parameters, "advanced.mpt.weight.near_objects_length", mpt_param_.near_objects_length); - - updateParam( - parameters, "advanced.mpt.weight.terminal_lat_error_weight", - mpt_param_.terminal_lat_error_weight); - updateParam( - parameters, "advanced.mpt.weight.terminal_yaw_error_weight", - mpt_param_.terminal_yaw_error_weight); - updateParam( - parameters, "advanced.mpt.weight.terminal_path_lat_error_weight", - mpt_param_.terminal_path_lat_error_weight); - updateParam( - parameters, "advanced.mpt.weight.terminal_path_yaw_error_weight", - mpt_param_.terminal_path_yaw_error_weight); - } + // parameters for trajectory + traj_param_.onParam(parameters); - { // replan - updateParam( - parameters, "replan.max_path_shape_change_dist", max_path_shape_change_dist_for_replan_); - updateParam( - parameters, "replan.max_ego_moving_dist_for_replan", max_ego_moving_dist_for_replan_); - updateParam( - parameters, "replan.max_delta_time_sec_for_replan", max_delta_time_sec_for_replan_); - } + // parameters for core algorithms + replan_checker_ptr_->onParam(parameters); + eb_path_smoother_ptr_->onParam(parameters); + mpt_optimizer_ptr_->onParam(parameters); - resetPlanning(); + // reset planners + initializePlanning(); rcl_interfaces::msg::SetParametersResult result; result.successful = true; @@ -884,809 +173,394 @@ rcl_interfaces::msg::SetParametersResult ObstacleAvoidancePlanner::onParam( return result; } -void ObstacleAvoidancePlanner::onOdometry(const Odometry::SharedPtr msg) +void ObstacleAvoidancePlanner::initializePlanning() { - current_odometry_ptr_ = std::make_unique(*msg); -} + RCLCPP_INFO(get_logger(), "Initialize planning"); -void ObstacleAvoidancePlanner::onObjects(const PredictedObjects::SharedPtr msg) -{ - objects_ptr_ = std::make_unique(*msg); -} + eb_path_smoother_ptr_->initialize(enable_debug_info_, traj_param_); + mpt_optimizer_ptr_->initialize(enable_debug_info_, traj_param_); -void ObstacleAvoidancePlanner::onEnableAvoidance( - const tier4_planning_msgs::msg::EnableAvoidance::SharedPtr msg) -{ - enable_avoidance_ = msg->enable_avoidance; + resetPreviousData(); } -void ObstacleAvoidancePlanner::resetPlanning() +void ObstacleAvoidancePlanner::resetPreviousData() { - RCLCPP_WARN(get_logger(), "[ObstacleAvoidancePlanner] Reset planning"); - - eb_path_optimizer_ptr_ = std::make_unique( - is_showing_debug_info_, traj_param_, eb_param_, vehicle_param_); + eb_path_smoother_ptr_->resetPreviousData(); + mpt_optimizer_ptr_->resetPreviousData(); - mpt_optimizer_ptr_ = - std::make_unique(is_showing_debug_info_, traj_param_, vehicle_param_, mpt_param_); - - prev_path_points_ptr_ = nullptr; - resetPrevOptimization(); -} - -void ObstacleAvoidancePlanner::resetPrevOptimization() -{ - prev_optimal_trajs_ptr_ = nullptr; - eb_solved_count_ = 0; + prev_optimized_traj_points_ptr_ = nullptr; } void ObstacleAvoidancePlanner::onPath(const Path::SharedPtr path_ptr) { - stop_watch_.tic(__func__); + time_keeper_ptr_->init(); + time_keeper_ptr_->tic(__func__); - if (path_ptr->points.empty() || !current_odometry_ptr_ || !objects_ptr_) { + // check if data is ready and valid + if (!isDataReady(*path_ptr, *get_clock())) { return; } - if (path_ptr->left_bound.empty() || path_ptr->right_bound.empty()) { - std::cerr << "Right or left bound is empty" << std::endl; - } + // 0. return if path is backward + // TODO(murooka): support backward path + const auto is_driving_forward = driving_direction_checker_.isDrivingForward(path_ptr->points); + if (!is_driving_forward) { + RCLCPP_WARN_THROTTLE( + get_logger(), *get_clock(), 5000, + "Backward path is NOT supported. Just converting path to trajectory"); - // create planner data - PlannerData planner_data; - planner_data.path = *path_ptr; - planner_data.ego_pose = current_odometry_ptr_->pose.pose; - planner_data.ego_vel = current_odometry_ptr_->twist.twist.linear.x; - planner_data.objects = objects_ptr_->objects; - - debug_data_ = DebugData(); - debug_data_.init( - is_showing_calculation_time_, mpt_visualize_sampling_num_, planner_data.ego_pose, - mpt_param_.vehicle_circle_radiuses, mpt_param_.vehicle_circle_longitudinal_offsets); - - const auto output_traj_msg = generateTrajectory(planner_data); - - // publish debug data - publishDebugDataInMain(*path_ptr); - - { // print and publish debug msg - debug_data_.msg_stream << __func__ << ":= " << stop_watch_.toc(__func__) << " [ms]\n" - << "========================================"; - tier4_debug_msgs::msg::StringStamped debug_msg_msg; - debug_msg_msg.stamp = get_clock()->now(); - debug_msg_msg.data = debug_data_.msg_stream.getString(); - debug_msg_pub_->publish(debug_msg_msg); + const auto traj_points = trajectory_utils::convertToTrajectoryPoints(path_ptr->points); + const auto output_traj_msg = trajectory_utils::createTrajectory(path_ptr->header, traj_points); + traj_pub_->publish(output_traj_msg); + return; } - // make previous variables - prev_path_points_ptr_ = std::make_unique>(path_ptr->points); - prev_ego_pose_ptr_ = std::make_unique(planner_data.ego_pose); + // 1. create planner data + const auto planner_data = createPlannerData(*path_ptr); - traj_pub_->publish(output_traj_msg); -} + // 2. generate optimized trajectory + const auto optimized_traj_points = generateOptimizedTrajectory(planner_data); -Trajectory ObstacleAvoidancePlanner::generateTrajectory(const PlannerData & planner_data) -{ - const auto & p = planner_data; + // 3. extend trajectory to connect the optimized trajectory and the following path smoothly + auto full_traj_points = extendTrajectory(planner_data.traj_points, optimized_traj_points); - // TODO(someone): support backward path - const auto is_driving_forward = motion_utils::isDrivingForward(p.path.points); - is_driving_forward_ = is_driving_forward ? is_driving_forward.get() : is_driving_forward_; - if (!is_driving_forward_) { - RCLCPP_WARN_THROTTLE( - get_logger(), *get_clock(), 3000, - "[ObstacleAvoidancePlanner] Backward path is NOT supported. Just converting path to " - "trajectory"); + // 4. set zero velocity after stop point + setZeroVelocityAfterStopPoint(full_traj_points); - const auto traj_points = points_utils::convertToTrajectoryPoints(p.path.points); - return createTrajectory(traj_points, p.path.header); - } + // 5. publish debug data + publishDebugData(planner_data.header); - // generate optimized trajectory - const auto optimized_traj_points = generateOptimizedTrajectory(planner_data); - // generate post processed trajectory - const auto post_processed_traj_points = - generatePostProcessedTrajectory(p.path.points, optimized_traj_points, planner_data); + time_keeper_ptr_->toc(__func__, ""); + *time_keeper_ptr_ << "========================================"; + time_keeper_ptr_->endLine(); + + // publish calculation_time + // NOTE: This function must be called after measuring onPath calculation time + const auto calculation_time_msg = createStringStamped(now(), time_keeper_ptr_->getLog()); + debug_calculation_time_pub_->publish(calculation_time_msg); - // convert to output msg type - return createTrajectory(post_processed_traj_points, p.path.header); + const auto output_traj_msg = + trajectory_utils::createTrajectory(path_ptr->header, full_traj_points); + traj_pub_->publish(output_traj_msg); } -std::vector ObstacleAvoidancePlanner::generateOptimizedTrajectory( - const PlannerData & planner_data) +bool ObstacleAvoidancePlanner::isDataReady(const Path & path, rclcpp::Clock clock) const { - stop_watch_.tic(__func__); - - if (reset_prev_optimization_) { - resetPrevOptimization(); + if (!ego_state_ptr_) { + RCLCPP_INFO_SKIPFIRST_THROTTLE(get_logger(), clock, 5000, "Waiting for ego pose and twist."); + return false; } - const auto & path = planner_data.path; - - // return prev trajectory if replan is not required - if (!checkReplan(planner_data)) { - if (prev_optimal_trajs_ptr_) { - return prev_optimal_trajs_ptr_->model_predictive_trajectory; - } - - return points_utils::convertToTrajectoryPoints(path.points); + if (path.points.size() < 2) { + RCLCPP_INFO_SKIPFIRST_THROTTLE(get_logger(), clock, 5000, "Path points size is less than 1."); + return false; } - prev_replanned_time_ptr_ = std::make_unique(this->now()); - - // calculate trajectory with EB and MPT - auto optimal_trajs = optimizeTrajectory(planner_data); - // calculate velocity - // NOTE: Velocity is not considered in optimization. - calcVelocity(path.points, optimal_trajs.model_predictive_trajectory); - - // insert 0 velocity when trajectory is over drivable area - if (is_stopping_if_outside_drivable_area_) { - insertZeroVelocityOutsideDrivableArea(planner_data, optimal_trajs.model_predictive_trajectory); + if (path.left_bound.empty() || path.right_bound.empty()) { + RCLCPP_INFO_SKIPFIRST_THROTTLE( + get_logger(), clock, 5000, "Left or right bound in path is empty."); + return false; } - publishDebugDataInOptimization(planner_data, optimal_trajs.model_predictive_trajectory); + return true; +} - // make previous trajectories - prev_optimal_trajs_ptr_ = - std::make_unique(makePrevTrajectories(path.points, optimal_trajs, planner_data)); +PlannerData ObstacleAvoidancePlanner::createPlannerData(const Path & path) const +{ + // create planner data + PlannerData planner_data; + planner_data.header = path.header; + planner_data.traj_points = trajectory_utils::convertToTrajectoryPoints(path.points); + planner_data.left_bound = path.left_bound; + planner_data.right_bound = path.right_bound; + planner_data.ego_pose = ego_state_ptr_->pose.pose; + planner_data.ego_vel = ego_state_ptr_->twist.twist.linear.x; - debug_data_.msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) << " [ms]\n"; - return optimal_trajs.model_predictive_trajectory; + debug_data_ptr_->ego_pose = planner_data.ego_pose; + return planner_data; } -// check if optimization is required or not. -// NOTE: previous trajectories information will be reset as well in some cases. -bool ObstacleAvoidancePlanner::checkReplan(const PlannerData & planner_data) +std::vector ObstacleAvoidancePlanner::generateOptimizedTrajectory( + const PlannerData & planner_data) { - const auto & p = planner_data; - - if ( - !prev_ego_pose_ptr_ || !prev_replanned_time_ptr_ || !prev_path_points_ptr_ || - !prev_optimal_trajs_ptr_) { - return true; - } + time_keeper_ptr_->tic(__func__); - if (prev_optimal_trajs_ptr_->model_predictive_trajectory.empty()) { - RCLCPP_INFO( - get_logger(), - "Replan with resetting optimization since previous optimized trajectory is empty."); - resetPrevOptimization(); - return true; - } + const auto & input_traj_points = planner_data.traj_points; - if (isPathShapeChanged(p)) { - RCLCPP_INFO(get_logger(), "Replan with resetting optimization since path shape was changed."); - resetPrevOptimization(); - return true; - } + // 1. calculate trajectory with EB and MPT + // NOTE: This function may return previously optimized trajectory points. + // Also, velocity on some points will not be updated for a logic purpose. + auto optimized_traj_points = optimizeTrajectory(planner_data); - if (isPathGoalChanged(p)) { - RCLCPP_INFO(get_logger(), "Replan with resetting optimization since path goal was changed."); - resetPrevOptimization(); - return true; - } + // 2. update velocity + // NOTE: When optimization failed or is skipped, velocity in trajectory points must + // be updated since velocity in input trajectory (path) may change. + applyInputVelocity(optimized_traj_points, input_traj_points, planner_data.ego_pose); - // For when ego pose is lost or new ego pose is designated in simulation - const double delta_dist = - tier4_autoware_utils::calcDistance2d(p.ego_pose, prev_ego_pose_ptr_->position); - if (delta_dist > max_ego_moving_dist_for_replan_) { - RCLCPP_INFO( - get_logger(), - "Replan with resetting optimization since current ego pose is far from previous ego pose."); - resetPrevOptimization(); - return true; - } + // 3. insert zero velocity when trajectory is over drivable area + insertZeroVelocityOutsideDrivableArea(planner_data, optimized_traj_points); - // For when ego pose moves far from trajectory - if (!isEgoNearToPrevTrajectory(p.ego_pose)) { - RCLCPP_INFO( - get_logger(), - "Replan with resetting optimization since valid nearest trajectory point from ego was not " - "found."); - resetPrevOptimization(); - return true; - } + // 4. publish debug marker + publishDebugMarkerOfOptimization(optimized_traj_points); - const double delta_time_sec = (this->now() - *prev_replanned_time_ptr_).seconds(); - if (delta_time_sec > max_delta_time_sec_for_replan_) { - return true; - } - return false; + time_keeper_ptr_->toc(__func__, " "); + return optimized_traj_points; } -bool ObstacleAvoidancePlanner::isPathShapeChanged(const PlannerData & planner_data) +std::vector ObstacleAvoidancePlanner::optimizeTrajectory( + const PlannerData & planner_data) { - if (!prev_path_points_ptr_) { - return false; - } - + time_keeper_ptr_->tic(__func__); const auto & p = planner_data; - const double max_mpt_length = - traj_param_.num_sampling_points * mpt_param_.delta_arc_length_for_mpt_points; - - // truncate prev points from ego pose to fixed end points - const auto prev_begin_idx = findEgoNearestIndex(*prev_path_points_ptr_, p.ego_pose); - const auto truncated_prev_points = - points_utils::clipForwardPoints(*prev_path_points_ptr_, prev_begin_idx, max_mpt_length); - - // truncate points from ego pose to fixed end points - const auto begin_idx = findEgoNearestIndex(p.path.points, p.ego_pose); - const auto truncated_points = - points_utils::clipForwardPoints(p.path.points, begin_idx, max_mpt_length); - - // guard for lateral offset - if (truncated_prev_points.size() < 2 || truncated_points.size() < 2) { - return false; - } - - // calculate lateral deviations between truncated path_points and prev_path_points - for (const auto & prev_point : truncated_prev_points) { - const double dist = - std::abs(motion_utils::calcLateralOffset(truncated_points, prev_point.pose.position)); - if (dist > max_path_shape_change_dist_for_replan_) { - return true; + // 1. check if replan (= optimization) is required + const bool reset_prev_optimization = replan_checker_ptr_->isResetRequired(planner_data); + if (enable_reset_prev_optimization_ || reset_prev_optimization) { + // NOTE: always replan when resetting previous optimization + resetPreviousData(); + } else { + // check replan when not resetting previous optimization + const bool is_replan_required = replan_checker_ptr_->isReplanRequired(now()); + if (!is_replan_required) { + return getPrevOptimizedTrajectory(p.traj_points); } } - return false; -} - -bool ObstacleAvoidancePlanner::isPathGoalChanged(const PlannerData & planner_data) -{ - const auto & p = planner_data; - - if (!prev_path_points_ptr_) { - return false; + if (enable_skip_optimization_) { + return p.traj_points; } - constexpr double min_vel = 1e-3; - if (std::abs(p.ego_vel) > min_vel) { - return false; + // 2. smooth trajectory with elastic band + const auto eb_traj = + enable_smoothing_ ? eb_path_smoother_ptr_->getEBTrajectory(planner_data) : p.traj_points; + if (!eb_traj) { + return getPrevOptimizedTrajectory(p.traj_points); } - // NOTE: Path may be cropped and does not contain the goal. - // Therefore we set a large value to distance threshold. - constexpr double max_goal_moving_dist = 1.0; - const double goal_moving_dist = - tier4_autoware_utils::calcDistance2d(p.path.points.back(), prev_path_points_ptr_->back()); - if (goal_moving_dist < max_goal_moving_dist) { - return false; + // 3. make trajectory kinematically-feasible and collision-free (= inside the drivable area) + // with model predictive trajectory + const auto mpt_traj = mpt_optimizer_ptr_->getModelPredictiveTrajectory(planner_data, *eb_traj); + if (!mpt_traj) { + return getPrevOptimizedTrajectory(p.traj_points); } - return true; + // 4. make prev trajectories + prev_optimized_traj_points_ptr_ = std::make_shared>(*mpt_traj); + + time_keeper_ptr_->toc(__func__, " "); + return *mpt_traj; } -bool ObstacleAvoidancePlanner::isEgoNearToPrevTrajectory(const geometry_msgs::msg::Pose & ego_pose) +std::vector ObstacleAvoidancePlanner::getPrevOptimizedTrajectory( + const std::vector & traj_points) const { - const auto & traj_points = prev_optimal_trajs_ptr_->model_predictive_trajectory; - - const auto resampled_traj_points = - resampleTrajectoryPoints(traj_points, traj_param_.delta_arc_length_for_trajectory); - const auto opt_nearest_idx = motion_utils::findNearestIndex( - resampled_traj_points, ego_pose, traj_param_.delta_dist_threshold_for_closest_point, - traj_param_.delta_yaw_threshold_for_closest_point); - - if (!opt_nearest_idx) { - return false; + if (prev_optimized_traj_points_ptr_) { + return *prev_optimized_traj_points_ptr_; } - return true; + + return traj_points; } -Trajectories ObstacleAvoidancePlanner::optimizeTrajectory(const PlannerData & planner_data) +void ObstacleAvoidancePlanner::applyInputVelocity( + std::vector & output_traj_points, + const std::vector & input_traj_points, + const geometry_msgs::msg::Pose & ego_pose) const { - stop_watch_.tic(__func__); - - const auto & p = planner_data; + time_keeper_ptr_->tic(__func__); - if (skip_optimization_) { - const auto traj = points_utils::convertToTrajectoryPoints(p.path.points); - Trajectories trajs; - trajs.smoothed_trajectory = traj; - trajs.model_predictive_trajectory = traj; - return trajs; - } + // crop forward for faster calculation + const auto forward_cropped_input_traj_points = [&]() { + const double optimized_traj_length = mpt_optimizer_ptr_->getTrajectoryLength(); + constexpr double margin_traj_length = 10.0; - // EB: smooth trajectory if enable_pre_smoothing is true - const auto eb_traj = [&]() -> boost::optional> { - if (enable_pre_smoothing_) { - return eb_path_optimizer_ptr_->getEBTrajectory( - p.ego_pose, p.path, prev_optimal_trajs_ptr_, p.ego_vel, debug_data_); - } - return points_utils::convertToTrajectoryPoints(p.path.points); + const size_t ego_seg_idx = + trajectory_utils::findEgoSegmentIndex(input_traj_points, ego_pose, ego_nearest_param_); + return trajectory_utils::cropForwardPoints( + input_traj_points, ego_pose.position, ego_seg_idx, + optimized_traj_length + margin_traj_length); }(); - if (!eb_traj) { - return getPrevTrajs(p.path.points); - } - // NOTE: Elastic band sometimes diverges with status = "OSQP_SOLVED". - constexpr double max_path_change_diff = 1.0e4; - for (size_t i = 0; i < eb_traj->size(); ++i) { - const auto & eb_pos = eb_traj->at(i).pose.position; - const auto & path_pos = p.path.points.at(std::min(i, p.path.points.size() - 1)).pose.position; + // update velocity + size_t input_traj_start_idx = 0; + for (size_t i = 0; i < output_traj_points.size(); i++) { + // crop backward for efficient calculation + const auto cropped_input_traj_points = std::vector{ + forward_cropped_input_traj_points.begin() + input_traj_start_idx, + forward_cropped_input_traj_points.end()}; - const double diff_x = eb_pos.x - path_pos.x; - const double diff_y = eb_pos.y - path_pos.y; - if (max_path_change_diff < std::abs(diff_x) || max_path_change_diff < std::abs(diff_y)) { - return getPrevTrajs(p.path.points); - } - } + const size_t nearest_seg_idx = trajectory_utils::findEgoSegmentIndex( + cropped_input_traj_points, output_traj_points.at(i).pose, ego_nearest_param_); + input_traj_start_idx = nearest_seg_idx; - // EB has to be solved twice before solving MPT with fixed points - // since the result of EB is likely to change with/without fixing (1st/2nd EB) - // that makes MPT fixing points worse. - if (eb_solved_count_ < 2) { - eb_solved_count_++; - - if (prev_optimal_trajs_ptr_) { - prev_optimal_trajs_ptr_->model_predictive_trajectory.clear(); - prev_optimal_trajs_ptr_->mpt_ref_points.clear(); - } + // calculate velocity with zero order hold + const double velocity = cropped_input_traj_points.at(nearest_seg_idx).longitudinal_velocity_mps; + output_traj_points.at(i).longitudinal_velocity_mps = velocity; } - // MPT: optimize trajectory to be kinematically feasible and collision free - const auto mpt_trajs = mpt_optimizer_ptr_->getModelPredictiveTrajectory( - enable_avoidance_, eb_traj.get(), p.path.points, p.path.left_bound, p.path.right_bound, - prev_optimal_trajs_ptr_, p.ego_pose, p.ego_vel, debug_data_); - if (!mpt_trajs) { - return getPrevTrajs(p.path.points); - } - - // make trajectories, which has all optimized trajectories information - Trajectories trajs; - trajs.smoothed_trajectory = eb_traj.get(); - trajs.mpt_ref_points = mpt_trajs.get().ref_points; - trajs.model_predictive_trajectory = mpt_trajs.get().mpt; - - // debug data - debug_data_.mpt_traj = mpt_trajs.get().mpt; - debug_data_.mpt_ref_traj = points_utils::convertToTrajectoryPoints(mpt_trajs.get().ref_points); - debug_data_.eb_traj = eb_traj.get(); - - debug_data_.msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) << " [ms]\n"; - return trajs; -} + // insert stop point explicitly + const auto stop_idx = motion_utils::searchZeroVelocityIndex(forward_cropped_input_traj_points); + if (stop_idx) { + const auto input_stop_pose = forward_cropped_input_traj_points.at(stop_idx.get()).pose; + const size_t stop_seg_idx = trajectory_utils::findEgoSegmentIndex( + output_traj_points, input_stop_pose, ego_nearest_param_); -Trajectories ObstacleAvoidancePlanner::getPrevTrajs( - const std::vector & path_points) const -{ - if (prev_optimal_trajs_ptr_) { - return *prev_optimal_trajs_ptr_; + // calculate and insert stop pose on output trajectory + trajectory_utils::insertStopPoint(output_traj_points, input_stop_pose, stop_seg_idx); } - const auto traj = points_utils::convertToTrajectoryPoints(path_points); - Trajectories trajs; - trajs.smoothed_trajectory = traj; - trajs.model_predictive_trajectory = traj; - return trajs; -} - -void ObstacleAvoidancePlanner::calcVelocity( - const std::vector & path_points, std::vector & traj_points) const -{ - for (size_t i = 0; i < traj_points.size(); i++) { - const size_t nearest_seg_idx = findNearestSegmentIndexWithSoftYawConstraints( - path_points, traj_points.at(i).pose, traj_param_.delta_dist_threshold_for_closest_point, - traj_param_.delta_yaw_threshold_for_closest_point); - - // add this line not to exceed max index size - const size_t max_idx = std::min(nearest_seg_idx + 1, path_points.size() - 1); - // NOTE: std::max, not std::min, is used here since traj_points' sampling width may be longer - // than path_points' sampling width. A zero velocity point is guaranteed to be inserted in an - // output trajectory in the alignVelocity function - traj_points.at(i).longitudinal_velocity_mps = std::max( - path_points.at(nearest_seg_idx).longitudinal_velocity_mps, - path_points.at(max_idx).longitudinal_velocity_mps); - } + time_keeper_ptr_->toc(__func__, " "); } void ObstacleAvoidancePlanner::insertZeroVelocityOutsideDrivableArea( - const PlannerData & planner_data, std::vector & traj_points) + const PlannerData & planner_data, std::vector & optimized_traj_points) const { - if (traj_points.empty()) { + time_keeper_ptr_->tic(__func__); + + if (!enable_outside_drivable_area_stop_) { return; } - stop_watch_.tic(__func__); - - const size_t nearest_idx = findEgoNearestIndex(traj_points, planner_data.ego_pose); - - // NOTE: Some end trajectory points will be ignored to check if outside the drivable area - // since these points might be outside drivable area if only end reference points have high - // curvature. - const size_t end_idx = [&]() { - const size_t enough_long_points_num = - static_cast(traj_param_.num_sampling_points * 0.7); - constexpr size_t end_ignored_points_num = 5; - if (traj_points.size() > enough_long_points_num) { - return std::max(static_cast(0), traj_points.size() - end_ignored_points_num); - } - return traj_points.size(); - }(); - - const auto left_bound = planner_data.path.left_bound; - const auto right_bound = planner_data.path.right_bound; - if (left_bound.empty() || right_bound.empty()) { + if (optimized_traj_points.empty()) { return; } - for (size_t i = nearest_idx; i < end_idx; ++i) { - const auto & traj_point = traj_points.at(i); - - // calculate the first point being outside drivable area - const bool is_outside = drivable_area_utils::isOutsideDrivableAreaFromRectangleFootprint( - traj_point, left_bound, right_bound, vehicle_param_, is_considering_footprint_edges_); - - // only insert zero velocity to the point with longitudinal offset margin from the first point - // outside drivable area - if (is_outside) { - size_t stop_idx = i; - const auto & op_target_point = motion_utils::calcLongitudinalOffsetPoint( - traj_points, traj_point.pose.position, -1.0 * vehicle_stop_margin_outside_drivable_area_); - - if (op_target_point) { - const auto target_point = op_target_point.get(); - // confirm that target point doesn't overlap with the stop point outside drivable area - const auto dist = tier4_autoware_utils::calcDistance2d(traj_point, target_point); - const double overlap_threshold = 1e-3; - if (dist > overlap_threshold) { - stop_idx = motion_utils::findNearestSegmentIndex(traj_points, target_point); - } - } - - traj_points[stop_idx].longitudinal_velocity_mps = 0.0; - debug_data_.stop_pose_by_drivable_area = traj_points[stop_idx].pose; - - // NOTE: traj_points does not have valid z for efficient calculation of trajectory - if (!planner_data.path.points.empty()) { - const size_t path_idx = motion_utils::findNearestIndex( - planner_data.path.points, traj_points[stop_idx].pose.position); - - debug_data_.stop_pose_by_drivable_area->position.z = - planner_data.path.points.at(path_idx).pose.position.z; + // 1. calculate ego_index nearest to optimized_traj_points + const size_t ego_idx = trajectory_utils::findEgoIndex( + optimized_traj_points, planner_data.ego_pose, ego_nearest_param_); + + // 2. calculate an end point to check being outside the drivable area + // NOTE: Some terminal trajectory points tend to be outside the drivable area when + // they have high curvature. + // Therefore, these points should be ignored to check if being outside the drivable area + constexpr int num_points_ignore_drivable_area = 5; + const int end_idx = std::min( + static_cast(optimized_traj_points.size()) - 1, + mpt_optimizer_ptr_->getNumberOfPoints() - num_points_ignore_drivable_area); + + // 3. assign zero velocity to the first point being outside the drivable area + const auto first_outside_idx = [&]() -> std::optional { + for (size_t i = ego_idx; i < static_cast(end_idx); ++i) { + auto & traj_point = optimized_traj_points.at(i); + + // check if the footprint is outside the drivable area + const bool is_outside = geometry_utils::isOutsideDrivableAreaFromRectangleFootprint( + traj_point.pose, planner_data.left_bound, planner_data.right_bound, vehicle_info_, + use_footprint_polygon_for_outside_drivable_area_check_); + + if (is_outside) { + publishVirtualWall(traj_point.pose); + return i; } - break; } - } - - debug_data_.msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) << " [ms]\n"; -} - -void ObstacleAvoidancePlanner::publishDebugDataInOptimization( - const PlannerData & planner_data, const std::vector & traj_points) -{ - stop_watch_.tic(__func__); - - const auto & p = planner_data; - - { // publish trajectories - const auto debug_eb_traj = createTrajectory(debug_data_.eb_traj, p.path.header); - debug_eb_traj_pub_->publish(debug_eb_traj); - - const auto debug_mpt_fixed_traj = createTrajectory(debug_data_.mpt_fixed_traj, p.path.header); - debug_mpt_fixed_traj_pub_->publish(debug_mpt_fixed_traj); - - const auto debug_mpt_ref_traj = createTrajectory(debug_data_.mpt_ref_traj, p.path.header); - debug_mpt_ref_traj_pub_->publish(debug_mpt_ref_traj); - - const auto debug_mpt_traj = createTrajectory(debug_data_.mpt_traj, p.path.header); - debug_mpt_traj_pub_->publish(debug_mpt_traj); - } + return std::nullopt; + }(); - { // publish markers - if (is_publishing_debug_visualization_marker_) { - stop_watch_.tic("getDebugVisualizationMarker"); - const auto & debug_marker = - debug_utils::getDebugVisualizationMarker(debug_data_, traj_points, vehicle_param_, false); - debug_data_.msg_stream << " getDebugVisualizationMarker:= " - << stop_watch_.toc("getDebugVisualizationMarker") << " [ms]\n"; - - stop_watch_.tic("publishDebugVisualizationMarker"); - debug_markers_pub_->publish(debug_marker); - debug_data_.msg_stream << " publishDebugVisualizationMarker:= " - << stop_watch_.toc("publishDebugVisualizationMarker") << " [ms]\n"; - - stop_watch_.tic("getDebugVisualizationWallMarker"); - const auto & debug_wall_marker = - debug_utils::getDebugVisualizationWallMarker(debug_data_, vehicle_param_); - debug_data_.msg_stream << " getDebugVisualizationWallMarker:= " - << stop_watch_.toc("getDebugVisualizationWallMarker") << " [ms]\n"; - - stop_watch_.tic("publishDebugVisualizationWallMarker"); - debug_wall_markers_pub_->publish(debug_wall_marker); - debug_data_.msg_stream << " publishDebugVisualizationWallMarker:= " - << stop_watch_.toc("publishDebugVisualizationWallMarker") << " [ms]\n"; + if (first_outside_idx) { + for (size_t i = *first_outside_idx; i < optimized_traj_points.size(); ++i) { + optimized_traj_points.at(i).longitudinal_velocity_mps = 0.0; } } - debug_data_.msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) << " [ms]\n"; + time_keeper_ptr_->toc(__func__, " "); } -Trajectories ObstacleAvoidancePlanner::makePrevTrajectories( - const std::vector & path_points, const Trajectories & trajs, - const PlannerData & planner_data) +void ObstacleAvoidancePlanner::publishVirtualWall(const geometry_msgs::msg::Pose & stop_pose) const { - stop_watch_.tic(__func__); + time_keeper_ptr_->tic(__func__); - const auto post_processed_smoothed_traj = - generatePostProcessedTrajectory(path_points, trajs.smoothed_trajectory, planner_data); + const auto virtual_wall_marker = motion_utils::createStopVirtualWallMarker( + stop_pose, "outside drivable area", now(), 0, vehicle_info_.max_longitudinal_offset_m); - // TODO(murooka) generatePoseProcessedTrajectory may be too large - Trajectories trajectories; - trajectories.smoothed_trajectory = post_processed_smoothed_traj; - trajectories.mpt_ref_points = trajs.mpt_ref_points; - trajectories.model_predictive_trajectory = trajs.model_predictive_trajectory; - - debug_data_.msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) << " [ms]\n"; - - return trajectories; + virtual_wall_pub_->publish(virtual_wall_marker); + time_keeper_ptr_->toc(__func__, " "); } -std::vector ObstacleAvoidancePlanner::generatePostProcessedTrajectory( - const std::vector & path_points, - const std::vector & optimized_traj_points, const PlannerData & planner_data) +void ObstacleAvoidancePlanner::publishDebugMarkerOfOptimization( + const std::vector & traj_points) const { - stop_watch_.tic(__func__); - - std::vector trajectory_points; - if (path_points.empty()) { - TrajectoryPoint tmp_point; - tmp_point.pose = planner_data.ego_pose; - tmp_point.longitudinal_velocity_mps = 0.0; - trajectory_points.push_back(tmp_point); - return trajectory_points; - } - if (optimized_traj_points.empty()) { - trajectory_points = points_utils::convertToTrajectoryPoints(path_points); - return trajectory_points; + if (!enable_pub_debug_marker_) { + return; } - // calculate extended trajectory that connects to optimized trajectory smoothly - const auto extended_traj_points = getExtendedTrajectory(path_points, optimized_traj_points); + time_keeper_ptr_->tic(__func__); - // concat trajectories - const auto full_traj_points = - points_utils::concatTrajectory(optimized_traj_points, extended_traj_points); + // debug marker + time_keeper_ptr_->tic("getDebugMarker"); + const auto debug_marker = getDebugMarker(*debug_data_ptr_, traj_points, vehicle_info_); + time_keeper_ptr_->toc("getDebugMarker", " "); - // NOTE: fine_traj_points has no velocity information - const auto fine_traj_points = generateFineTrajectoryPoints(path_points, full_traj_points); + time_keeper_ptr_->tic("publishDebugMarker"); + debug_markers_pub_->publish(debug_marker); + time_keeper_ptr_->toc("publishDebugMarker", " "); - const auto fine_traj_points_with_vel = - alignVelocity(fine_traj_points, path_points, full_traj_points); - - debug_data_.msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) << " [ms]\n"; - return fine_traj_points_with_vel; + time_keeper_ptr_->toc(__func__, " "); } -std::vector ObstacleAvoidancePlanner::getExtendedTrajectory( - const std::vector & path_points, const std::vector & optimized_points) +std::vector ObstacleAvoidancePlanner::extendTrajectory( + const std::vector & traj_points, + const std::vector & optimized_traj_points) const { - stop_watch_.tic(__func__); - - assert(!path_points.empty()); + time_keeper_ptr_->tic(__func__); - const double accum_arc_length = motion_utils::calcArcLength(optimized_points); - if (accum_arc_length > traj_param_.trajectory_length) { - RCLCPP_INFO_THROTTLE( - this->get_logger(), *this->get_clock(), std::chrono::milliseconds(10000).count(), - "[Avoidance] Not extend trajectory"); - return std::vector{}; - } + const auto & joint_start_pose = optimized_traj_points.back().pose; // calculate end idx of optimized points on path points - const auto opt_end_path_idx = motion_utils::findNearestIndex( - path_points, optimized_points.back().pose, std::numeric_limits::max(), - traj_param_.delta_yaw_threshold_for_closest_point); - if (!opt_end_path_idx) { - RCLCPP_INFO_THROTTLE( - this->get_logger(), *this->get_clock(), std::chrono::milliseconds(10000).count(), - "[Avoidance] Not extend trajectory since could not find nearest idx from last opt point"); - return std::vector{}; - } - - auto extended_traj_points = [&]() -> std::vector { - const size_t non_fixed_begin_path_idx = opt_end_path_idx.get(); - const size_t non_fixed_end_path_idx = points_utils::findForwardIndex( - path_points, non_fixed_begin_path_idx, traj_param_.non_fixed_trajectory_length); - - if ( - non_fixed_begin_path_idx == path_points.size() - 1 || - non_fixed_begin_path_idx == non_fixed_end_path_idx) { - if (points_utils::isNearLastPathPoint( - optimized_points.back(), path_points, traj_param_.max_dist_for_extending_end_point, - traj_param_.delta_yaw_threshold_for_closest_point)) { - return std::vector{}; - } - const auto last_traj_point = points_utils::convertToTrajectoryPoint(path_points.back()); - return std::vector{last_traj_point}; - } else if (non_fixed_end_path_idx == path_points.size() - 1) { - // no need to connect smoothly since extended trajectory length is short enough - const auto last_traj_point = points_utils::convertToTrajectoryPoint(path_points.back()); - return std::vector{last_traj_point}; + const size_t joint_start_traj_seg_idx = + trajectory_utils::findEgoSegmentIndex(traj_points, joint_start_pose, ego_nearest_param_); + + // crop trajectory for extension + constexpr double joint_traj_length_for_smoothing = 5.0; + const auto joint_end_traj_point_idx = trajectory_utils::getPointIndexAfter( + traj_points, joint_start_pose.position, joint_start_traj_seg_idx, + joint_traj_length_for_smoothing); + + // calculate full trajectory points + const auto full_traj_points = [&]() { + if (!joint_end_traj_point_idx) { + return optimized_traj_points; } - - // define non_fixed/fixed_traj_points - const auto begin_point = optimized_points.back(); - const auto end_point = - points_utils::convertToTrajectoryPoint(path_points.at(non_fixed_end_path_idx)); - const std::vector non_fixed_traj_points{begin_point, end_point}; - const std::vector fixed_path_points{ - path_points.begin() + non_fixed_end_path_idx + 1, path_points.end()}; - const auto fixed_traj_points = points_utils::convertToTrajectoryPoints(fixed_path_points); - - // spline interpolation to two traj points with end diff constraints - const double begin_yaw = tf2::getYaw(begin_point.pose.orientation); - const double end_yaw = tf2::getYaw(end_point.pose.orientation); - const auto interpolated_non_fixed_traj_points = - interpolation_utils::getConnectedInterpolatedPoints( - non_fixed_traj_points, eb_param_.delta_arc_length_for_eb, begin_yaw, end_yaw); - - // concat interpolated_non_fixed and fixed traj points - auto extended_points = interpolated_non_fixed_traj_points; - extended_points.insert( - extended_points.end(), fixed_traj_points.begin(), fixed_traj_points.end()); - - debug_data_.extended_non_fixed_traj = interpolated_non_fixed_traj_points; - debug_data_.extended_fixed_traj = fixed_traj_points; - - return extended_points; + const auto extended_traj_points = std::vector{ + traj_points.begin() + *joint_end_traj_point_idx, traj_points.end()}; + return concatVectors(optimized_traj_points, extended_traj_points); }(); - // NOTE: Extended points will be concatenated with optimized points. - // Then, minimum velocity of each point is chosen from concatenated points or path points - // since optimized points has zero velocity outside drivable area - constexpr double large_velocity = 1e4; - for (auto & point : extended_traj_points) { - point.longitudinal_velocity_mps = large_velocity; - } - - debug_data_.msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) << " [ms]\n"; - return extended_traj_points; -} - -std::vector ObstacleAvoidancePlanner::generateFineTrajectoryPoints( - const std::vector & path_points, - const std::vector & traj_points) const -{ - stop_watch_.tic(__func__); - - // interpolate x and y - auto interpolated_traj_points = interpolation_utils::getInterpolatedTrajectoryPoints( - traj_points, traj_param_.delta_arc_length_for_trajectory); - - // calculate yaw from x and y - // NOTE: We do not use spline interpolation to yaw in behavior path since the yaw is unstable. - // Currently this implementation is removed since this calculation is heavy (~20ms) - // fillYawInTrajectoryPoint(interpolated_traj_points); - - // compensate last pose - points_utils::compensateLastPose( - path_points.back(), interpolated_traj_points, traj_param_.max_dist_for_extending_end_point, - traj_param_.delta_yaw_threshold_for_closest_point); - - debug_data_.msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) << " [ms]\n"; - - return interpolated_traj_points; -} - -std::vector ObstacleAvoidancePlanner::alignVelocity( - const std::vector & fine_traj_points, const std::vector & path_points, - const std::vector & traj_points) const -{ - stop_watch_.tic(__func__); - - // insert zero velocity path index, and get optional zero_vel_path_idx - const auto path_zero_vel_info = - [&]() -> std::pair, boost::optional> { - const auto opt_path_zero_vel_idx = motion_utils::searchZeroVelocityIndex(path_points); - if (opt_path_zero_vel_idx) { - const auto & zero_vel_path_point = path_points.at(opt_path_zero_vel_idx.get()); - const auto opt_traj_seg_idx = motion_utils::findNearestSegmentIndex( - fine_traj_points, zero_vel_path_point.pose, std::numeric_limits::max(), - traj_param_.delta_yaw_threshold_for_closest_point); - if (opt_traj_seg_idx) { - const auto interpolated_pose = - lerpPose(fine_traj_points, zero_vel_path_point.pose.position, opt_traj_seg_idx.get()); - if (interpolated_pose) { - TrajectoryPoint zero_vel_traj_point; - zero_vel_traj_point.pose = interpolated_pose.get(); - zero_vel_traj_point.longitudinal_velocity_mps = - zero_vel_path_point.longitudinal_velocity_mps; - - if ( - tier4_autoware_utils::calcDistance2d( - fine_traj_points.at(opt_traj_seg_idx.get()).pose, zero_vel_traj_point.pose) < 1e-3) { - return {fine_traj_points, opt_traj_seg_idx.get()}; - } else if ( - tier4_autoware_utils::calcDistance2d( - fine_traj_points.at(opt_traj_seg_idx.get() + 1).pose, zero_vel_traj_point.pose) < - 1e-3) { - return {fine_traj_points, opt_traj_seg_idx.get() + 1}; - } - - auto fine_traj_points_with_zero_vel = fine_traj_points; - fine_traj_points_with_zero_vel.insert( - fine_traj_points_with_zero_vel.begin() + opt_traj_seg_idx.get() + 1, - zero_vel_traj_point); - return {fine_traj_points_with_zero_vel, opt_traj_seg_idx.get() + 1}; - } + // resample trajectory points + auto resampled_traj_points = trajectory_utils::resampleTrajectoryPoints( + full_traj_points, traj_param_.output_delta_arc_length); + + // update velocity on joint + for (size_t i = joint_start_traj_seg_idx + 1; i <= joint_end_traj_point_idx; ++i) { + if (hasZeroVelocity(traj_points.at(i))) { + if (i != 0 && !hasZeroVelocity(traj_points.at(i - 1))) { + // Here is when current point is 0 velocity, but previous point is not 0 velocity. + const auto & input_stop_pose = traj_points.at(i).pose; + const size_t stop_seg_idx = trajectory_utils::findEgoSegmentIndex( + resampled_traj_points, input_stop_pose, ego_nearest_param_); + + // calculate and insert stop pose on output trajectory + trajectory_utils::insertStopPoint(resampled_traj_points, input_stop_pose, stop_seg_idx); } } - - return {fine_traj_points, {}}; - }(); - const auto fine_traj_points_with_path_zero_vel = path_zero_vel_info.first; - const auto opt_zero_vel_path_idx = path_zero_vel_info.second; - - // search zero velocity index of fine_traj_points - const size_t zero_vel_fine_traj_idx = [&]() { - // zero velocity for being outside drivable area - const size_t zero_vel_traj_idx = searchExtendedZeroVelocityIndex( - fine_traj_points_with_path_zero_vel, traj_points, - traj_param_.delta_yaw_threshold_for_closest_point); - - // zero velocity in path points - if (opt_zero_vel_path_idx) { - return std::min(opt_zero_vel_path_idx.get(), zero_vel_traj_idx); - } - return zero_vel_traj_idx; - }(); - - // interpolate z and velocity - auto fine_traj_points_with_vel = fine_traj_points_with_path_zero_vel; - size_t prev_begin_idx = 0; - for (size_t i = 0; i < fine_traj_points_with_vel.size(); ++i) { - auto truncated_points = points_utils::clipForwardPoints(path_points, prev_begin_idx, 5.0); - if (truncated_points.size() < 2) { - // NOTE: At least, two points must be contained in truncated_points - truncated_points = std::vector( - path_points.begin() + prev_begin_idx, - path_points.begin() + std::min(path_points.size(), prev_begin_idx + 2)); - } - - const auto & target_pose = fine_traj_points_with_vel[i].pose; - const size_t closest_seg_idx = findNearestSegmentIndexWithSoftYawConstraints( - truncated_points, target_pose, traj_param_.delta_dist_threshold_for_closest_point, - traj_param_.delta_yaw_threshold_for_closest_point); - - // lerp z - fine_traj_points_with_vel[i].pose.position.z = - lerpPoseZ(truncated_points, target_pose.position, closest_seg_idx); - - // lerp vx - const double target_vel = lerpTwistX(truncated_points, target_pose.position, closest_seg_idx); - - if (i >= zero_vel_fine_traj_idx) { - fine_traj_points_with_vel[i].longitudinal_velocity_mps = 0.0; - } else { - fine_traj_points_with_vel[i].longitudinal_velocity_mps = target_vel; - } - - // NOTE: closest_seg_idx is for the clipped trajectory. This operation must be "+=". - prev_begin_idx += closest_seg_idx; } - debug_data_.msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) << " [ms]\n"; - return fine_traj_points_with_vel; + // debug_data_ptr_->extended_traj_points = + // extended_traj_points ? *extended_traj_points : std::vector(); + time_keeper_ptr_->toc(__func__, " "); + return resampled_traj_points; } -void ObstacleAvoidancePlanner::publishDebugDataInMain(const Path & path) const +void ObstacleAvoidancePlanner::publishDebugData(const Header & header) const { - stop_watch_.tic(__func__); - - { // publish trajectories - const auto debug_extended_fixed_traj = - createTrajectory(debug_data_.extended_fixed_traj, path.header); - debug_extended_fixed_traj_pub_->publish(debug_extended_fixed_traj); + time_keeper_ptr_->tic(__func__); - const auto debug_extended_non_fixed_traj = - createTrajectory(debug_data_.extended_non_fixed_traj, path.header); - debug_extended_non_fixed_traj_pub_->publish(debug_extended_non_fixed_traj); - } + // publish trajectories + const auto debug_extended_traj = + trajectory_utils::createTrajectory(header, debug_data_ptr_->extended_traj_points); + debug_extended_traj_pub_->publish(debug_extended_traj); - debug_data_.msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) << " [ms]\n"; + time_keeper_ptr_->toc(__func__, " "); } +} // namespace obstacle_avoidance_planner #include "rclcpp_components/register_node_macro.hpp" -RCLCPP_COMPONENTS_REGISTER_NODE(ObstacleAvoidancePlanner) +RCLCPP_COMPONENTS_REGISTER_NODE(obstacle_avoidance_planner::ObstacleAvoidancePlanner) diff --git a/planning/obstacle_avoidance_planner/src/replan_checker.cpp b/planning/obstacle_avoidance_planner/src/replan_checker.cpp new file mode 100644 index 0000000000000..42ebab7f14e5d --- /dev/null +++ b/planning/obstacle_avoidance_planner/src/replan_checker.cpp @@ -0,0 +1,158 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "obstacle_avoidance_planner/replan_checker.hpp" + +#include "motion_utils/motion_utils.hpp" +#include "obstacle_avoidance_planner/utils/trajectory_utils.hpp" +#include "tier4_autoware_utils/tier4_autoware_utils.hpp" + +#include + +namespace obstacle_avoidance_planner +{ +ReplanChecker::ReplanChecker(rclcpp::Node * node, const EgoNearestParam & ego_nearest_param) +: ego_nearest_param_(ego_nearest_param), logger_(node->get_logger().get_child("replan_checker")) +{ + max_path_shape_around_ego_lat_dist_ = + node->declare_parameter("replan.max_path_shape_around_ego_lat_dist"); + max_ego_moving_dist_ = node->declare_parameter("replan.max_ego_moving_dist"); + max_goal_moving_dist_ = node->declare_parameter("replan.max_goal_moving_dist"); + max_delta_time_sec_ = node->declare_parameter("replan.max_delta_time_sec"); +} + +void ReplanChecker::onParam(const std::vector & parameters) +{ + using tier4_autoware_utils::updateParam; + + updateParam( + parameters, "replan.max_path_shape_around_ego_lat_dist", max_path_shape_around_ego_lat_dist_); + updateParam(parameters, "replan.max_ego_moving_dist", max_ego_moving_dist_); + updateParam(parameters, "replan.max_goal_moving_dist", max_goal_moving_dist_); + updateParam(parameters, "replan.max_delta_time_sec", max_delta_time_sec_); +} + +bool ReplanChecker::isResetRequired(const PlannerData & planner_data) +{ + const auto & p = planner_data; + + const bool reset_required = [&]() { + // guard for invalid variables + if (!prev_traj_points_ptr_ || !prev_ego_pose_ptr_) { + return true; + } + const auto & prev_traj_points = *prev_traj_points_ptr_; + + // path shape changes + if (isPathAroundEgoChanged(planner_data, prev_traj_points)) { + RCLCPP_INFO(logger_, "Replan with resetting optimization since path shape changed."); + return true; + } + + // path goal changes + if (isPathGoalChanged(planner_data, prev_traj_points)) { + RCLCPP_INFO(logger_, "Replan with resetting optimization since path goal changed."); + return true; + } + + // ego pose is lost or new ego pose is designated in simulation + const double delta_dist = + tier4_autoware_utils::calcDistance2d(p.ego_pose, prev_ego_pose_ptr_->position); + if (max_ego_moving_dist_ < delta_dist) { + RCLCPP_INFO( + logger_, + "Replan with resetting optimization since current ego pose is far from previous ego pose."); + return true; + } + + return false; + }(); + + // update previous information required in this function + prev_traj_points_ptr_ = std::make_shared>(p.traj_points); + prev_ego_pose_ptr_ = std::make_shared(p.ego_pose); + + return reset_required; +} + +bool ReplanChecker::isReplanRequired(const rclcpp::Time & current_time) +{ + const bool replan_required = [&]() { + // guard for invalid variables + if (!prev_replanned_time_ptr_ || !prev_traj_points_ptr_) { + return true; + } + + // time elapses + const double delta_time_sec = (current_time - *prev_replanned_time_ptr_).seconds(); + if (max_delta_time_sec_ < delta_time_sec) { + return true; + } + + return false; + }(); + + // update previous information required in this function + if (replan_required) { + prev_replanned_time_ptr_ = std::make_shared(current_time); + } + + return replan_required; +} + +bool ReplanChecker::isPathAroundEgoChanged( + const PlannerData & planner_data, const std::vector & prev_traj_points) const +{ + const auto & p = planner_data; + + // calculate ego's lateral offset to previous trajectory points + const auto prev_ego_seg_idx = + trajectory_utils::findEgoSegmentIndex(prev_traj_points, p.ego_pose, ego_nearest_param_); + const double prev_ego_lat_offset = + motion_utils::calcLateralOffset(prev_traj_points, p.ego_pose.position, prev_ego_seg_idx); + + // calculate ego's lateral offset to current trajectory points + const auto ego_seg_idx = + trajectory_utils::findEgoSegmentIndex(p.traj_points, p.ego_pose, ego_nearest_param_); + const double ego_lat_offset = + motion_utils::calcLateralOffset(p.traj_points, p.ego_pose.position, ego_seg_idx); + + const double diff_ego_lat_offset = prev_ego_lat_offset - ego_lat_offset; + if (std::abs(diff_ego_lat_offset) < max_path_shape_around_ego_lat_dist_) { + return false; + } + + return true; +} + +bool ReplanChecker::isPathGoalChanged( + const PlannerData & planner_data, const std::vector & prev_traj_points) const +{ + const auto & p = planner_data; + + // check if the vehicle is stopping + constexpr double min_vel = 1e-3; + if (min_vel < std::abs(p.ego_vel)) { + return false; + } + + const double goal_moving_dist = + tier4_autoware_utils::calcDistance2d(p.traj_points.back(), prev_traj_points.back()); + if (goal_moving_dist < max_goal_moving_dist_) { + return false; + } + + return true; +} +} // namespace obstacle_avoidance_planner diff --git a/planning/obstacle_avoidance_planner/src/state_equation_generator.cpp b/planning/obstacle_avoidance_planner/src/state_equation_generator.cpp new file mode 100644 index 0000000000000..a6eaf7ffdd2ac --- /dev/null +++ b/planning/obstacle_avoidance_planner/src/state_equation_generator.cpp @@ -0,0 +1,87 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "obstacle_avoidance_planner/state_equation_generator.hpp" + +#include "obstacle_avoidance_planner/mpt_optimizer.hpp" + +namespace obstacle_avoidance_planner +{ +// state equation: x = B u + W (u includes x_0) +// NOTE: Originally, x_t+1 = Ad x_t + Bd u + Wd. +StateEquationGenerator::Matrix StateEquationGenerator::calcMatrix( + const std::vector & ref_points) const +{ + time_keeper_ptr_->tic(__func__); + + const size_t N_ref = ref_points.size(); + const size_t D_x = vehicle_model_ptr_->getDimX(); + const size_t D_u = vehicle_model_ptr_->getDimU(); + const size_t D_v = D_x + D_u * (N_ref - 1); + + // matrices for whole state equation + Eigen::MatrixXd B = Eigen::MatrixXd::Zero(D_x * N_ref, D_v); + Eigen::VectorXd W = Eigen::VectorXd::Zero(D_x * N_ref); + + // matrices for one-step state equation + Eigen::MatrixXd Ad(D_x, D_x); + Eigen::MatrixXd Bd(D_x, D_u); + Eigen::MatrixXd Wd(D_x, 1); + + // calculate one-step state equation considering kinematics N_ref times + for (size_t i = 0; i < N_ref; ++i) { + if (i == 0) { + B.block(0, 0, D_x, D_x) = Eigen::MatrixXd::Identity(D_x, D_x); + continue; + } + + const int idx_x_i = i * D_x; + const int idx_x_i_prev = (i - 1) * D_x; + const int idx_u_i_prev = (i - 1) * D_u; + + // get discrete kinematics matrix A, B, W + const auto & p = ref_points.at(i - 1); + + // TODO(murooka) use curvature by stabling optimization + // Currently, when using curvature, the optimization result is weird with sample_map. + // vehicle_model_ptr_->calculateStateEquationMatrix(Ad, Bd, Wd, p.curvature, + // p.delta_arc_length); + vehicle_model_ptr_->calculateStateEquationMatrix(Ad, Bd, Wd, 0.0, p.delta_arc_length); + + B.block(idx_x_i, 0, D_x, D_x) = Ad * B.block(idx_x_i_prev, 0, D_x, D_x); + B.block(idx_x_i, D_x + idx_u_i_prev, D_x, D_u) = Bd; + + for (size_t j = 0; j < i - 1; ++j) { + size_t idx_u_j = j * D_u; + B.block(idx_x_i, D_x + idx_u_j, D_x, D_u) = + Ad * B.block(idx_x_i_prev, D_x + idx_u_j, D_x, D_u); + } + + W.segment(idx_x_i, D_x) = Ad * W.block(idx_x_i_prev, 0, D_x, 1) + Wd; + } + + Matrix mat; + mat.B = B; + mat.W = W; + + time_keeper_ptr_->toc(__func__, " "); + return mat; +} + +Eigen::VectorXd StateEquationGenerator::predict( + const StateEquationGenerator::Matrix & mat, const Eigen::VectorXd U) const +{ + return mat.B * U + mat.W; +} +} // namespace obstacle_avoidance_planner diff --git a/planning/obstacle_avoidance_planner/src/utils/debug_utils.cpp b/planning/obstacle_avoidance_planner/src/utils/debug_utils.cpp deleted file mode 100644 index eb689301dd88a..0000000000000 --- a/planning/obstacle_avoidance_planner/src/utils/debug_utils.cpp +++ /dev/null @@ -1,858 +0,0 @@ -// Copyright 2020 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "obstacle_avoidance_planner/utils/debug_utils.hpp" - -#include "obstacle_avoidance_planner/mpt_optimizer.hpp" -#include "obstacle_avoidance_planner/utils/utils.hpp" -#include "tf2/utils.h" - -#include "visualization_msgs/msg/marker.hpp" -#include "visualization_msgs/msg/marker_array.hpp" - -#include -#include - -using tier4_autoware_utils::appendMarkerArray; -using tier4_autoware_utils::createDefaultMarker; -using tier4_autoware_utils::createMarkerColor; -using tier4_autoware_utils::createMarkerScale; - -namespace -{ -template -visualization_msgs::msg::MarkerArray getPointsMarkerArray( - const std::vector & points, const std::string & ns, const double r, const double g, - const double b) -{ - if (points.empty()) { - return visualization_msgs::msg::MarkerArray{}; - } - - auto marker = createDefaultMarker( - "map", rclcpp::Clock().now(), ns, 0, visualization_msgs::msg::Marker::LINE_LIST, - createMarkerScale(0.5, 0.5, 0.5), createMarkerColor(r, g, b, 0.99)); - marker.lifetime = rclcpp::Duration::from_seconds(1.0); - - for (const auto & point : points) { - marker.points.push_back(tier4_autoware_utils::getPoint(point)); - } - - visualization_msgs::msg::MarkerArray msg; - msg.markers.push_back(marker); - - return msg; -} - -template -visualization_msgs::msg::MarkerArray getPointsTextMarkerArray( - const std::vector & points, const std::string & ns, const double r, const double g, - const double b) -{ - if (points.empty()) { - return visualization_msgs::msg::MarkerArray{}; - } - - auto marker = createDefaultMarker( - "map", rclcpp::Clock().now(), ns, 0, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, - createMarkerScale(0.0, 0.0, 0.15), createMarkerColor(r, g, b, 0.99)); - marker.lifetime = rclcpp::Duration::from_seconds(1.5); - - visualization_msgs::msg::MarkerArray msg; - for (size_t i = 0; i < points.size(); i++) { - marker.id = i; - marker.text = std::to_string(i); - marker.pose.position = tier4_autoware_utils::getPoint(points[i]); - msg.markers.push_back(marker); - } - - return msg; -} - -geometry_msgs::msg::Pose getVirtualWallPose( - const geometry_msgs::msg::Pose & target_pose, const VehicleParam & vehicle_param) -{ - const double base_link2front = vehicle_param.wheelbase + vehicle_param.front_overhang; - tf2::Transform tf_base_link2front( - tf2::Quaternion(0.0, 0.0, 0.0, 1.0), tf2::Vector3(base_link2front, 0.0, 0.0)); - tf2::Transform tf_map2base_link; - tf2::fromMsg(target_pose, tf_map2base_link); - tf2::Transform tf_map2front = tf_map2base_link * tf_base_link2front; - geometry_msgs::msg::Pose virtual_wall_pose; - tf2::toMsg(tf_map2front, virtual_wall_pose); - return virtual_wall_pose; -} - -visualization_msgs::msg::MarkerArray getDebugPointsMarkers( - const std::vector & interpolated_points, - const std::vector & optimized_points, - const std::vector & straight_points, - const std::vector & fixed_points, - const std::vector & non_fixed_points) -{ - visualization_msgs::msg::MarkerArray marker_array; - int unique_id = 0; - - unique_id = 0; - visualization_msgs::msg::Marker interpolated_points_marker; - interpolated_points_marker.lifetime = rclcpp::Duration::from_seconds(0); - interpolated_points_marker.header.frame_id = "map"; - interpolated_points_marker.header.stamp = rclcpp::Time(0); - interpolated_points_marker.ns = std::string("interpolated_points_marker"); - interpolated_points_marker.action = visualization_msgs::msg::Marker::ADD; - interpolated_points_marker.pose.orientation.w = 1.0; - interpolated_points_marker.id = unique_id; - interpolated_points_marker.type = visualization_msgs::msg::Marker::SPHERE_LIST; - interpolated_points_marker.scale = createMarkerScale(.3, .3, .3); - interpolated_points_marker.color = createMarkerColor(1.0f, 0, 0, 0.8); - unique_id++; - for (const auto & point : interpolated_points) { - interpolated_points_marker.points.push_back(point); - } - if (!interpolated_points_marker.points.empty()) { - marker_array.markers.push_back(interpolated_points_marker); - } - - unique_id = 0; - visualization_msgs::msg::Marker optimized_points_marker; - optimized_points_marker.lifetime = rclcpp::Duration::from_seconds(0); - optimized_points_marker.header.frame_id = "map"; - optimized_points_marker.header.stamp = rclcpp::Time(0); - optimized_points_marker.ns = std::string("optimized_points_marker"); - optimized_points_marker.action = visualization_msgs::msg::Marker::ADD; - optimized_points_marker.pose.orientation.w = 1.0; - optimized_points_marker.id = unique_id; - optimized_points_marker.type = visualization_msgs::msg::Marker::SPHERE_LIST; - optimized_points_marker.scale = createMarkerScale(0.35, 0.35, 0.35); - optimized_points_marker.color = createMarkerColor(0, 1.0f, 0, 0.99); - unique_id++; - for (const auto & point : optimized_points) { - optimized_points_marker.points.push_back(point.pose.position); - } - if (!optimized_points_marker.points.empty()) { - marker_array.markers.push_back(optimized_points_marker); - } - - unique_id = 0; - for (size_t i = 0; i < optimized_points.size(); i++) { - visualization_msgs::msg::Marker optimized_points_text_marker; - optimized_points_text_marker.lifetime = rclcpp::Duration::from_seconds(0); - optimized_points_text_marker.header.frame_id = "map"; - optimized_points_text_marker.header.stamp = rclcpp::Time(0); - optimized_points_text_marker.ns = std::string("optimized_points_text_marker"); - optimized_points_text_marker.action = visualization_msgs::msg::Marker::ADD; - optimized_points_text_marker.pose.orientation.w = 1.0; - optimized_points_text_marker.id = unique_id; - optimized_points_text_marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; - optimized_points_text_marker.pose.position = optimized_points[i].pose.position; - optimized_points_text_marker.scale = createMarkerScale(0, 0, 0.15); - optimized_points_text_marker.color = createMarkerColor(0, 1.0, 0, 0.99); - optimized_points_text_marker.text = std::to_string(i); - unique_id++; - marker_array.markers.push_back(optimized_points_text_marker); - } - - unique_id = 0; - for (size_t i = 0; i < interpolated_points.size(); i++) { - visualization_msgs::msg::Marker interpolated_points_text_marker; - interpolated_points_text_marker.lifetime = rclcpp::Duration::from_seconds(0); - interpolated_points_text_marker.header.frame_id = "map"; - interpolated_points_text_marker.header.stamp = rclcpp::Time(0); - interpolated_points_text_marker.ns = std::string("interpolated_points_text_marker"); - interpolated_points_text_marker.action = visualization_msgs::msg::Marker::ADD; - interpolated_points_text_marker.pose.orientation.w = 1.0; - interpolated_points_text_marker.id = unique_id; - interpolated_points_text_marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; - interpolated_points_text_marker.pose.position = interpolated_points[i]; - interpolated_points_text_marker.scale = createMarkerScale(0, 0, 0.5); - interpolated_points_text_marker.color = createMarkerColor(0, 1.0, 0, 0.99); - interpolated_points_text_marker.text = std::to_string(i); - unique_id++; - marker_array.markers.push_back(interpolated_points_text_marker); - } - - unique_id = 0; - visualization_msgs::msg::Marker straight_points_marker; - straight_points_marker.lifetime = rclcpp::Duration::from_seconds(40); - straight_points_marker.header.frame_id = "map"; - straight_points_marker.header.stamp = rclcpp::Time(0); - straight_points_marker.ns = std::string("straight_points_marker"); - straight_points_marker.action = visualization_msgs::msg::Marker::ADD; - straight_points_marker.pose.orientation.w = 1.0; - straight_points_marker.id = unique_id; - straight_points_marker.type = visualization_msgs::msg::Marker::SPHERE_LIST; - straight_points_marker.scale = createMarkerScale(1.0, 1.0, 1.0); - straight_points_marker.color = createMarkerColor(1.0, 1.0, 0, 0.99); - unique_id++; - for (const auto & point : straight_points) { - straight_points_marker.points.push_back(point); - } - if (!straight_points_marker.points.empty()) { - marker_array.markers.push_back(straight_points_marker); - } - - unique_id = 0; - visualization_msgs::msg::Marker fixed_marker; - fixed_marker.lifetime = rclcpp::Duration::from_seconds(0); - fixed_marker.header.frame_id = "map"; - fixed_marker.header.stamp = rclcpp::Time(0); - fixed_marker.ns = std::string("fixed_points_marker"); - fixed_marker.action = visualization_msgs::msg::Marker::ADD; - fixed_marker.pose.orientation.w = 1.0; - fixed_marker.id = unique_id; - fixed_marker.type = visualization_msgs::msg::Marker::SPHERE_LIST; - fixed_marker.scale = createMarkerScale(0.3, 0.3, 0.3); - fixed_marker.color = createMarkerColor(1.0, 0, 0, 0.99); - unique_id++; - for (const auto & point : fixed_points) { - fixed_marker.points.push_back(point.position); - } - if (!fixed_marker.points.empty()) { - marker_array.markers.push_back(fixed_marker); - } - - visualization_msgs::msg::Marker non_fixed_marker; - non_fixed_marker.lifetime = rclcpp::Duration::from_seconds(20); - non_fixed_marker.header.frame_id = "map"; - non_fixed_marker.header.stamp = rclcpp::Time(0); - non_fixed_marker.ns = std::string("non_fixed_points_marker"); - non_fixed_marker.action = visualization_msgs::msg::Marker::ADD; - non_fixed_marker.pose.orientation.w = 1.0; - non_fixed_marker.id = unique_id; - non_fixed_marker.type = visualization_msgs::msg::Marker::SPHERE_LIST; - non_fixed_marker.scale = createMarkerScale(1.0, 1.0, 1.0); - non_fixed_marker.color = createMarkerColor(0, 1.0, 0, 0.99); - unique_id++; - for (const auto & point : non_fixed_points) { - non_fixed_marker.points.push_back(point.position); - } - if (!non_fixed_marker.points.empty()) { - marker_array.markers.push_back(non_fixed_marker); - } - return marker_array; -} - -visualization_msgs::msg::MarkerArray getDebugConstrainMarkers( - const std::vector & constrain_ranges, const std::string & ns) -{ - visualization_msgs::msg::MarkerArray marker_array; - int unique_id = 0; - for (size_t i = 0; i < constrain_ranges.size(); i++) { - visualization_msgs::msg::Marker constrain_rect_marker; - constrain_rect_marker.lifetime = rclcpp::Duration::from_seconds(0); - constrain_rect_marker.header.frame_id = "map"; - constrain_rect_marker.header.stamp = rclcpp::Time(0); - constrain_rect_marker.ns = ns; - constrain_rect_marker.action = visualization_msgs::msg::Marker::ADD; - constrain_rect_marker.pose.orientation.w = 1.0; - constrain_rect_marker.id = unique_id; - constrain_rect_marker.type = visualization_msgs::msg::Marker::LINE_STRIP; - constrain_rect_marker.scale = createMarkerScale(0.01, 0, 0); - constrain_rect_marker.color = createMarkerColor(1.0, 0, 0, 0.99); - unique_id++; - geometry_msgs::msg::Point top_left_point = constrain_ranges[i].top_left; - geometry_msgs::msg::Point top_right_point = constrain_ranges[i].top_right; - geometry_msgs::msg::Point bottom_right_point = constrain_ranges[i].bottom_right; - geometry_msgs::msg::Point bottom_left_point = constrain_ranges[i].bottom_left; - constrain_rect_marker.points.push_back(top_left_point); - constrain_rect_marker.points.push_back(top_right_point); - constrain_rect_marker.points.push_back(bottom_right_point); - constrain_rect_marker.points.push_back(bottom_left_point); - constrain_rect_marker.points.push_back(top_left_point); - marker_array.markers.push_back(constrain_rect_marker); - } - - for (size_t i = 0; i < constrain_ranges.size(); i++) { - visualization_msgs::msg::Marker marker; - marker.header.frame_id = "map"; - marker.header.stamp = rclcpp::Time(0); - marker.ns = ns + "_text"; - marker.id = unique_id++; - marker.lifetime = rclcpp::Duration::from_seconds(0); - marker.action = visualization_msgs::msg::Marker::ADD; - marker.pose.orientation.w = 1.0; - marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; - marker.scale = createMarkerScale(0, 0, 0.15); - marker.color = createMarkerColor(1.0, 0, 0, 0.99); - marker.text = std::to_string(i); - marker.pose.position = constrain_ranges[i].top_left; - marker_array.markers.push_back(marker); - } - - unique_id = 0; - for (size_t i = 0; i < constrain_ranges.size(); i++) { - visualization_msgs::msg::Marker constrain_range_text_marker; - constrain_range_text_marker.lifetime = rclcpp::Duration::from_seconds(0); - constrain_range_text_marker.header.frame_id = "map"; - constrain_range_text_marker.header.stamp = rclcpp::Time(0); - constrain_range_text_marker.ns = ns + "_location"; - constrain_range_text_marker.action = visualization_msgs::msg::Marker::ADD; - constrain_range_text_marker.pose.orientation.w = 1.0; - constrain_range_text_marker.id = unique_id; - constrain_range_text_marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; - constrain_range_text_marker.pose.position = constrain_ranges[i].top_left; - constrain_range_text_marker.scale = createMarkerScale(0, 0, 0.1); - constrain_range_text_marker.color = createMarkerColor(1.0, 0, 0, 0.99); - constrain_range_text_marker.text = std::to_string(i) + std::string(" x ") + - std::to_string(constrain_range_text_marker.pose.position.x) + - std::string("y ") + - std::to_string(constrain_range_text_marker.pose.position.y); - unique_id++; - marker_array.markers.push_back(constrain_range_text_marker); - - constrain_range_text_marker.id = unique_id; - constrain_range_text_marker.pose.position = constrain_ranges[i].top_right; - constrain_range_text_marker.text = std::to_string(i) + std::string(" x ") + - std::to_string(constrain_range_text_marker.pose.position.x) + - std::string("y ") + - std::to_string(constrain_range_text_marker.pose.position.y); - unique_id++; - marker_array.markers.push_back(constrain_range_text_marker); - - constrain_range_text_marker.id = unique_id; - constrain_range_text_marker.pose.position = constrain_ranges[i].bottom_left; - constrain_range_text_marker.text = std::to_string(i) + std::string(" x ") + - std::to_string(constrain_range_text_marker.pose.position.x) + - std::string("y ") + - std::to_string(constrain_range_text_marker.pose.position.y); - unique_id++; - marker_array.markers.push_back(constrain_range_text_marker); - - constrain_range_text_marker.id = unique_id; - constrain_range_text_marker.pose.position = constrain_ranges[i].bottom_right; - constrain_range_text_marker.text = std::to_string(i) + std::string(" x ") + - std::to_string(constrain_range_text_marker.pose.position.x) + - std::string("y ") + - std::to_string(constrain_range_text_marker.pose.position.y); - unique_id++; - marker_array.markers.push_back(constrain_range_text_marker); - } - return marker_array; -} - -visualization_msgs::msg::MarkerArray getObjectsMarkerArray( - const std::vector & objects, - const std::string & ns, const double r, const double g, const double b) -{ - visualization_msgs::msg::MarkerArray msg; - - auto marker = createDefaultMarker( - "map", rclcpp::Clock().now(), ns, 0, visualization_msgs::msg::Marker::CUBE, - createMarkerScale(3.0, 1.0, 1.0), createMarkerColor(r, g, b, 0.8)); - marker.lifetime = rclcpp::Duration::from_seconds(1.5); - - for (size_t i = 0; i < objects.size(); ++i) { - const auto & object = objects.at(i); - marker.id = i; - marker.pose = object.kinematics.initial_pose_with_covariance.pose; - msg.markers.push_back(marker); - } - - return msg; -} - -visualization_msgs::msg::MarkerArray getRectanglesMarkerArray( - const std::vector mpt_traj, - const VehicleParam & vehicle_param, const std::string & ns, const double r, const double g, - const double b, const size_t sampling_num) -{ - visualization_msgs::msg::MarkerArray msg; - for (size_t i = 0; i < mpt_traj.size(); ++i) { - if (i % sampling_num != 0) { - continue; - } - const auto & traj_point = mpt_traj.at(i); - - auto marker = createDefaultMarker( - "map", rclcpp::Clock().now(), ns, i, visualization_msgs::msg::Marker::LINE_STRIP, - createMarkerScale(0.05, 0.0, 0.0), createMarkerColor(r, g, b, 1.0)); - marker.lifetime = rclcpp::Duration::from_seconds(1.5); - - const double base_to_right = (vehicle_param.wheel_tread / 2.0) + vehicle_param.right_overhang; - const double base_to_left = (vehicle_param.wheel_tread / 2.0) + vehicle_param.left_overhang; - const double base_to_front = vehicle_param.length - vehicle_param.rear_overhang; - const double base_to_rear = vehicle_param.rear_overhang; - - marker.points.push_back( - tier4_autoware_utils::calcOffsetPose(traj_point.pose, base_to_front, -base_to_right, 0.0) - .position); - marker.points.push_back( - tier4_autoware_utils::calcOffsetPose(traj_point.pose, base_to_front, base_to_left, 0.0) - .position); - marker.points.push_back( - tier4_autoware_utils::calcOffsetPose(traj_point.pose, -base_to_rear, base_to_left, 0.0) - .position); - marker.points.push_back( - tier4_autoware_utils::calcOffsetPose(traj_point.pose, -base_to_rear, -base_to_right, 0.0) - .position); - marker.points.push_back(marker.points.front()); - - msg.markers.push_back(marker); - } - return msg; -} - -visualization_msgs::msg::MarkerArray getRectanglesNumMarkerArray( - const std::vector mpt_traj, - const VehicleParam & vehicle_param, const std::string & ns, const double r, const double g, - const double b) -{ - auto marker = createDefaultMarker( - "map", rclcpp::Clock().now(), ns, 0, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, - createMarkerScale(0.0, 0.0, 0.125), createMarkerColor(r, g, b, 0.99)); - marker.lifetime = rclcpp::Duration::from_seconds(1.5); - - visualization_msgs::msg::MarkerArray msg; - for (size_t i = 0; i < mpt_traj.size(); ++i) { - const auto & traj_point = mpt_traj.at(i); - - marker.text = std::to_string(i); - - const double base_to_right = (vehicle_param.wheel_tread / 2.0) + vehicle_param.right_overhang; - const double base_to_front = vehicle_param.length - vehicle_param.rear_overhang; - - const auto top_right_pos = - tier4_autoware_utils::calcOffsetPose(traj_point.pose, base_to_front, -base_to_right, 0.0) - .position; - marker.id = i; - marker.pose.position = top_right_pos; - msg.markers.push_back(marker); - - marker.id = i + mpt_traj.size(); - marker.pose.position = top_right_pos; - msg.markers.push_back(marker); - } - return msg; -} - -visualization_msgs::msg::MarkerArray getBoundsCandidatesLineMarkerArray( - const std::vector & ref_points, std::vector & bounds_candidates, - const double r, const double g, const double b, [[maybe_unused]] const double vehicle_width, - const size_t sampling_num) -{ - const auto current_time = rclcpp::Clock().now(); - visualization_msgs::msg::MarkerArray msg; - const std::string ns = "bounds_candidates"; - - if (ref_points.empty() || bounds_candidates.empty()) return msg; - - auto marker = createDefaultMarker( - "map", rclcpp::Clock().now(), ns, 0, visualization_msgs::msg::Marker::LINE_LIST, - createMarkerScale(0.05, 0.0, 0.0), createMarkerColor(r + 0.5, g, b, 0.3)); - marker.lifetime = rclcpp::Duration::from_seconds(1.5); - - for (size_t i = 0; i < ref_points.size(); i++) { - if (i % sampling_num != 0) { - continue; - } - - const auto & bound_candidates = bounds_candidates.at(i); - geometry_msgs::msg::Pose pose; - pose.position = ref_points.at(i).p; - pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(ref_points.at(i).yaw); - - // lower bound - const double lb_y = bound_candidates.lower_bound; - const auto lb = tier4_autoware_utils::calcOffsetPose(pose, 0.0, lb_y, 0.0).position; - marker.points.push_back(lb); - - // upper bound - const double ub_y = bound_candidates.upper_bound; - const auto ub = tier4_autoware_utils::calcOffsetPose(pose, 0.0, ub_y, 0.0).position; - marker.points.push_back(ub); - } - - return msg; -} - -visualization_msgs::msg::MarkerArray getBoundsLineMarkerArray( - const std::vector & ref_points, const VehicleParam & vehicle_param, - const double r, const double g, const double b, const size_t sampling_num) -{ - const auto current_time = rclcpp::Clock().now(); - visualization_msgs::msg::MarkerArray msg; - - if (ref_points.empty()) return msg; - const double base_to_right = (vehicle_param.wheel_tread / 2.0) + vehicle_param.right_overhang; - const double base_to_left = (vehicle_param.wheel_tread / 2.0) + vehicle_param.left_overhang; - - for (size_t bound_idx = 0; bound_idx < ref_points.at(0).vehicle_bounds.size(); ++bound_idx) { - const std::string ns = "base_bounds_" + std::to_string(bound_idx); - - { // lower bound - auto marker = createDefaultMarker( - "map", rclcpp::Clock().now(), ns, 0, visualization_msgs::msg::Marker::LINE_LIST, - createMarkerScale(0.05, 0.0, 0.0), createMarkerColor(r + 0.5, g, b, 0.3)); - marker.lifetime = rclcpp::Duration::from_seconds(1.5); - - for (size_t i = 0; i < ref_points.size(); i++) { - if (i % sampling_num != 0) { - continue; - } - - const geometry_msgs::msg::Pose & pose = ref_points.at(i).vehicle_bounds_poses.at(bound_idx); - const double lb_y = ref_points.at(i).vehicle_bounds[bound_idx].lower_bound - base_to_right; - const auto lb = tier4_autoware_utils::calcOffsetPose(pose, 0.0, lb_y, 0.0).position; - - marker.points.push_back(pose.position); - marker.points.push_back(lb); - } - msg.markers.push_back(marker); - } - - { // upper bound - auto marker = createDefaultMarker( - "map", rclcpp::Clock().now(), ns, 1, visualization_msgs::msg::Marker::LINE_LIST, - createMarkerScale(0.05, 0.0, 0.0), createMarkerColor(r, g + 0.5, b, 0.3)); - marker.lifetime = rclcpp::Duration::from_seconds(1.5); - - for (size_t i = 0; i < ref_points.size(); i++) { - if (i % sampling_num != 0) { - continue; - } - - const geometry_msgs::msg::Pose & pose = ref_points.at(i).vehicle_bounds_poses.at(bound_idx); - const double ub_y = ref_points.at(i).vehicle_bounds[bound_idx].upper_bound + base_to_left; - const auto ub = tier4_autoware_utils::calcOffsetPose(pose, 0.0, ub_y, 0.0).position; - - marker.points.push_back(pose.position); - marker.points.push_back(ub); - } - msg.markers.push_back(marker); - } - } - - return msg; -} - -visualization_msgs::msg::MarkerArray getVehicleCircleLineMarkerArray( - const std::vector> & vehicle_circles_pose, - const VehicleParam & vehicle_param, const size_t sampling_num, const std::string & ns, - const double r, const double g, const double b) -{ - const auto current_time = rclcpp::Clock().now(); - visualization_msgs::msg::MarkerArray msg; - const double base_to_right = (vehicle_param.wheel_tread / 2.0) + vehicle_param.right_overhang; - const double base_to_left = (vehicle_param.wheel_tread / 2.0) + vehicle_param.left_overhang; - for (size_t i = 0; i < vehicle_circles_pose.size(); ++i) { - if (i % sampling_num != 0) { - continue; - } - - auto marker = createDefaultMarker( - "map", rclcpp::Clock().now(), ns, i, visualization_msgs::msg::Marker::LINE_LIST, - createMarkerScale(0.1, 0, 0), createMarkerColor(r, g, b, 0.25)); - marker.lifetime = rclcpp::Duration::from_seconds(1.5); - - for (size_t j = 0; j < vehicle_circles_pose.at(i).size(); ++j) { - const geometry_msgs::msg::Pose & pose = vehicle_circles_pose.at(i).at(j); - const auto ub = tier4_autoware_utils::calcOffsetPose(pose, 0.0, base_to_left, 0.0).position; - const auto lb = tier4_autoware_utils::calcOffsetPose(pose, 0.0, -base_to_right, 0.0).position; - - marker.points.push_back(ub); - marker.points.push_back(lb); - } - msg.markers.push_back(marker); - } - - return msg; -} - -visualization_msgs::msg::MarkerArray getLateralErrorsLineMarkerArray( - const std::vector mpt_ref_poses, std::vector lateral_errors, - const size_t sampling_num, const std::string & ns, const double r, const double g, const double b) -{ - auto marker = createDefaultMarker( - "map", rclcpp::Clock().now(), ns, 0, visualization_msgs::msg::Marker::LINE_LIST, - createMarkerScale(0.1, 0, 0), createMarkerColor(r, g, b, 1.0)); - marker.lifetime = rclcpp::Duration::from_seconds(1.5); - - for (size_t i = 0; i < mpt_ref_poses.size(); ++i) { - if (i % sampling_num != 0) { - continue; - } - - const auto vehicle_pose = - tier4_autoware_utils::calcOffsetPose(mpt_ref_poses.at(i), 0.0, lateral_errors.at(i), 0.0); - marker.points.push_back(mpt_ref_poses.at(i).position); - marker.points.push_back(vehicle_pose.position); - } - - visualization_msgs::msg::MarkerArray msg; - msg.markers.push_back(marker); - - return msg; -} - -visualization_msgs::msg::MarkerArray getCurrentVehicleCirclesMarkerArray( - const geometry_msgs::msg::Pose & current_ego_pose, const VehicleParam & vehicle_param, - const std::vector & vehicle_circle_longitudinal_offsets, - const std::vector & vehicle_circle_radiuses, const std::string & ns, const double r, - const double g, const double b) -{ - visualization_msgs::msg::MarkerArray msg; - - size_t id = 0; - for (size_t v_idx = 0; v_idx < vehicle_circle_longitudinal_offsets.size(); ++v_idx) { - const double offset = vehicle_circle_longitudinal_offsets.at(v_idx); - const double lateral_offset = - abs(vehicle_param.right_overhang - vehicle_param.left_overhang) / 2.0; - - auto marker = createDefaultMarker( - "map", rclcpp::Clock().now(), ns, id, visualization_msgs::msg::Marker::LINE_STRIP, - createMarkerScale(0.05, 0.0, 0.0), createMarkerColor(r, g, b, 0.8)); - marker.lifetime = rclcpp::Duration::from_seconds(1.5); - marker.pose = tier4_autoware_utils::calcOffsetPose(current_ego_pose, offset, 0.0, 0.0); - - constexpr size_t circle_dividing_num = 16; - for (size_t e_idx = 0; e_idx < circle_dividing_num + 1; ++e_idx) { - geometry_msgs::msg::Point edge_pos; - - const double edge_angle = - static_cast(e_idx) / static_cast(circle_dividing_num) * 2.0 * M_PI; - edge_pos.x = vehicle_circle_radiuses.at(v_idx) * std::cos(edge_angle) + lateral_offset; - edge_pos.y = vehicle_circle_radiuses.at(v_idx) * std::sin(edge_angle) - lateral_offset; - - marker.points.push_back(edge_pos); - } - - msg.markers.push_back(marker); - ++id; - } - return msg; -} - -visualization_msgs::msg::MarkerArray getVehicleCirclesMarkerArray( - const std::vector & mpt_traj_points, - [[maybe_unused]] const VehicleParam & vehicle_param, - const std::vector & vehicle_circle_longitudinal_offsets, - const std::vector & vehicle_circle_radiuses, const size_t sampling_num, - const std::string & ns, const double r, const double g, const double b) -{ - visualization_msgs::msg::MarkerArray msg; - - size_t id = 0; - for (size_t i = 0; i < mpt_traj_points.size(); ++i) { - if (i % sampling_num != 0) { - continue; - } - const auto & mpt_traj_point = mpt_traj_points.at(i); - - for (size_t v_idx = 0; v_idx < vehicle_circle_longitudinal_offsets.size(); ++v_idx) { - const double offset = vehicle_circle_longitudinal_offsets.at(v_idx); - const double lateral_offset = - abs(vehicle_param.right_overhang - vehicle_param.left_overhang) / 2.0; - - auto marker = createDefaultMarker( - "map", rclcpp::Clock().now(), ns, id, visualization_msgs::msg::Marker::LINE_STRIP, - createMarkerScale(0.05, 0.0, 0.0), createMarkerColor(r, g, b, 0.8)); - marker.lifetime = rclcpp::Duration::from_seconds(1.5); - marker.pose = tier4_autoware_utils::calcOffsetPose(mpt_traj_point.pose, offset, 0.0, 0.0); - - constexpr size_t circle_dividing_num = 16; - for (size_t e_idx = 0; e_idx < circle_dividing_num + 1; ++e_idx) { - geometry_msgs::msg::Point edge_pos; - - const double edge_angle = - static_cast(e_idx) / static_cast(circle_dividing_num) * 2.0 * M_PI; - edge_pos.x = vehicle_circle_radiuses.at(v_idx) * std::cos(edge_angle) + lateral_offset; - edge_pos.y = vehicle_circle_radiuses.at(v_idx) * std::sin(edge_angle) - lateral_offset; - - marker.points.push_back(edge_pos); - } - - msg.markers.push_back(marker); - ++id; - } - } - return msg; -} - -visualization_msgs::msg::MarkerArray getFootprintByDrivableAreaMarkerArray( - const geometry_msgs::msg::Pose & stop_pose, const VehicleParam & vehicle_param, - const std::string & ns, const double r, const double g, const double b) -{ - visualization_msgs::msg::MarkerArray msg; - - auto marker = createDefaultMarker( - "map", rclcpp::Clock().now(), ns, 1, visualization_msgs::msg::Marker::LINE_STRIP, - createMarkerScale(0.05, 0.0, 0.0), createMarkerColor(r, g, b, 1.0)); - marker.lifetime = rclcpp::Duration::from_seconds(1.5); - - const double base_to_right = (vehicle_param.wheel_tread / 2.0) + vehicle_param.right_overhang; - const double base_to_left = (vehicle_param.wheel_tread / 2.0) + vehicle_param.left_overhang; - const double base_to_front = vehicle_param.length - vehicle_param.rear_overhang; - const double base_to_rear = vehicle_param.rear_overhang; - - marker.points.push_back( - tier4_autoware_utils::calcOffsetPose(stop_pose, base_to_front, base_to_left, 0.0).position); - marker.points.push_back( - tier4_autoware_utils::calcOffsetPose(stop_pose, base_to_front, -base_to_right, 0.0).position); - marker.points.push_back( - tier4_autoware_utils::calcOffsetPose(stop_pose, -base_to_rear, -base_to_right, 0.0).position); - marker.points.push_back( - tier4_autoware_utils::calcOffsetPose(stop_pose, -base_to_rear, base_to_left, 0.0).position); - marker.points.push_back(marker.points.front()); - - msg.markers.push_back(marker); - - return msg; -} - -visualization_msgs::msg::MarkerArray getVirtualWallMarkerArray( - const geometry_msgs::msg::Pose & pose, const std::string & ns, const double r, const double g, - const double b) -{ - auto marker = createDefaultMarker( - "map", rclcpp::Clock().now(), ns, 0, visualization_msgs::msg::Marker::CUBE, - createMarkerScale(0.1, 5.0, 2.0), createMarkerColor(r, g, b, 0.5)); - marker.lifetime = rclcpp::Duration::from_seconds(1.5); - marker.pose = pose; - marker.pose.position.z += 1.0; - - visualization_msgs::msg::MarkerArray msg; - msg.markers.push_back(marker); - - return msg; -} - -visualization_msgs::msg::MarkerArray getVirtualWallTextMarkerArray( - const geometry_msgs::msg::Pose & pose, const std::string & ns, const double r, const double g, - const double b) -{ - auto marker = createDefaultMarker( - "map", rclcpp::Clock().now(), ns, 0, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, - createMarkerScale(0.0, 0.0, 1.0), createMarkerColor(r, g, b, 0.99)); - marker.lifetime = rclcpp::Duration::from_seconds(1.5); - marker.pose = pose; - marker.pose.position.z += 2.0; - marker.text = "drivable area"; - - visualization_msgs::msg::MarkerArray msg; - msg.markers.push_back(marker); - - return msg; -} -} // namespace - -namespace debug_utils -{ -visualization_msgs::msg::MarkerArray getDebugVisualizationMarker( - DebugData & debug_data, - const std::vector & optimized_points, - const VehicleParam & vehicle_param, const bool is_showing_debug_detail) -{ - visualization_msgs::msg::MarkerArray vis_marker_array; - - if (is_showing_debug_detail) { - const auto points_marker_array = getDebugPointsMarkers( - debug_data.interpolated_points, optimized_points, debug_data.straight_points, - debug_data.fixed_points, debug_data.non_fixed_points); - - const auto constrain_marker_array = - getDebugConstrainMarkers(debug_data.constrain_rectangles, "constrain_rect"); - - appendMarkerArray(points_marker_array, &vis_marker_array); - appendMarkerArray(constrain_marker_array, &vis_marker_array); - - appendMarkerArray( - getRectanglesNumMarkerArray( - debug_data.mpt_traj, vehicle_param, "num_vehicle_footprint", 0.99, 0.99, 0.2), - &vis_marker_array); - - appendMarkerArray( - getPointsTextMarkerArray(debug_data.eb_traj, "eb_traj_text", 0.99, 0.99, 0.2), - &vis_marker_array); - } - - // avoiding objects - appendMarkerArray( - getObjectsMarkerArray(debug_data.avoiding_objects, "avoiding_objects", 0.99, 0.99, 0.2), - &vis_marker_array); - // mpt footprints - appendMarkerArray( - getRectanglesMarkerArray( - debug_data.mpt_traj, vehicle_param, "mpt_footprints", 0.99, 0.99, 0.2, - debug_data.mpt_visualize_sampling_num), - &vis_marker_array); - // bounds - appendMarkerArray( - getBoundsLineMarkerArray( - debug_data.ref_points, vehicle_param, 0.99, 0.99, 0.2, debug_data.mpt_visualize_sampling_num), - &vis_marker_array); - - // bounds candidates - appendMarkerArray( - getBoundsCandidatesLineMarkerArray( - debug_data.ref_points, debug_data.bounds_candidates, 0.2, 0.99, 0.99, vehicle_param.width, - debug_data.mpt_visualize_sampling_num), - &vis_marker_array); - - // vehicle circle line - appendMarkerArray( - getVehicleCircleLineMarkerArray( - debug_data.vehicle_circles_pose, vehicle_param, debug_data.mpt_visualize_sampling_num, - "vehicle_circle_lines", 0.99, 0.99, 0.2), - &vis_marker_array); - - // lateral error line - appendMarkerArray( - getLateralErrorsLineMarkerArray( - debug_data.mpt_ref_poses, debug_data.lateral_errors, debug_data.mpt_visualize_sampling_num, - "lateral_errors", 0.1, 0.1, 0.8), - &vis_marker_array); - - // current vehicle circles - appendMarkerArray( - getCurrentVehicleCirclesMarkerArray( - debug_data.current_ego_pose, vehicle_param, debug_data.vehicle_circle_longitudinal_offsets, - debug_data.vehicle_circle_radiuses, "current_vehicle_circles", 1.0, 0.3, 0.3), - &vis_marker_array); - - // vehicle circles - appendMarkerArray( - getVehicleCirclesMarkerArray( - debug_data.mpt_traj, vehicle_param, debug_data.vehicle_circle_longitudinal_offsets, - debug_data.vehicle_circle_radiuses, debug_data.mpt_visualize_sampling_num, "vehicle_circles", - 1.0, 0.3, 0.3), - &vis_marker_array); - - // footprint by drivable area - if (debug_data.stop_pose_by_drivable_area) { - appendMarkerArray( - getFootprintByDrivableAreaMarkerArray( - *debug_data.stop_pose_by_drivable_area, vehicle_param, "footprint_by_drivable_area", 1.0, - 0.0, 0.0), - &vis_marker_array); - } - - return vis_marker_array; -} - -visualization_msgs::msg::MarkerArray getDebugVisualizationWallMarker( - DebugData & debug_data, const VehicleParam & vehicle_param) -{ - visualization_msgs::msg::MarkerArray vis_marker_array; - if (debug_data.stop_pose_by_drivable_area) { - const auto virtual_wall_pose = - getVirtualWallPose(debug_data.stop_pose_by_drivable_area.get(), vehicle_param); - appendMarkerArray( - getVirtualWallMarkerArray(virtual_wall_pose, "virtual_wall", 1.0, 0, 0), &vis_marker_array); - appendMarkerArray( - getVirtualWallTextMarkerArray(virtual_wall_pose, "virtual_wall_text", 1.0, 1.0, 1.0), - &vis_marker_array); - } - return vis_marker_array; -} -} // namespace debug_utils diff --git a/planning/obstacle_avoidance_planner/src/utils/geometry_utils.cpp b/planning/obstacle_avoidance_planner/src/utils/geometry_utils.cpp new file mode 100644 index 0000000000000..e471baf1951db --- /dev/null +++ b/planning/obstacle_avoidance_planner/src/utils/geometry_utils.cpp @@ -0,0 +1,205 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "obstacle_avoidance_planner/utils/geometry_utils.hpp" + +#include "motion_utils/motion_utils.hpp" +#include "obstacle_avoidance_planner/eb_path_smoother.hpp" +#include "obstacle_avoidance_planner/mpt_optimizer.hpp" +#include "tf2/utils.h" + +#include "autoware_auto_planning_msgs/msg/path_point.hpp" +#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" +#include "geometry_msgs/msg/point32.hpp" +#include "geometry_msgs/msg/pose.hpp" + +#include +#include +#include +#include +#include +#include + +namespace obstacle_avoidance_planner +{ +namespace bg = boost::geometry; +using tier4_autoware_utils::LinearRing2d; +using tier4_autoware_utils::LineString2d; +using tier4_autoware_utils::Point2d; +using tier4_autoware_utils::Polygon2d; + +namespace +{ +geometry_msgs::msg::Point getStartPoint( + const std::vector & bound, const geometry_msgs::msg::Point & point) +{ + const size_t segment_idx = motion_utils::findNearestSegmentIndex(bound, point); + const auto & curr_seg_point = bound.at(segment_idx); + const auto & next_seg_point = bound.at(segment_idx); + const Eigen::Vector2d first_to_target{point.x - curr_seg_point.x, point.y - curr_seg_point.y}; + const Eigen::Vector2d first_to_second{ + next_seg_point.x - curr_seg_point.x, next_seg_point.y - curr_seg_point.y}; + const double length = first_to_target.dot(first_to_second.normalized()); + + if (length < 0.0) { + return bound.front(); + } + + const auto first_point = motion_utils::calcLongitudinalOffsetPoint(bound, segment_idx, length); + if (first_point) { + return *first_point; + } + + return bound.front(); +} + +bool isFrontDrivableArea( + const geometry_msgs::msg::Point & point, + const std::vector & left_bound, + const std::vector & right_bound) +{ + if (left_bound.empty() || right_bound.empty()) { + return false; + } + + constexpr double min_dist = 0.1; + const auto left_start_point = getStartPoint(left_bound, right_bound.front()); + const auto right_start_point = getStartPoint(right_bound, left_bound.front()); + + // ignore point behind of the front line + const std::vector front_bound = {left_start_point, right_start_point}; + const double lat_dist_to_front_bound = motion_utils::calcLateralOffset(front_bound, point); + if (lat_dist_to_front_bound < min_dist) { + return true; + } + + return false; +} + +Polygon2d createDrivablePolygon( + const std::vector & left_bound, + const std::vector & right_bound) +{ + Polygon2d drivable_area_poly; + + // left bound + for (const auto & p : left_bound) { + drivable_area_poly.outer().push_back(Point2d(p.x, p.y)); + } + + // right bound + auto reversed_right_bound = right_bound; + std::reverse(reversed_right_bound.begin(), reversed_right_bound.end()); + for (const auto & p : reversed_right_bound) { + drivable_area_poly.outer().push_back(Point2d(p.x, p.y)); + } + + drivable_area_poly.outer().push_back(drivable_area_poly.outer().front()); + return drivable_area_poly; +} +} // namespace + +namespace geometry_utils +{ +bool isOutsideDrivableAreaFromRectangleFootprint( + const geometry_msgs::msg::Pose & pose, const std::vector & left_bound, + const std::vector & right_bound, + const vehicle_info_util::VehicleInfo & vehicle_info, + const bool use_footprint_polygon_for_outside_drivable_area_check) +{ + if (left_bound.empty() || right_bound.empty()) { + return false; + } + + const double base_to_right = vehicle_info.wheel_tread_m / 2.0 + vehicle_info.right_overhang_m; + const double base_to_left = vehicle_info.wheel_tread_m / 2.0 + vehicle_info.left_overhang_m; + const double base_to_front = vehicle_info.vehicle_length_m - vehicle_info.rear_overhang_m; + const double base_to_rear = vehicle_info.rear_overhang_m; + + // calculate footprint corner points + const auto top_left_pos = + tier4_autoware_utils::calcOffsetPose(pose, base_to_front, base_to_left, 0.0).position; + const auto top_right_pos = + tier4_autoware_utils::calcOffsetPose(pose, base_to_front, -base_to_right, 0.0).position; + const auto bottom_right_pos = + tier4_autoware_utils::calcOffsetPose(pose, -base_to_rear, -base_to_right, 0.0).position; + const auto bottom_left_pos = + tier4_autoware_utils::calcOffsetPose(pose, -base_to_rear, base_to_left, 0.0).position; + + if (use_footprint_polygon_for_outside_drivable_area_check) { + // calculate footprint polygon + LinearRing2d footprint_polygon; + footprint_polygon.push_back({top_left_pos.x, top_left_pos.y}); + footprint_polygon.push_back({top_right_pos.x, top_right_pos.y}); + footprint_polygon.push_back({bottom_right_pos.x, bottom_right_pos.y}); + footprint_polygon.push_back({bottom_left_pos.x, bottom_left_pos.y}); + bg::correct(footprint_polygon); + + // calculate boundary line strings + LineString2d left_bound_line; + for (const auto & p : left_bound) { + left_bound_line.push_back({p.x, p.y}); + } + + LineString2d right_bound_line; + for (const auto & p : right_bound) { + right_bound_line.push_back({p.x, p.y}); + } + + LineString2d back_bound_line; + back_bound_line = {left_bound_line.back(), right_bound_line.back()}; + + if ( + bg::intersects(footprint_polygon, left_bound_line) || + bg::intersects(footprint_polygon, right_bound_line) || + bg::intersects(footprint_polygon, back_bound_line)) { + return true; + } + return false; + } + + const bool front_top_left = isFrontDrivableArea(top_left_pos, left_bound, right_bound); + const bool front_top_right = isFrontDrivableArea(top_right_pos, left_bound, right_bound); + const bool front_bottom_left = isFrontDrivableArea(bottom_left_pos, left_bound, right_bound); + const bool front_bottom_right = isFrontDrivableArea(bottom_right_pos, left_bound, right_bound); + + // create rectangle + const auto drivable_area_poly = createDrivablePolygon(left_bound, right_bound); + + if (!front_top_left && !bg::within(Point2d(top_left_pos.x, top_left_pos.y), drivable_area_poly)) { + return true; + } + + if ( + !front_top_right && + !bg::within(Point2d(top_right_pos.x, top_right_pos.y), drivable_area_poly)) { + return true; + } + + if ( + !front_bottom_left && + !bg::within(Point2d(bottom_left_pos.x, bottom_left_pos.y), drivable_area_poly)) { + return true; + } + + if ( + !front_bottom_right && + !bg::within(Point2d(bottom_right_pos.x, bottom_right_pos.y), drivable_area_poly)) { + return true; + } + + return false; +} +} // namespace geometry_utils +} // namespace obstacle_avoidance_planner diff --git a/planning/obstacle_avoidance_planner/src/utils/trajectory_utils.cpp b/planning/obstacle_avoidance_planner/src/utils/trajectory_utils.cpp new file mode 100644 index 0000000000000..8a4ab4384f7eb --- /dev/null +++ b/planning/obstacle_avoidance_planner/src/utils/trajectory_utils.cpp @@ -0,0 +1,235 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "obstacle_avoidance_planner/utils/trajectory_utils.hpp" + +#include "motion_utils/motion_utils.hpp" +#include "obstacle_avoidance_planner/eb_path_smoother.hpp" +#include "obstacle_avoidance_planner/mpt_optimizer.hpp" +#include "obstacle_avoidance_planner/utils/geometry_utils.hpp" +#include "tf2/utils.h" + +#include "autoware_auto_planning_msgs/msg/path_point.hpp" +#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" +#include "geometry_msgs/msg/point32.hpp" +#include "geometry_msgs/msg/pose.hpp" + +#include +#include +#include +#include +#include +#include + +namespace tier4_autoware_utils +{ +template <> +geometry_msgs::msg::Point getPoint(const obstacle_avoidance_planner::ReferencePoint & p) +{ + return p.pose.position; +} + +template <> +geometry_msgs::msg::Pose getPose(const obstacle_avoidance_planner::ReferencePoint & p) +{ + return p.pose; +} + +template <> +double getLongitudinalVelocity(const obstacle_avoidance_planner::ReferencePoint & p) +{ + return p.longitudinal_velocity_mps; +} +} // namespace tier4_autoware_utils + +namespace obstacle_avoidance_planner +{ +namespace trajectory_utils +{ +ReferencePoint convertToReferencePoint(const TrajectoryPoint & traj_point) +{ + ReferencePoint ref_point; + + ref_point.pose = traj_point.pose; + ref_point.longitudinal_velocity_mps = traj_point.longitudinal_velocity_mps; + return ref_point; +} + +std::vector convertToReferencePoints( + const std::vector & traj_points) +{ + std::vector ref_points; + for (const auto & traj_point : traj_points) { + const auto ref_point = convertToReferencePoint(traj_point); + ref_points.push_back(ref_point); + } + + return ref_points; +} + +void compensateLastPose( + const PathPoint & last_path_point, std::vector & traj_points, + const double delta_dist_threshold, const double delta_yaw_threshold) +{ + if (traj_points.empty()) { + traj_points.push_back(convertToTrajectoryPoint(last_path_point)); + return; + } + + const geometry_msgs::msg::Pose last_traj_pose = traj_points.back().pose; + + const double dist = + tier4_autoware_utils::calcDistance2d(last_path_point.pose.position, last_traj_pose.position); + const double norm_diff_yaw = [&]() { + const double diff_yaw = + tf2::getYaw(last_path_point.pose.orientation) - tf2::getYaw(last_traj_pose.orientation); + return tier4_autoware_utils::normalizeRadian(diff_yaw); + }(); + if (dist > delta_dist_threshold || std::fabs(norm_diff_yaw) > delta_yaw_threshold) { + traj_points.push_back(convertToTrajectoryPoint(last_path_point)); + } +} + +geometry_msgs::msg::Point getNearestPosition( + const std::vector & points, const int target_idx, const double offset) +{ + double sum_arc_length = 0.0; + for (size_t i = target_idx; i < points.size(); ++i) { + sum_arc_length += points.at(i).delta_arc_length; + + if (offset < sum_arc_length) { + return points.at(i).pose.position; + } + } + + return points.back().pose.position; +} + +Trajectory createTrajectory( + const std_msgs::msg::Header & header, const std::vector & traj_points) +{ + auto traj = motion_utils::convertToTrajectory(traj_points); + traj.header = header; + + return traj; +} + +std::vector resampleTrajectoryPoints( + const std::vector traj_points, const double interval) +{ + constexpr bool enable_resampling_stop_point = true; + + const auto traj = motion_utils::convertToTrajectory(traj_points); + const auto resampled_traj = motion_utils::resampleTrajectory( + traj, interval, false, true, true, enable_resampling_stop_point); + return motion_utils::convertToTrajectoryPointArray(resampled_traj); +} + +// NOTE: stop point will not be resampled +std::vector resampleTrajectoryPointsWithoutStopPoint( + const std::vector traj_points, const double interval) +{ + constexpr bool enable_resampling_stop_point = false; + + const auto traj = motion_utils::convertToTrajectory(traj_points); + const auto resampled_traj = motion_utils::resampleTrajectory( + traj, interval, false, true, true, enable_resampling_stop_point); + return motion_utils::convertToTrajectoryPointArray(resampled_traj); +} + +std::vector resampleReferencePoints( + const std::vector ref_points, const double interval) +{ + // resample pose and velocity + const auto traj_points = convertToTrajectoryPoints(ref_points); + const auto resampled_traj_points = + resampleTrajectoryPointsWithoutStopPoint(traj_points, interval); + const auto resampled_ref_points = convertToReferencePoints(resampled_traj_points); + + // resample curvature + std::vector base_keys; + std::vector base_values; + for (size_t i = 0; i < ref_points.size(); ++i) { + if (i == 0) { + base_keys.push_back(0.0); + } else { + const double delta_arc_length = + tier4_autoware_utils::calcDistance2d(ref_points.at(i), ref_points.at(i - 1)); + base_keys.push_back(base_keys.back() + delta_arc_length); + } + + base_values.push_back(ref_points.at(i).curvature); + } + + std::vector query_keys; + for (size_t i = 0; i < resampled_ref_points.size(); ++i) { + if (i == 0) { + query_keys.push_back(0.0); + } else { + const double delta_arc_length = tier4_autoware_utils::calcDistance2d( + resampled_ref_points.at(i), resampled_ref_points.at(i - 1)); + const double key = query_keys.back() + delta_arc_length; + if (base_keys.back() < key) { + break; + } + + query_keys.push_back(key); + } + } + + if (query_keys.size() != resampled_ref_points.size()) { + // compensate last key + constexpr double epsilon = 1e-6; + query_keys.push_back(base_keys.back() - epsilon); + } + + const auto query_values = interpolation::lerp(base_keys, base_values, query_keys); + + // create output reference points by updating curvature with resampled one + std::vector output_ref_points; + for (size_t i = 0; i < query_values.size(); ++i) { + output_ref_points.push_back(resampled_ref_points.at(i)); + output_ref_points.at(i).curvature = query_values.at(i); + } + + return output_ref_points; +} + +void insertStopPoint( + std::vector & traj_points, const geometry_msgs::msg::Pose & input_stop_pose, + const size_t stop_seg_idx) +{ + const double offset_to_segment = motion_utils::calcLongitudinalOffsetToSegment( + traj_points, stop_seg_idx, input_stop_pose.position); + + const auto traj_spline = SplineInterpolationPoints2d(traj_points); + const auto stop_pose = traj_spline.getSplineInterpolatedPose(stop_seg_idx, offset_to_segment); + + if (geometry_utils::isSamePoint(traj_points.at(stop_seg_idx), stop_pose)) { + traj_points.at(stop_seg_idx).longitudinal_velocity_mps = 0.0; + return; + } + if (geometry_utils::isSamePoint(traj_points.at(stop_seg_idx + 1), stop_pose)) { + traj_points.at(stop_seg_idx + 1).longitudinal_velocity_mps = 0.0; + return; + } + + TrajectoryPoint additional_traj_point; + additional_traj_point.pose = stop_pose; + additional_traj_point.longitudinal_velocity_mps = 0.0; + + traj_points.insert(traj_points.begin() + stop_seg_idx + 1, additional_traj_point); +} +} // namespace trajectory_utils +} // namespace obstacle_avoidance_planner diff --git a/planning/obstacle_avoidance_planner/src/utils/utils.cpp b/planning/obstacle_avoidance_planner/src/utils/utils.cpp deleted file mode 100644 index 377cb8150d60c..0000000000000 --- a/planning/obstacle_avoidance_planner/src/utils/utils.cpp +++ /dev/null @@ -1,780 +0,0 @@ -// Copyright 2020 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "obstacle_avoidance_planner/utils/utils.hpp" - -#include "obstacle_avoidance_planner/eb_path_optimizer.hpp" -#include "obstacle_avoidance_planner/mpt_optimizer.hpp" -#include "tf2/utils.h" - -#include "autoware_auto_planning_msgs/msg/path_point.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" -#include "geometry_msgs/msg/point32.hpp" -#include "geometry_msgs/msg/pose.hpp" -#include "nav_msgs/msg/map_meta_data.hpp" - -#include "boost/optional.hpp" - -#include -#include -#include -#include -#include - -namespace bg = boost::geometry; -typedef bg::model::d2::point_xy bg_point; -typedef bg::model::polygon bg_polygon; - -namespace -{ -namespace bg = boost::geometry; -using tier4_autoware_utils::LinearRing2d; -using tier4_autoware_utils::LineString2d; -std::vector convertEulerAngleToMonotonic(const std::vector & angle) -{ - if (angle.empty()) { - return std::vector{}; - } - - std::vector monotonic_angle{angle.front()}; - for (size_t i = 1; i < angle.size(); ++i) { - const double diff_angle = angle.at(i) - monotonic_angle.back(); - monotonic_angle.push_back( - monotonic_angle.back() + tier4_autoware_utils::normalizeRadian(diff_angle)); - } - - return monotonic_angle; -} - -std::vector calcEuclidDist(const std::vector & x, const std::vector & y) -{ - if (x.size() != y.size()) { - std::cerr << "x y vector size should be the same." << std::endl; - } - - std::vector dist_v; - dist_v.push_back(0.0); - for (unsigned int i = 0; i < x.size() - 1; ++i) { - const double dx = x.at(i + 1) - x.at(i); - const double dy = y.at(i + 1) - y.at(i); - dist_v.push_back(dist_v.at(i) + std::hypot(dx, dy)); - } - - return dist_v; -} - -std::array, 3> validateTrajectoryPoints( - const std::vector & points) -{ - constexpr double epsilon = 1e-6; - - std::vector x; - std::vector y; - std::vector yaw; - for (size_t i = 0; i < points.size(); i++) { - if (i > 0) { - if ( - std::fabs(points[i].pose.position.x - points[i - 1].pose.position.x) < epsilon && - std::fabs(points[i].pose.position.y - points[i - 1].pose.position.y) < epsilon) { - continue; - } - } - x.push_back(points[i].pose.position.x); - y.push_back(points[i].pose.position.y); - yaw.push_back(tf2::getYaw(points[i].pose.orientation)); - } - return {x, y, yaw}; -} - -std::array, 2> validatePoints( - const std::vector & points) -{ - std::vector x; - std::vector y; - for (size_t i = 0; i < points.size(); i++) { - if (i > 0) { - if ( - std::fabs(points[i].pose.position.x - points[i - 1].pose.position.x) < 1e-6 && - std::fabs(points[i].pose.position.y - points[i - 1].pose.position.y) < 1e-6) { - continue; - } - } - x.push_back(points[i].pose.position.x); - y.push_back(points[i].pose.position.y); - } - return {x, y}; -} - -// only two points is supported -std::vector splineTwoPoints( - std::vector base_s, std::vector base_x, const double begin_diff, - const double end_diff, std::vector new_s) -{ - const double h = base_s.at(1) - base_s.at(0); - - const double c = begin_diff; - const double d = base_x.at(0); - const double a = (end_diff * h - 2 * base_x.at(1) + c * h + 2 * d) / std::pow(h, 3); - const double b = (3 * base_x.at(1) - end_diff * h - 2 * c * h - 3 * d) / std::pow(h, 2); - - std::vector res; - for (const auto & s : new_s) { - const double ds = s - base_s.at(0); - res.push_back(d + (c + (b + a * ds) * ds) * ds); - } - - return res; -} -} // namespace - -namespace tier4_autoware_utils -{ -template <> -geometry_msgs::msg::Point getPoint(const ReferencePoint & p) -{ - return p.p; -} - -template <> -geometry_msgs::msg::Pose getPose(const ReferencePoint & p) -{ - geometry_msgs::msg::Pose pose; - pose.position = p.p; - pose.orientation = createQuaternionFromYaw(p.yaw); - return pose; -} -} // namespace tier4_autoware_utils - -namespace geometry_utils -{ -geometry_msgs::msg::Point transformToAbsoluteCoordinate2D( - const geometry_msgs::msg::Point & point, const geometry_msgs::msg::Pose & origin) -{ - // NOTE: implement transformation without defining yaw variable - // but directly sin/cos of yaw for fast calculation - const auto & q = origin.orientation; - const double cos_yaw = 1 - 2 * q.z * q.z; - const double sin_yaw = 2 * q.w * q.z; - - geometry_msgs::msg::Point absolute_p; - absolute_p.x = point.x * cos_yaw - point.y * sin_yaw + origin.position.x; - absolute_p.y = point.x * sin_yaw + point.y * cos_yaw + origin.position.y; - absolute_p.z = point.z; - - return absolute_p; -} - -geometry_msgs::msg::Quaternion getQuaternionFromPoints( - const geometry_msgs::msg::Point & a, const geometry_msgs::msg::Point & a_root) -{ - const double yaw = tier4_autoware_utils::calcAzimuthAngle(a_root, a); - return tier4_autoware_utils::createQuaternionFromYaw(yaw); -} - -geometry_msgs::msg::Quaternion getQuaternionFromPoints( - const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2, - const geometry_msgs::msg::Point & p3, const geometry_msgs::msg::Point & p4) -{ - const double dx = (8.0 * (p3.x - p2.x) - (p4.x - p1.x)) / 12.0; - const double dy = (8.0 * (p3.y - p2.y) - (p4.y - p1.y)) / 12.0; - const double yaw = std::atan2(dy, dx); - - return tier4_autoware_utils::createQuaternionFromYaw(yaw); -} - -boost::optional transformMapToOptionalImage( - const geometry_msgs::msg::Point & map_point, - const nav_msgs::msg::MapMetaData & occupancy_grid_info) -{ - const geometry_msgs::msg::Point relative_p = - transformToRelativeCoordinate2D(map_point, occupancy_grid_info.origin); - const double resolution = occupancy_grid_info.resolution; - const double map_y_height = occupancy_grid_info.height; - const double map_x_width = occupancy_grid_info.width; - const double map_x_in_image_resolution = relative_p.x / resolution; - const double map_y_in_image_resolution = relative_p.y / resolution; - const double image_x = map_y_height - map_y_in_image_resolution; - const double image_y = map_x_width - map_x_in_image_resolution; - if ( - image_x >= 0 && image_x < static_cast(map_y_height) && image_y >= 0 && - image_y < static_cast(map_x_width)) { - geometry_msgs::msg::Point image_point; - image_point.x = image_x; - image_point.y = image_y; - return image_point; - } else { - return boost::none; - } -} - -bool transformMapToImage( - const geometry_msgs::msg::Point & map_point, - const nav_msgs::msg::MapMetaData & occupancy_grid_info, geometry_msgs::msg::Point & image_point) -{ - geometry_msgs::msg::Point relative_p = - transformToRelativeCoordinate2D(map_point, occupancy_grid_info.origin); - const double map_y_height = occupancy_grid_info.height; - const double map_x_width = occupancy_grid_info.width; - const double scale = 1 / occupancy_grid_info.resolution; - const double map_x_in_image_resolution = relative_p.x * scale; - const double map_y_in_image_resolution = relative_p.y * scale; - const double image_x = map_y_height - map_y_in_image_resolution; - const double image_y = map_x_width - map_x_in_image_resolution; - if ( - image_x >= 0 && image_x < static_cast(map_y_height) && image_y >= 0 && - image_y < static_cast(map_x_width)) { - image_point.x = image_x; - image_point.y = image_y; - return true; - } else { - return false; - } -} -} // namespace geometry_utils - -namespace interpolation_utils -{ -std::vector interpolate2DPoints( - const std::vector & base_x, const std::vector & base_y, const double resolution, - const double offset = 0.0) -{ - if (base_x.empty() || base_y.empty()) { - return std::vector{}; - } - const std::vector base_s = calcEuclidDist(base_x, base_y); - if (base_s.empty() || base_s.size() == 1) { - return std::vector{}; - } - - std::vector new_s; - for (double i = offset; i < base_s.back() - 1e-6; i += resolution) { - new_s.push_back(i); - } - if (new_s.empty()) { - return std::vector{}; - } - - // spline interpolation - const std::vector interpolated_x = interpolation::spline(base_s, base_x, new_s); - const std::vector interpolated_y = interpolation::spline(base_s, base_y, new_s); - for (size_t i = 0; i < interpolated_x.size(); ++i) { - if (std::isnan(interpolated_x[i]) || std::isnan(interpolated_y[i])) { - return std::vector{}; - } - } - - std::vector interpolated_points; - for (size_t i = 0; i < interpolated_x.size(); ++i) { - geometry_msgs::msg::Point point; - point.x = interpolated_x[i]; - point.y = interpolated_y[i]; - interpolated_points.push_back(point); - } - - return interpolated_points; -} - -std::vector interpolateConnected2DPoints( - const std::vector & base_x, const std::vector & base_y, const double resolution, - const double begin_yaw, const double end_yaw) -{ - if (base_x.empty() || base_y.empty()) { - return std::vector{}; - } - std::vector base_s = calcEuclidDist(base_x, base_y); - if (base_s.empty() || base_s.size() == 1) { - return std::vector{}; - } - std::vector new_s; - for (double i = 0.0; i < base_s.back() - 1e-6; i += resolution) { - new_s.push_back(i); - } - - // spline interpolation - const auto interpolated_x = - splineTwoPoints(base_s, base_x, std::cos(begin_yaw), std::cos(end_yaw), new_s); - const auto interpolated_y = - splineTwoPoints(base_s, base_y, std::sin(begin_yaw), std::sin(end_yaw), new_s); - - for (size_t i = 0; i < interpolated_x.size(); i++) { - if (std::isnan(interpolated_x[i]) || std::isnan(interpolated_y[i])) { - return std::vector{}; - } - } - - std::vector interpolated_points; - for (size_t i = 0; i < interpolated_x.size(); i++) { - autoware_auto_planning_msgs::msg::TrajectoryPoint point; - point.pose.position.x = interpolated_x[i]; - point.pose.position.y = interpolated_y[i]; - - const size_t front_idx = (i == interpolated_x.size() - 1) ? i - 1 : i; - const double dx = interpolated_x[front_idx + 1] - interpolated_x[front_idx]; - const double dy = interpolated_y[front_idx + 1] - interpolated_y[front_idx]; - const double yaw = std::atan2(dy, dx); - point.pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(yaw); - - interpolated_points.push_back(point); - } - - return interpolated_points; -} - -std::vector interpolate2DTrajectoryPoints( - const std::vector & base_x, const std::vector & base_y, - const std::vector & base_yaw, const double resolution) -{ - if (base_x.empty() || base_y.empty() || base_yaw.empty()) { - return std::vector{}; - } - std::vector base_s = calcEuclidDist(base_x, base_y); - if (base_s.empty() || base_s.size() == 1) { - return std::vector{}; - } - std::vector new_s; - for (double i = 0.0; i < base_s.back() - 1e-6; i += resolution) { - new_s.push_back(i); - } - - const auto monotonic_base_yaw = convertEulerAngleToMonotonic(base_yaw); - - // spline interpolation - const auto interpolated_x = interpolation::spline(base_s, base_x, new_s); - const auto interpolated_y = interpolation::spline(base_s, base_y, new_s); - const auto interpolated_yaw = interpolation::spline(base_s, monotonic_base_yaw, new_s); - - for (size_t i = 0; i < interpolated_x.size(); i++) { - if (std::isnan(interpolated_x[i]) || std::isnan(interpolated_y[i])) { - return std::vector{}; - } - } - - std::vector interpolated_points; - for (size_t i = 0; i < interpolated_x.size(); i++) { - autoware_auto_planning_msgs::msg::TrajectoryPoint point; - point.pose.position.x = interpolated_x[i]; - point.pose.position.y = interpolated_y[i]; - point.pose.orientation = tier4_autoware_utils::createQuaternionFromYaw( - tier4_autoware_utils::normalizeRadian(interpolated_yaw[i])); - interpolated_points.push_back(point); - } - - return interpolated_points; -} - -std::vector interpolate2DTrajectoryPoints( - const std::vector & base_x, const std::vector & base_y, const double resolution) -{ - if (base_x.empty() || base_y.empty()) { - return std::vector{}; - } - std::vector base_s = calcEuclidDist(base_x, base_y); - if (base_s.empty() || base_s.size() == 1) { - return std::vector{}; - } - std::vector new_s; - for (double i = 0.0; i < base_s.back() - 1e-6; i += resolution) { - new_s.push_back(i); - } - - // spline interpolation - // x = interpolated[0], y = interpolated[1], yaw = interpolated[2] - std::array, 3> interpolated = - interpolation::slerp2dFromXY(base_s, base_x, base_y, new_s); - const auto & interpolated_x = interpolated[0]; - const auto & interpolated_y = interpolated[1]; - const auto & interpolated_yaw = interpolated[2]; - - for (size_t i = 0; i < interpolated_x.size(); i++) { - if (std::isnan(interpolated_x[i]) || std::isnan(interpolated_y[i])) { - return std::vector{}; - } - } - - std::vector interpolated_points; - for (size_t i = 0; i < interpolated_x.size(); i++) { - autoware_auto_planning_msgs::msg::TrajectoryPoint point; - point.pose.position.x = interpolated_x[i]; - point.pose.position.y = interpolated_y[i]; - point.pose.orientation = tier4_autoware_utils::createQuaternionFromYaw( - tier4_autoware_utils::normalizeRadian(interpolated_yaw[i])); - interpolated_points.push_back(point); - } - - return interpolated_points; -} - -std::vector getInterpolatedTrajectoryPoints( - const std::vector & points, - const double delta_arc_length) -{ - std::array, 3> validated_pose = validateTrajectoryPoints(points); - return interpolation_utils::interpolate2DTrajectoryPoints( - validated_pose.at(0), validated_pose.at(1), delta_arc_length); -} - -std::vector getConnectedInterpolatedPoints( - const std::vector & points, - const double delta_arc_length, const double begin_yaw, const double end_yaw) -{ - std::array, 2> validated_pose = validatePoints(points); - return interpolation_utils::interpolateConnected2DPoints( - validated_pose.at(0), validated_pose.at(1), delta_arc_length, begin_yaw, end_yaw); -} -} // namespace interpolation_utils - -namespace points_utils -{ -// functions to convert to another type of points -std::vector convertToPosesWithYawEstimation( - const std::vector points) -{ - std::vector poses; - if (points.empty()) { - return poses; - } else if (points.size() == 1) { - geometry_msgs::msg::Pose pose; - pose.position = points.at(0); - poses.push_back(pose); - return poses; - } - - for (size_t i = 0; i < points.size(); ++i) { - geometry_msgs::msg::Pose pose; - pose.position = points.at(i); - - const size_t front_idx = (i == points.size() - 1) ? i - 1 : i; - const double points_yaw = - tier4_autoware_utils::calcAzimuthAngle(points.at(front_idx), points.at(front_idx + 1)); - pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(points_yaw); - - poses.push_back(pose); - } - return poses; -} - -template -ReferencePoint convertToReferencePoint(const T & point) -{ - ReferencePoint ref_point; - - const auto & pose = tier4_autoware_utils::getPose(point); - ref_point.p = pose.position; - ref_point.yaw = tf2::getYaw(pose.orientation); - - return ref_point; -} - -template ReferencePoint convertToReferencePoint( - const autoware_auto_planning_msgs::msg::TrajectoryPoint & point); -template ReferencePoint convertToReferencePoint( - const geometry_msgs::msg::Pose & point); -template <> -ReferencePoint convertToReferencePoint(const geometry_msgs::msg::Point & point) -{ - ReferencePoint ref_point; - - ref_point.p = point; - - return ref_point; -} - -std::vector concatTrajectory( - const std::vector & traj_points, - const std::vector & extended_traj_points) -{ - std::vector trajectory; - trajectory.insert(trajectory.end(), traj_points.begin(), traj_points.end()); - trajectory.insert(trajectory.end(), extended_traj_points.begin(), extended_traj_points.end()); - return trajectory; -} - -void compensateLastPose( - const autoware_auto_planning_msgs::msg::PathPoint & last_path_point, - std::vector & traj_points, - const double delta_dist_threshold, const double delta_yaw_threshold) -{ - if (traj_points.empty()) { - traj_points.push_back(convertToTrajectoryPoint(last_path_point)); - return; - } - - const geometry_msgs::msg::Pose last_traj_pose = traj_points.back().pose; - - const double dist = - tier4_autoware_utils::calcDistance2d(last_path_point.pose.position, last_traj_pose.position); - const double norm_diff_yaw = [&]() { - const double diff_yaw = - tf2::getYaw(last_path_point.pose.orientation) - tf2::getYaw(last_traj_pose.orientation); - return tier4_autoware_utils::normalizeRadian(diff_yaw); - }(); - if (dist > delta_dist_threshold || std::fabs(norm_diff_yaw) > delta_yaw_threshold) { - traj_points.push_back(convertToTrajectoryPoint(last_path_point)); - } -} - -int getNearestIdx( - const std::vector & points, const double target_s, const int begin_idx) -{ - double nearest_delta_s = std::numeric_limits::max(); - int nearest_idx = begin_idx; - for (size_t i = begin_idx; i < points.size(); i++) { - double diff = std::fabs(target_s - points[i].s); - if (diff < nearest_delta_s) { - nearest_delta_s = diff; - nearest_idx = i; - } - } - return nearest_idx; -} -} // namespace points_utils - -namespace utils -{ -void logOSQPSolutionStatus(const int solution_status, const std::string & msg) -{ - /****************** - * Solver Status * - ******************/ - const int LOCAL_OSQP_SOLVED = 1; - const int LOCAL_OSQP_SOLVED_INACCURATE = 2; - const int LOCAL_OSQP_MAX_ITER_REACHED = -2; - const int LOCAL_OSQP_PRIMAL_INFEASIBLE = -3; - const int LOCAL_OSQP_PRIMAL_INFEASIBLE_INACCURATE = 3; - const int LOCAL_OSQP_DUAL_INFEASIBLE = -4; - const int LOCAL_OSQP_DUAL_INFEASIBLE_INACCURATE = 4; - const int LOCAL_OSQP_SIGINT = -5; - const int LOCAL_OSQP_TIME_LIMIT_REACHED = -6; - const int LOCAL_OSQP_UNSOLVED = -10; - const int LOCAL_OSQP_NON_CVX = -7; - - if (solution_status == LOCAL_OSQP_SOLVED) { - } else if (solution_status == LOCAL_OSQP_DUAL_INFEASIBLE_INACCURATE) { - RCLCPP_WARN( - rclcpp::get_logger("util"), - "[Avoidance] %s OSQP solution status: OSQP_DUAL_INFEASIBLE_INACCURATE", msg.c_str()); - } else if (solution_status == LOCAL_OSQP_PRIMAL_INFEASIBLE_INACCURATE) { - RCLCPP_WARN( - rclcpp::get_logger("util"), - "[Avoidance] %s OSQP solution status: OSQP_PRIMAL_INFEASIBLE_INACCURATE", msg.c_str()); - } else if (solution_status == LOCAL_OSQP_SOLVED_INACCURATE) { - RCLCPP_WARN( - rclcpp::get_logger("util"), "[Avoidance] %s OSQP solution status: OSQP_SOLVED_INACCURATE", - msg.c_str()); - } else if (solution_status == LOCAL_OSQP_MAX_ITER_REACHED) { - RCLCPP_WARN( - rclcpp::get_logger("util"), "[Avoidance] %s OSQP solution status: OSQP_ITER_REACHED", - msg.c_str()); - } else if (solution_status == LOCAL_OSQP_PRIMAL_INFEASIBLE) { - RCLCPP_WARN( - rclcpp::get_logger("util"), "[Avoidance] %s OSQP solution status: OSQP_PRIMAL_INFEASIBLE", - msg.c_str()); - } else if (solution_status == LOCAL_OSQP_DUAL_INFEASIBLE) { - RCLCPP_WARN( - rclcpp::get_logger("util"), "[Avoidance] %s OSQP solution status: OSQP_DUAL_INFEASIBLE", - msg.c_str()); - } else if (solution_status == LOCAL_OSQP_SIGINT) { - RCLCPP_WARN( - rclcpp::get_logger("util"), "[Avoidance] %s OSQP solution status: OSQP_SIGINT", msg.c_str()); - RCLCPP_WARN( - rclcpp::get_logger("util"), "[Avoidance] Interrupted by user, process will be finished."); - std::exit(0); - } else if (solution_status == LOCAL_OSQP_TIME_LIMIT_REACHED) { - RCLCPP_WARN( - rclcpp::get_logger("util"), "[Avoidance] %s OSQP solution status: OSQP_TIME_LIMIT_REACHED", - msg.c_str()); - } else if (solution_status == LOCAL_OSQP_UNSOLVED) { - RCLCPP_WARN( - rclcpp::get_logger("util"), "[Avoidance] %s OSQP solution status: OSQP_UNSOLVED", - msg.c_str()); - } else if (solution_status == LOCAL_OSQP_NON_CVX) { - RCLCPP_WARN( - rclcpp::get_logger("util"), "[Avoidance] %s OSQP solution status: OSQP_NON_CVX", msg.c_str()); - } else { - RCLCPP_WARN( - rclcpp::get_logger("util"), "[Avoidance] %s OSQP solution status: Not defined %d", - msg.c_str(), solution_status); - } -} -} // namespace utils - -namespace -{ -geometry_msgs::msg::Point getStartPoint( - const std::vector & bound, const geometry_msgs::msg::Point & point) -{ - const size_t segment_idx = motion_utils::findNearestSegmentIndex(bound, point); - const auto & curr_seg_point = bound.at(segment_idx); - const auto & next_seg_point = bound.at(segment_idx); - const Eigen::Vector2d first_to_target{point.x - curr_seg_point.x, point.y - curr_seg_point.y}; - const Eigen::Vector2d first_to_second{ - next_seg_point.x - curr_seg_point.x, next_seg_point.y - curr_seg_point.y}; - const double length = first_to_target.dot(first_to_second.normalized()); - - if (length < 0.0) { - return bound.front(); - } - - const auto first_point = motion_utils::calcLongitudinalOffsetPoint(bound, segment_idx, length); - - if (first_point) { - return *first_point; - } - - return bound.front(); -} - -bool isFrontDrivableArea( - const geometry_msgs::msg::Point & point, - const std::vector & left_bound, - const std::vector & right_bound) -{ - if (left_bound.empty() || right_bound.empty()) { - return false; - } - - constexpr double min_dist = 0.1; - const auto left_start_point = getStartPoint(left_bound, right_bound.front()); - const auto right_start_point = getStartPoint(right_bound, left_bound.front()); - - // ignore point behind of the front line - const std::vector front_bound = {left_start_point, right_start_point}; - const double lat_dist_to_front_bound = motion_utils::calcLateralOffset(front_bound, point); - if (lat_dist_to_front_bound < min_dist) { - return true; - } - - return false; -} - -bg_polygon createDrivablePolygon( - const std::vector & left_bound, - const std::vector & right_bound) -{ - bg_polygon drivable_area_poly; - - // left bound - for (const auto lp : left_bound) { - drivable_area_poly.outer().push_back(bg_point(lp.x, lp.y)); - } - - // right bound - auto reversed_right_bound = right_bound; - std::reverse(reversed_right_bound.begin(), reversed_right_bound.end()); - for (const auto rp : reversed_right_bound) { - drivable_area_poly.outer().push_back(bg_point(rp.x, rp.y)); - } - - drivable_area_poly.outer().push_back(drivable_area_poly.outer().front()); - return drivable_area_poly; -} -} // namespace - -namespace drivable_area_utils -{ -bool isOutsideDrivableAreaFromRectangleFootprint( - const autoware_auto_planning_msgs::msg::TrajectoryPoint & traj_point, - const std::vector & left_bound, - const std::vector & right_bound, const VehicleParam & vehicle_param, - const bool is_considering_footprint_edges) -{ - if (left_bound.empty() || right_bound.empty()) { - return false; - } - - const double base_to_right = (vehicle_param.wheel_tread / 2.0) + vehicle_param.right_overhang; - const double base_to_left = (vehicle_param.wheel_tread / 2.0) + vehicle_param.left_overhang; - - const double base_to_front = vehicle_param.length - vehicle_param.rear_overhang; - const double base_to_rear = vehicle_param.rear_overhang; - - const auto top_left_pos = - tier4_autoware_utils::calcOffsetPose(traj_point.pose, base_to_front, base_to_left, 0.0) - .position; - const auto top_right_pos = - tier4_autoware_utils::calcOffsetPose(traj_point.pose, base_to_front, -base_to_right, 0.0) - .position; - const auto bottom_right_pos = - tier4_autoware_utils::calcOffsetPose(traj_point.pose, -base_to_rear, -base_to_right, 0.0) - .position; - const auto bottom_left_pos = - tier4_autoware_utils::calcOffsetPose(traj_point.pose, -base_to_rear, base_to_left, 0.0) - .position; - - if (is_considering_footprint_edges) { - LinearRing2d footprint_polygon; - LineString2d left_bound_line; - LineString2d right_bound_line; - LineString2d back_bound_line; - - footprint_polygon.push_back({top_left_pos.x, top_left_pos.y}); - footprint_polygon.push_back({top_right_pos.x, top_right_pos.y}); - footprint_polygon.push_back({bottom_right_pos.x, bottom_right_pos.y}); - footprint_polygon.push_back({bottom_left_pos.x, bottom_left_pos.y}); - - bg::correct(footprint_polygon); - - for (const auto & p : left_bound) { - left_bound_line.push_back({p.x, p.y}); - } - - for (const auto & p : right_bound) { - right_bound_line.push_back({p.x, p.y}); - } - - back_bound_line = {left_bound_line.back(), right_bound_line.back()}; - - if ( - bg::intersects(footprint_polygon, left_bound_line) || - bg::intersects(footprint_polygon, right_bound_line) || - bg::intersects(footprint_polygon, back_bound_line)) { - return true; - } - return false; - } - - const bool front_top_left = isFrontDrivableArea(top_left_pos, left_bound, right_bound); - const bool front_top_right = isFrontDrivableArea(top_right_pos, left_bound, right_bound); - const bool front_bottom_left = isFrontDrivableArea(bottom_left_pos, left_bound, right_bound); - const bool front_bottom_right = isFrontDrivableArea(bottom_right_pos, left_bound, right_bound); - - // create rectangle - const auto drivable_area_poly = createDrivablePolygon(left_bound, right_bound); - - if ( - !front_top_left && !bg::within(bg_point(top_left_pos.x, top_left_pos.y), drivable_area_poly)) { - return true; - } - - if ( - !front_top_right && - !bg::within(bg_point(top_right_pos.x, top_right_pos.y), drivable_area_poly)) { - return true; - } - - if ( - !front_bottom_left && - !bg::within(bg_point(bottom_left_pos.x, bottom_left_pos.y), drivable_area_poly)) { - return true; - } - - if ( - !front_bottom_right && - !bg::within(bg_point(bottom_right_pos.x, bottom_right_pos.y), drivable_area_poly)) { - return true; - } - - return false; -} -} // namespace drivable_area_utils diff --git a/planning/obstacle_avoidance_planner/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp b/planning/obstacle_avoidance_planner/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp index e752c31f242da..19ff1775a1211 100644 --- a/planning/obstacle_avoidance_planner/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp +++ b/planning/obstacle_avoidance_planner/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp @@ -1,4 +1,4 @@ -// Copyright 2018-2019 Autoware Foundation +// Copyright 2023 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -18,50 +18,24 @@ #include #include -KinematicsBicycleModel::KinematicsBicycleModel(const double wheel_base, const double steer_limit) -: VehicleModelInterface(/* dim_x */ 2, /* dim_u */ 1, /* dim_y */ 2, wheel_base, steer_limit) +KinematicsBicycleModel::KinematicsBicycleModel(const double wheelbase, const double steer_limit) +: VehicleModelInterface(2, 1, 2, wheelbase, steer_limit) { } void KinematicsBicycleModel::calculateStateEquationMatrix( - Eigen::MatrixXd & Ad, Eigen::MatrixXd & Bd, Eigen::MatrixXd & Wd, const double ds) + Eigen::MatrixXd & Ad, Eigen::MatrixXd & Bd, Eigen::MatrixXd & Wd, const double curvature, + const double ds) const { - // const double epsilon = std::numeric_limits::epsilon(); - // constexpr double dt = 0.03; // assuming delta time for steer tau - - /* - const double lf = wheel_base_ - center_offset_from_base_; - const double lr = center_offset_from_base_; - */ - - const double delta_r = std::atan(wheel_base_ * curvature_); + const double delta_r = std::atan(wheelbase_ * curvature); const double cropped_delta_r = std::clamp(delta_r, -steer_limit_, steer_limit_); - // NOTE: cos(delta_r) will not be zero since that happens only when curvature is infinity - Ad << 1.0, ds, // - 0.0, 1.0; - - Bd << 0.0, ds / wheel_base_ / std::pow(std::cos(delta_r), 2.0); + // NOTE: cos(delta_r) will not be zero since curvature will not be infinity + Ad << 1.0, ds, 0.0, 1.0; - Wd << 0.0, -ds * curvature_ + ds / wheel_base_ * - (std::tan(cropped_delta_r) - - cropped_delta_r / std::pow(std::cos(cropped_delta_r), 2.0)); -} + Bd << 0.0, ds / wheelbase_ / std::pow(std::cos(delta_r), 2.0); -void KinematicsBicycleModel::calculateObservationMatrix(Eigen::MatrixXd & Cd) -{ - Cd << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0; -} - -void KinematicsBicycleModel::calculateObservationSparseMatrix( - std::vector> & Cd_vec) -{ - Cd_vec.clear(); - Cd_vec.push_back({0, 0, 1.0}); - Cd_vec.push_back({1, 1, 1.0}); -} - -void KinematicsBicycleModel::calculateReferenceInput(Eigen::MatrixXd * Uref) -{ - (*Uref)(0, 0) = std::atan(wheel_base_ * curvature_); + Wd << 0.0, -ds * curvature + ds / wheelbase_ * + (std::tan(cropped_delta_r) - + cropped_delta_r / std::pow(std::cos(cropped_delta_r), 2.0)); } diff --git a/planning/obstacle_avoidance_planner/src/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.cpp b/planning/obstacle_avoidance_planner/src/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.cpp deleted file mode 100644 index 3e317515c0d7e..0000000000000 --- a/planning/obstacle_avoidance_planner/src/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.cpp +++ /dev/null @@ -1,51 +0,0 @@ -// Copyright 2020 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "obstacle_avoidance_planner/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp" - -KinematicsBicycleModelNoDelay::KinematicsBicycleModelNoDelay( - const double & wheelbase, const double & steer_lim) -: VehicleModelInterface(/* dim_x */ 2, /* dim_u */ 1, /* dim_y */ 2) -{ - wheelbase_ = wheelbase; - steer_lim_ = steer_lim; -} - -void KinematicsBicycleModelNoDelay::calculateDiscreteMatrix( - Eigen::MatrixXd * Ad, Eigen::MatrixXd * Bd, Eigen::MatrixXd * Cd, Eigen::MatrixXd * Wd, - const double ds) -{ - auto sign = [](double x) { return (x > 0.0) - (x < 0.0); }; - - /* Linearize delta around delta_r (reference delta) */ - double delta_r = atan(wheelbase_ * curvature_); - if (abs(delta_r) >= steer_lim_) { - delta_r = steer_lim_ * static_cast(sign(delta_r)); - } - double cos_delta_r_squared_inv = 1 / (cos(delta_r) * cos(delta_r)); - - *Ad << 1.0, ds, 0.0, 1.0; - - *Bd << 0.0, ds / wheelbase_ * cos_delta_r_squared_inv; - - *Cd << 1.0, 0.0, 0.0, 1.0; - - // Wd << 0.0, -ds / wheelbase_ * delta_r * cos_delta_r_squared_inv; - *Wd << 0.0, (-ds * delta_r) / (wheelbase_ * cos_delta_r_squared_inv); -} - -void KinematicsBicycleModelNoDelay::calculateReferenceInput(Eigen::MatrixXd * Uref) -{ - (*Uref)(0, 0) = std::atan(wheelbase_ * curvature_); -} diff --git a/planning/obstacle_avoidance_planner/src/vehicle_model/vehicle_model_interface.cpp b/planning/obstacle_avoidance_planner/src/vehicle_model/vehicle_model_interface.cpp index 8e6522725c133..35da4a2eab467 100644 --- a/planning/obstacle_avoidance_planner/src/vehicle_model/vehicle_model_interface.cpp +++ b/planning/obstacle_avoidance_planner/src/vehicle_model/vehicle_model_interface.cpp @@ -1,4 +1,4 @@ -// Copyright 2018-2019 Autoware Foundation +// Copyright 2023 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -15,23 +15,12 @@ #include "obstacle_avoidance_planner/vehicle_model/vehicle_model_interface.hpp" VehicleModelInterface::VehicleModelInterface( - int dim_x, int dim_u, int dim_y, double wheel_base, double steer_limit) -: dim_x_(dim_x), - dim_u_(dim_u), - dim_y_(dim_y), - wheel_base_(wheel_base), - steer_limit_(steer_limit), - center_offset_from_base_(0.0) + const int dim_x, const int dim_u, const int dim_y, const double wheelbase, + const double steer_limit) +: dim_x_(dim_x), dim_u_(dim_u), dim_y_(dim_y), wheelbase_(wheelbase), steer_limit_(steer_limit) { } -int VehicleModelInterface::getDimX() { return dim_x_; } -int VehicleModelInterface::getDimU() { return dim_u_; } -int VehicleModelInterface::getDimY() { return dim_y_; } - -void VehicleModelInterface::updateCenterOffset(const double center_offset_from_base) -{ - center_offset_from_base_ = center_offset_from_base; -} - -void VehicleModelInterface::setCurvature(const double curvature) { curvature_ = curvature; } +int VehicleModelInterface::getDimX() const { return dim_x_; } +int VehicleModelInterface::getDimU() const { return dim_u_; } +int VehicleModelInterface::getDimY() const { return dim_y_; } diff --git a/planning/static_centerline_optimizer/CMakeLists.txt b/planning/static_centerline_optimizer/CMakeLists.txt index 73bb97761ba67..4731d44ed30bb 100644 --- a/planning/static_centerline_optimizer/CMakeLists.txt +++ b/planning/static_centerline_optimizer/CMakeLists.txt @@ -24,7 +24,7 @@ autoware_package() ament_auto_add_executable(main src/main.cpp src/static_centerline_optimizer_node.cpp - src/collision_free_optimizer_node.cpp + src/successive_trajectory_optimizer_node.cpp src/utils.cpp ) diff --git a/planning/static_centerline_optimizer/include/static_centerline_optimizer/successive_trajectory_optimizer_node.hpp b/planning/static_centerline_optimizer/include/static_centerline_optimizer/successive_trajectory_optimizer_node.hpp new file mode 100644 index 0000000000000..57ebb947b03c2 --- /dev/null +++ b/planning/static_centerline_optimizer/include/static_centerline_optimizer/successive_trajectory_optimizer_node.hpp @@ -0,0 +1,45 @@ +// Copyright 2022 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// NOTE: This file was copied from a part of implementation in +// https://github.com/autowarefoundation/autoware.universe/blob/main/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp + +#ifndef STATIC_CENTERLINE_OPTIMIZER__SUCCESSIVE_TRAJECTORY_OPTIMIZER_NODE_HPP_ +#define STATIC_CENTERLINE_OPTIMIZER__SUCCESSIVE_TRAJECTORY_OPTIMIZER_NODE_HPP_ + +#include "obstacle_avoidance_planner/node.hpp" +#include "static_centerline_optimizer/type_alias.hpp" + +#include +#include +#include +#include + +namespace static_centerline_optimizer +{ +class SuccessiveTrajectoryOptimizer : public obstacle_avoidance_planner::ObstacleAvoidancePlanner +{ +private: + // subscriber + rclcpp::Subscription::SharedPtr centerline_sub_; + +public: + explicit SuccessiveTrajectoryOptimizer(const rclcpp::NodeOptions & node_options); + + // subscriber callback functions + Trajectory on_centerline(const Path & path); +}; +} // namespace static_centerline_optimizer + +#endif // STATIC_CENTERLINE_OPTIMIZER__SUCCESSIVE_TRAJECTORY_OPTIMIZER_NODE_HPP_ diff --git a/planning/static_centerline_optimizer/launch/static_centerline_optimizer.launch.xml b/planning/static_centerline_optimizer/launch/static_centerline_optimizer.launch.xml index 2bc0aa6a2aedf..7b315adbf29e8 100644 --- a/planning/static_centerline_optimizer/launch/static_centerline_optimizer.launch.xml +++ b/planning/static_centerline_optimizer/launch/static_centerline_optimizer.launch.xml @@ -19,7 +19,7 @@ @@ -56,8 +56,8 @@ - - + + diff --git a/planning/static_centerline_optimizer/rviz/static_centerline_optimizer.rviz b/planning/static_centerline_optimizer/rviz/static_centerline_optimizer.rviz index 30a3776fd8526..b1834ec122f65 100644 --- a/planning/static_centerline_optimizer/rviz/static_centerline_optimizer.rviz +++ b/planning/static_centerline_optimizer/rviz/static_centerline_optimizer.rviz @@ -3,9 +3,12 @@ Panels: Help Height: 0 Name: Displays Property Tree Widget: - Expanded: ~ + Expanded: + - /Raw Centerline1/Topic1 + - /DrivableArea1/Topic1 + - /Optimized Centerline1/View Footprint1 Splitter Ratio: 0.550000011920929 - Tree Height: 388 + Tree Height: 386 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -143,16 +146,55 @@ Visualization Manager: Reliability Policy: Reliable Value: /static_centerline_optimizer/input_centerline Value: true - View LaneId: - Scale: 0.10000000149011612 - Value: true - View PathWithLaneId Footprint: - Alpha: 0.5 + View Footprint: + Alpha: 1 Color: 230; 230; 50 + Offset from BaseLink: 0 Rear Overhang: 1.0299999713897705 Value: true Vehicle Length: 4.769999980926514 Vehicle Width: 1.8300000429153442 + View LaneId: + Scale: 0.10000000149011612 + Value: true + View Point: + Alpha: 1 + Color: 0; 60; 255 + Offset: 0 + Radius: 0.10000000149011612 + Value: false + - Class: rviz_plugins/PathWithLaneId + Color Border Vel Max: 3 + Enabled: true + Name: DrivableArea + Topic: + Depth: 5 + Durability Policy: Transient Local + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /static_centerline_optimizer/input_centerline + Value: true + View Drivable Area: + Alpha: 0.9990000128746033 + Color: 0; 148; 205 + Value: true + Width: 0.30000001192092896 + View Path: + Alpha: 1 + Color: 0; 0; 0 + Constant Color: false + Value: false + Width: 2 + View Text Velocity: + Scale: 0.30000001192092896 + Value: false + View Velocity: + Alpha: 1 + Color: 0; 0; 0 + Constant Color: false + Scale: 0.30000001192092896 + Value: false - Class: rviz_plugins/TrajectoryFootprint Enabled: true Name: Optimized Centerline @@ -164,14 +206,15 @@ Visualization Manager: Reliability Policy: Reliable Value: /static_centerline_optimizer/output_centerline Value: true - View Trajectory Footprint: - Alpha: 0.5 - Color: 239; 41; 41 + View Footprint: + Alpha: 1 + Color: 255; 0; 0 + Offset from BaseLink: 0 Rear Overhang: 1.0299999713897705 Value: true Vehicle Length: 4.769999980926514 Vehicle Width: 1.8300000429153442 - View Trajectory Point: + View Point: Alpha: 1 Color: 0; 60; 255 Offset: 0 @@ -190,13 +233,20 @@ Visualization Manager: Reliability Policy: Reliable Value: /static_centerline_optimizer/debug/raw_centerline Value: false - View Path Footprint: + View Footprint: Alpha: 1 Color: 230; 230; 50 + Offset from BaseLink: 0 Rear Overhang: 1.0299999713897705 Value: true Vehicle Length: 4.769999980926514 Vehicle Width: 1.8300000429153442 + View Point: + Alpha: 1 + Color: 0; 60; 255 + Offset: 0 + Radius: 0.10000000149011612 + Value: false - Class: rviz_default_plugins/MarkerArray Enabled: true Name: MarkerArray @@ -362,7 +412,7 @@ Window Geometry: Hide Right Dock: false InitialPoseButtonPanel: collapsed: false - QMainWindow State: 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 + QMainWindow State: 000000ff00000000fd00000004000000000000019200000357fc020000000efb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003e000001c0000000ca00fffffffc0000020400000191000000c40100001dfa000000000100000002fb0000000a0056006900650077007301000000000000033c0000010000fffffffb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000000ffffffff0000008a00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d0065007200610100000682000000eb0000000000000000fb0000000a0049006d0061006700650100000505000002680000000000000000fb0000002c0049006e0069007400690061006c0050006f007300650042007500740074006f006e00500061006e0065006c000000068f000000de0000007a00fffffffb0000002c0049006e0069007400690061006c0050006f007300650042007500740074006f006e00500061006e0065006c000000068f000000de0000000000000000fb00000030005200650063006f0067006e006900740069006f006e0052006500730075006c0074004f006e0049006d00610067006500000001d3000000af0000000000000000fb00000024004100750074006f00770061007200650053007400610074006500500061006e0065006c000000029b000000fb0000026e00ffffff000000010000015f000006fffc0200000002fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000e7a0000005afc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000005afc0100000002fb0000000800540069006d00650100000000000007800000027100fffffffb0000000800540069006d00650100000000000004500000000000000000000005e80000035700000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: diff --git a/planning/static_centerline_optimizer/src/static_centerline_optimizer_node.cpp b/planning/static_centerline_optimizer/src/static_centerline_optimizer_node.cpp index df2e01cbcd31c..7187123c1dfd9 100644 --- a/planning/static_centerline_optimizer/src/static_centerline_optimizer_node.cpp +++ b/planning/static_centerline_optimizer/src/static_centerline_optimizer_node.cpp @@ -19,8 +19,8 @@ #include "lanelet2_extension/utility/utilities.hpp" #include "map_loader/lanelet2_map_loader_node.hpp" #include "motion_utils/motion_utils.hpp" -#include "static_centerline_optimizer/collision_free_optimizer_node.hpp" #include "static_centerline_optimizer/msg/points_with_lane_id.hpp" +#include "static_centerline_optimizer/successive_trajectory_optimizer_node.hpp" #include "static_centerline_optimizer/type_alias.hpp" #include "static_centerline_optimizer/utils.hpp" @@ -391,9 +391,8 @@ std::vector StaticCenterlineOptimizerNode::plan_path( RCLCPP_INFO(get_logger(), "Converted to path and published."); // optimize trajectory by the obstacle_avoidance_planner package - CollisionFreeOptimizerNode successive_path_optimizer(create_node_options()); - const auto optimized_traj = - successive_path_optimizer.pathCallback(std::make_shared(raw_path)); + SuccessiveTrajectoryOptimizer successive_trajectory_optimizer(create_node_options()); + const auto optimized_traj = successive_trajectory_optimizer.on_centerline(raw_path); pub_optimized_centerline_->publish(optimized_traj); const auto optimized_traj_points = motion_utils::convertToTrajectoryPointArray(optimized_traj); diff --git a/planning/static_centerline_optimizer/src/successive_trajectory_optimizer_node.cpp b/planning/static_centerline_optimizer/src/successive_trajectory_optimizer_node.cpp new file mode 100644 index 0000000000000..325c257e228f9 --- /dev/null +++ b/planning/static_centerline_optimizer/src/successive_trajectory_optimizer_node.cpp @@ -0,0 +1,109 @@ +// Copyright 2022 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "static_centerline_optimizer/successive_trajectory_optimizer_node.hpp" + +#include "motion_utils/motion_utils.hpp" +#include "obstacle_avoidance_planner/utils/trajectory_utils.hpp" +#include "tier4_autoware_utils/tier4_autoware_utils.hpp" +#include "vehicle_info_util/vehicle_info_util.hpp" + +#include +#include +#include +#include +#include +#include + +namespace static_centerline_optimizer +{ +SuccessiveTrajectoryOptimizer::SuccessiveTrajectoryOptimizer( + const rclcpp::NodeOptions & node_options) +: ObstacleAvoidancePlanner(node_options) +{ + // subscriber + centerline_sub_ = create_subscription( + "debug/raw_centerline", rclcpp::QoS{1}.transient_local(), + std::bind(&SuccessiveTrajectoryOptimizer::on_centerline, this, std::placeholders::_1)); + + // update parameters for replan_checker to execute optimization every cycle + std::vector parameters; + parameters.push_back(rclcpp::Parameter("replan.max_path_shape_around_ego_lat_dist", 100.0)); + parameters.push_back(rclcpp::Parameter("replan.max_ego_moving_dist", 100.0)); + parameters.push_back(rclcpp::Parameter("replan.max_goal_moving_dist", 100.0)); + parameters.push_back(rclcpp::Parameter("replan.max_delta_time_sec", 0.0)); + onParam(parameters); +} + +Trajectory SuccessiveTrajectoryOptimizer::on_centerline(const Path & path) +{ + if (path.points.size() < 2) { + RCLCPP_WARN(get_logger(), "Input path size is less than 2."); + return Trajectory{}; + } + + // parameters for input path sampling + const double resample_interval = 2.0; + const double valid_optimized_path_length = 30.0; + const double path_length = motion_utils::calcArcLength(path.points); + const size_t path_segment_num = static_cast(path_length / valid_optimized_path_length); + + const auto resampled_path = motion_utils::resamplePath(path, resample_interval); // TODO(murooka) + const auto resampled_traj_points = + obstacle_avoidance_planner::trajectory_utils::convertToTrajectoryPoints(resampled_path.points); + + const size_t initial_index = 3; + std::vector whole_optimized_traj_points; + + for (size_t i = 0; i < path_segment_num; ++i) { + // calculate initial pose to start optimization + const auto initial_pose = + resampled_path.points.at(initial_index + valid_optimized_path_length / resample_interval * i) + .pose; + + // create planner data + obstacle_avoidance_planner::PlannerData planner_data; + planner_data.traj_points = resampled_traj_points; + planner_data.left_bound = path.left_bound; + planner_data.right_bound = path.right_bound; + planner_data.ego_pose = initial_pose; + + const auto optimized_traj_points = optimizeTrajectory(planner_data); + + for (size_t j = 0; j < whole_optimized_traj_points.size(); ++j) { + const double dist = tier4_autoware_utils::calcDistance2d( + whole_optimized_traj_points.at(j), optimized_traj_points.front()); + if (dist < 0.5) { + const std::vector extracted_whole_optimized_traj_points{ + whole_optimized_traj_points.begin(), whole_optimized_traj_points.begin() + j - 1}; + whole_optimized_traj_points = extracted_whole_optimized_traj_points; + } + } + + for (size_t j = 0; j < optimized_traj_points.size(); ++j) { + whole_optimized_traj_points.push_back(optimized_traj_points.at(j)); + } + } + + // resample + auto output_traj_msg = motion_utils::resampleTrajectory( + motion_utils::convertToTrajectory(whole_optimized_traj_points), 1.0); + output_traj_msg.header = path.header; + + return output_traj_msg; +} +} // namespace static_centerline_optimizer + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(static_centerline_optimizer::SuccessiveTrajectoryOptimizer)