feat(obstacle avoidance planner): fix out curve, make calculation cos…

…t low, make optimization stable, refactor, etc. (tier4#233) * feat (obstacle avoidance planner) fix out curve, make optimization stable, make computation cost low, etc Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * fix typos Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * remove unnecessary codes Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * minor refactoring Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * add path shape change detection for replan Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * add feature explanation in README Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * move some parameters to advanced Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * calcualte radius num automatically Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * update config in tier4_planning_launch Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * truncate path to detect path change, and tune path change detection Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * disable yaw slerp Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * fix ci error Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>
boyali · Oct 19, 2022 · 7b8364c · 7b8364c
1 parent e609b5b
commit 7b8364c
Show file tree

Hide file tree

Showing 38 changed files with 7,632 additions and 5,115 deletions.
diff --git a/..._driving/motion_planning/obstacle_avoidance_planner/obstacle_avoidance_planner.param.yaml b/..._driving/motion_planning/obstacle_avoidance_planner/obstacle_avoidance_planner.param.yaml
@@ -1,90 +1,173 @@
 /**:
   ros__parameters:
-    # trajectory total/fixing length
-    trajectory_length: 300 # total trajectory length[m]
-    forward_fixing_distance: 5.0 # forward fixing length from base_link[m]
-    backward_fixing_distance: 3.0 # backward fixing length from base_link[m]
-
-    # clearance(distance) when generating trajectory
-    clearance_from_road: 0.2 # clearance from road boundary[m]
-    clearance_from_object: 1.0 # clearance from object[m]
-    min_object_clearance_for_joint: 3.2 # minimum clearance from object[m] when judging is_including_only_smooth_range
-    extra_desired_clearance_from_road: 0.0 # extra desired clearance from road
-
-    # clearance for unique points
-    clearance_for_straight_line: 0.05 # minimum optimizing range around straight points
-    clearance_for_joint: 0.1 # minimum optimizing range around joint points
-    clearance_for_only_smoothing: 0.1 # minimum optimizing range when applying only smoothing
-    range_for_extend_joint: 0.1 # minimum optimizing range around joint points for extending
-    clearance_from_object_for_straight: 10.0 # minimum clearance from object when judging straight line
-
-    # avoiding param
-    max_avoiding_objects_velocity_ms: 0.5 # maximum velocity for avoiding objects[m/s]
-    max_avoiding_ego_velocity_ms: 6.0 # maximum ego velocity when avoiding objects[m/s]
-    center_line_width: 1.7 # replaced with /vehicle_info/vehicle_width when is_using_vehicle_config is true, center line width around path points used for judging that object is required to avoid or not
-    acceleration_for_non_deceleration_range: 1.0 # assumed acceleration for calculating non deceleration range
-
-    # solving quadratic programming
-    qp_max_iteration: 10000 # max iteration when solving QP
-    qp_eps_abs: 1.0e-8 # eps abs when solving OSQP
-    qp_eps_rel: 1.0e-11 # eps rel when solving OSQP
-    qp_eps_abs_for_extending: 1.0e-8 # eps abs when solving OSQP for extending
-    qp_eps_rel_for_extending: 1.0e-9 # eps rel when solving OSQP for extending
-    qp_eps_abs_for_visualizing: 1.0e-6 # eps abs when solving OSQP for visualizing
-    qp_eps_rel_for_visualizing: 1.0e-8 # eps rel when solving OSQP for visualizing
-
-    # constrain space
-    is_getting_constraints_close2path_points: true # generate trajectory closer to path points
-    max_x_constrain_search_range: 0.0 # maximum x search range in local coordinate
-    coef_x_constrain_search_resolution: 1.0 # coef for fine sampling when exploring x direction
-    coef_y_constrain_search_resolution: 0.5 # coef for fine sampling when exploring y direction
-    keep_space_shape_x: 0.2 # keep space for x direction from base_link[m]
-    keep_space_shape_y: 0.1 # replaced with /vehicle_info/vehicle_width when is_using_vehicle_config is true, keep space for y direction from base_link[m]
-    max_lon_space_for_driveable_constraint: 0.5 # maximum lon(x) space when optimizing point[m]
-
-    is_publishing_clearance_map: true # publish clearance map as nav_msgs::OccupancyGrid
-    is_publishing_area_with_objects: true # publish occupancy map as nav_msgs::OccupancyGrid
-    enable_avoidance: true # enable avoidance function
-    is_using_vehicle_config: true # use vehicle config
-    num_sampling_points: 100 # number of optimizing points
-    num_joint_buffer_points: 3 # number of joint buffer points
-    num_joint_buffer_points_for_extending: 6 # number of joint buffer points for extending
-    num_offset_for_begin_idx: 2 # number of shifting points for beginning non-fix idx
-    num_fix_points_for_extending: 50 # number of fixing points when extending
-    delta_arc_length_for_optimization: 1.0 # delta arc length when optimizing[m]
-    delta_arc_length_for_trajectory: 0.1 # delta arc length for trajectory[m]
-    delta_dist_threshold_for_closest_point: 3.0 # delta dist thres for closest point[m]
-    delta_yaw_threshold_for_closest_point: 1.046 #M_PI/3.0, delta yaw thres for closest point
-    delta_yaw_threshold_for_straight: 0.02 # delta dist thres for straight point
+    option:
+      # publish
+      is_publishing_debug_visualization_marker: true
+      is_publishing_clearance_map: false # publish clearance map as nav_msgs::OccupancyGrid
+      is_publishing_object_clearance_map: false # publish clearance map as nav_msgs::OccupancyGrid
+      is_publishing_area_with_objects: false # publish occupancy map as nav_msgs::OccupancyGrid
+
+      is_stopping_if_outside_drivable_area: true # stop if the ego's footprint will be outside the drivable area
+
+      # show
+      is_showing_debug_info: false
+      is_showing_calculation_time: false
+
+      # other
+      enable_avoidance: false # enable avoidance function
+      enable_pre_smoothing: true # enable EB
+      skip_optimization: false # skip MPT and EB
+      reset_prev_optimization: false
+
+    common:
+      # sampling
+      num_sampling_points: 100 # number of optimizing points
+
+      # trajectory total/fixing length
+      trajectory_length: 300.0 # total trajectory length[m]
+
+      forward_fixing_min_distance: 1.0 # number of fixing points around ego vehicle [m]
+      forward_fixing_min_time: 0.5 # forward fixing time with current velocity [s]
+
+      backward_fixing_distance: 5.0 # backward fixing length from base_link [m]
+      delta_arc_length_for_trajectory: 0.1 # delta arc length for trajectory [m]
+
+      delta_dist_threshold_for_closest_point: 3.0 # delta dist thres for closest point [m]
+      delta_yaw_threshold_for_closest_point: 1.046 #M_PI/3.0, delta yaw thres for closest point
+      delta_yaw_threshold_for_straight: 0.02 # delta dist thres for straight point
+
+      num_fix_points_for_extending: 50 # number of fixing points when extending
+      max_dist_for_extending_end_point: 0.0001 # minimum delta dist thres for extending last point [m]
+
+    object:  # avoiding object
+      max_avoiding_objects_velocity_ms: 0.5 # maximum velocity for avoiding objects [m/s]
+      max_avoiding_ego_velocity_ms: 6.0 # maximum ego velocity when avoiding objects [m/s]
+
+      avoiding_object_type:
+        unknown: true
+        car: true
+        truck: true
+        bus: true
+        bicycle: true
+        motorbike: true
+        pedestrian: true
+        animal: true
 
     # mpt param
-    # vehicle param for mpt
-    max_steer_deg: 40.0 # max steering angle [deg]
-    steer_tau: 0.1 # steering dynamics time constant (1d approximation) [s]
-
-    # trajectory param for mpt
-    num_curvature_sampling_points: 5 # number of sampling points when calculating curvature
-    delta_arc_length_for_mpt_points: 0.5 # delta arc length when generating mpt[m]
-    forward_fixing_mpt_distance: 10 # number of fixing points around ego vehicle
-
-    # optimization param for mpt
-    is_hard_fixing_terminal_point: false # hard fixing terminal point
-    base_point_weight: 2000.0 # slack weight for lateral error around base_link
-    top_point_weight: 1000.0 # slack weight for lateral error around vehicle-top point
-    mid_point_weight: 1000.0 # slack weight for lateral error around the middle point
-    lat_error_weight: 10.0 # weight for lateral error
-    yaw_error_weight: 0.0 # weight for yaw error
-    steer_input_weight: 0.01 # weight for steering input
-    steer_rate_weight: 1.0 # weight for steering rate
-    steer_acc_weight: 0.000001 # weight for steering acceleration
-    terminal_lat_error_weight: 0.0 # weight for lateral error at terminal point
-    terminal_yaw_error_weight: 100.0 # weight for yaw error at terminal point
-    terminal_path_lat_error_weight: 1000.0 # weight for lateral error at path end point
-    terminal_path_yaw_error_weight: 1000.0 # weight for yaw error at path end point
-    zero_ff_steer_angle: 0.5 # threshold that feed-forward angle becomes zero
+    mpt:
+      option:
+        steer_limit_constraint: true
+        fix_points_around_ego: true
+        # plan_from_ego: false
+        visualize_sampling_num: 1
+        enable_manual_warm_start: true
+        enable_warm_start: true # false
+
+      common:
+        num_curvature_sampling_points: 5 # number of sampling points when calculating curvature
+        delta_arc_length_for_mpt_points: 0.5 # delta arc length when generating mpt [m]
+
+      kinematics:
+        max_steer_deg: 40.0 # max steering angle [deg]
+
+        # If this parameter is commented out, the parameter is set as below by default.
+        # The logic could be `optimization_center_offset = vehicle_info.wheel_base * 0.8`
+        # The 0.8 scale is adopted as it performed the best.
+        # optimization_center_offset: 2.3 # optimization center offset from base link
 
     # replanning & trimming trajectory param outside algorithm
-    min_delta_dist_for_replan: 10.0 # minimum delta dist thres for replan[m]
-    min_delta_time_sec_for_replan: 1.0 # minimum delta time for replan[second]
-    max_dist_for_extending_end_point: 5.0 # minimum delta dist thres for extending last point[m]
-    distance_for_path_shape_change_detection: 2.0 # minimum delta dist thres for detecting path shape change
+    replan:
+      max_path_shape_change_dist: 0.3 # threshold of path shape change from behavior path [m]
+      max_ego_moving_dist_for_replan: 3.0 # threshold of ego's moving distance for replan [m]
+      max_delta_time_sec_for_replan: 1.0 # threshold of delta time for replan [second]
+
+    # advanced parameters to improve performance as much as possible
+    advanced:
+      option:
+        # check if planned trajectory is outside drivable area
+        drivability_check:
+          # true: vehicle shape is considered as a set of circles
+          # false: vehicle shape is considered as footprint (= rectangle)
+          use_vehicle_circles: false
+
+          # parameters only when use_vehicle_circles is true
+          vehicle_circles:
+            use_manual_vehicle_circles: false
+            num_for_constraints: 4
+
+            # parameters only when use_manual_vehicle_circles is true
+            longitudinal_offsets: [-0.15, 0.88, 1.9, 2.95]
+            radius: 0.95
+
+            # parameters only when use_manual_vehicle_circles is false
+            radius_ratio: 0.9
+            # If this parameter is commented out, the parameter is calculated automatically
+            # based on the vehicle length and width
+            # num_for_radius: 4
+
+      eb:
+        common:
+          num_joint_buffer_points: 3 # number of joint buffer points
+          num_offset_for_begin_idx: 2 # number of shifting points for beginning non-fix idx
+          delta_arc_length_for_eb: 0.6 # 1.0 # delta arc length when optimizing[m] When the value is around 1.0, overshoot at the corner happens.
+          num_sampling_points_for_eb: 95 # number of optimizing points # must be longer than mpt points
+
+        clearance:
+          clearance_for_straight_line: 0.05 # minimum optimizing range around straight points
+          clearance_for_joint: 0.1 # minimum optimizing range around joint points
+          clearance_for_only_smoothing: 0.1 # minimum optimizing range when applying only smoothing
+
+        qp:
+          max_iteration: 10000 # max iteration when solving QP
+          eps_abs: 1.0e-8 # eps abs when solving OSQP
+          eps_rel: 1.0e-10 # eps rel when solving OSQP
+
+      mpt:
+        clearance:  # clearance(distance) between vehicle and roads/objects when generating trajectory
+          hard_clearance_from_road: 0.0 # clearance from road boundary[m]
+          soft_clearance_from_road: 0.1 # clearance from road boundary[m]
+          soft_second_clearance_from_road: 1.0 # clearance from road boundary[m]
+          clearance_from_object: 1.0 # clearance from object[m]
+          extra_desired_clearance_from_road: 0.0 # extra desired clearance from road
+
+        weight:
+          soft_avoidance_weight: 1000.0 # slack weight for lateral error around the middle point
+          soft_second_avoidance_weight: 100.0 # slack weight for lateral error around the middle point
+
+          lat_error_weight: 100.0 # weight for lateral error
+          yaw_error_weight: 0.0 # weight for yaw error
+          yaw_error_rate_weight: 0.0 # weight for yaw error rate
+          steer_input_weight: 10.0 # weight for steering input
+          steer_rate_weight: 10.0 # weight for steering rate
+
+          obstacle_avoid_lat_error_weight: 3.0 # weight for lateral error
+          obstacle_avoid_yaw_error_weight: 0.0 # weight for yaw error
+          obstacle_avoid_steer_input_weight: 1000.0 # weight for yaw error
+          near_objects_length: 30.0 # weight for yaw error
+
+          terminal_lat_error_weight: 100.0 # weight for lateral error at terminal point
+          terminal_yaw_error_weight: 100.0 # weight for yaw error at terminal point
+          terminal_path_lat_error_weight: 1000.0 # weight for lateral error at path end point
+          terminal_path_yaw_error_weight: 1000.0 # weight for yaw error at path end point
+
+        # check if planned trajectory is outside drivable area
+        collision_free_constraints:
+          option:
+            l_inf_norm: true
+            soft_constraint: true
+            hard_constraint: false
+            # two_step_soft_constraint: false
+
+          vehicle_circles:
+            use_manual_vehicle_circles: false
+            num_for_constraints: 3
+
+            # parameters only when use_manual_vehicle_circles is true
+            longitudinal_offsets: [-0.15, 0.88, 1.9, 2.95]
+            radius: 0.95
+
+            # parameters only when use_manual_vehicle_circles is false
+            radius_ratio: 0.8
+            # If this parameter is commented out, the parameter is calculated automatically
+            # based on the vehicle length and width
+            # num_for_radius: 4
diff --git a/planning/obstacle_avoidance_planner/CMakeLists.txt b/planning/obstacle_avoidance_planner/CMakeLists.txt
@@ -19,12 +19,12 @@ find_package(OpenCV REQUIRED)
 ament_auto_add_library(obstacle_avoidance_planner SHARED
   src/vehicle_model/vehicle_model_interface.cpp
   src/vehicle_model/vehicle_model_bicycle_kinematics.cpp
-  src/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.cpp
-  src/util.cpp
-  src/debug.cpp
+  src/utils.cpp
+  src/costmap_generator.cpp
+  src/debug_visualization.cpp
   src/eb_path_optimizer.cpp
   src/mpt_optimizer.cpp
-  src/process_cv.cpp
+  src/cv_utils.cpp
   src/node.cpp
 )