Ros2 v0.8.0 planning launch (autowarefoundation#59)

* [planning_launch] restore file name for ros2 porting Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com> * Add obstacle_stop_planner.yaml (autowarefoundation#82) Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * add surround obstacle checker options (autowarefoundation#86) * fix slow down param related to tier4/autoware.iv@a9cdcb2 (autowarefoundation#91) * fix parame max_steer_deg (autowarefoundation#92) * add refine_goal_search_radius_range (autowarefoundation#93) * Change default evaluation in motion velocity optimizer (autowarefoundation#97) * Use Linf * Add new line * Add maximum_deceleration parameter (autowarefoundation#98) * Add maximum_deceleration parameter * Change default value * Unable abort lane change (autowarefoundation#102) * add param stoping velocity and fix typo (autowarefoundation#106) * Add a parameter for minimum velocity for lane change (autowarefoundation#109) * Add parameters for collision check for lane change (autowarefoundation#110) * Add a parameter for disable collision check at prepare phase * Add parameters for collision check with predicted_path * Add a parameter for backward buffer for end of lane (autowarefoundation#114) * Add parameters for backward buffer for end of lane * Remove comment out * add extend_distance param (autowarefoundation#107) * add parameter of acc (autowarefoundation#129) Signed-off-by: Yuma Nihei <yuma.nihei@tier4.jp> * fix typo & change static object velocity thres in lane_change_planner (autowarefoundation#104) * change static object velocity thres Signed-off-by: Kosuke Murakami <kosuke.murakami@tier4.jp> * fix typo Signed-off-by: Kosuke Murakami <kosuke.murakami@tier4.jp> * Change minimum_lane_change_velocity (autowarefoundation#131) * Feature/update avoidance param (autowarefoundation#140) * update avoidance param Signed-off-by: Kosuke Murakami <kosuke.murakami@tier4.jp> * disable unnecesarry marker Signed-off-by: Kosuke Murakami <kosuke.murakami@tier4.jp> * modify min_behavior_stop_margin (autowarefoundation#127) * modify min_behavior_stop_margin Signed-off-by: Yamato Ando <yamato.ando@gmail.com> * Update obstacle_stop_planner.yaml Co-authored-by: Yukihiro Saito <yukky.saito@gmail.com> * Add expand_stop_range to obstacle_stop_planner.yaml (autowarefoundation#152) Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Update obstacle_stop_planner.yaml (autowarefoundation#153) * Visualize echo back goal_pose instead of 2D Nav Goal (autowarefoundation#150) * Visualize echo back goal_pose instead of 2D Nav Goal Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Fix mission_planning.launch Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Add surround_obstacle_checker.yaml (autowarefoundation#157) * Add parameter (autowarefoundation#158) * Revert "[planning_launch] restore file name for ros2 porting" This reverts commit 275f0df232323bf24627adea9cb08888c250625e. * fix namespace Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com> * fix relay Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com> * [planning_launch]: Add vehicle_param_file for turn signal decider Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * [planning_launch]: Change topic type of lane change approval Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> Co-authored-by: tkimura4 <tomoya.kimura@tier4.jp> Co-authored-by: Taichi Higashide <taichi.higashide@tier4.jp> Co-authored-by: Satoshi Tanaka <st14.828soccer@gmail.com> Co-authored-by: Fumiya Watanabe <rej55.g@gmail.com> Co-authored-by: Yuma Nihei <yuma.nihei@tier4.jp> Co-authored-by: Kosuke Murakami <kosuke.murakami@tier4.jp> Co-authored-by: YamatoAndo <yamato.ando@gmail.com> Co-authored-by: Yukihiro Saito <yukky.saito@gmail.com> Co-authored-by: hiroaki-ishikawa-t4 <57431939+hiroaki-ishikawa-t4@users.noreply.github.com> Co-authored-by: wep21 <border_goldenmarket@yahoo.co.jp>
tkimura4 · Feb 18, 2021 · 95c13d5 · 95c13d5
1 parent 3885d12
commit 95c13d5
Show file tree

Hide file tree

Showing 8 changed files with 108 additions and 27 deletions.
diff --git a/planning_launch/config/adaptive_cruise_control.yaml b/planning_launch/config/adaptive_cruise_control.yaml
@@ -0,0 +1,37 @@
+/**:
+  ros__parameters:
+    adaptive_cruise_control:
+      # Adaptive Cruise Control (ACC) config
+      use_object_to_estimate_vel: true # use tracking objects for estimating object velocity or not
+      use_pcl_to_estimate_vel: true # use pcl for estimating object velocity or not
+      consider_obj_velocity: true # consider forward vehicle velocity to ACC or not
+
+      # general parameter for ACC
+      obstacle_stop_velocity_thresh: 1.0 # threshold of forward obstacle velocity to insert stop line (to stop acc) [m/s]
+      emergency_stop_acceleration: -5.0 # supposed minimum acceleration (deceleration) in emergency stop [m/ss]
+      emergency_stop_idling_time: 0.5 # supposed idling time to start emergency stop [s]
+      min_dist_stop: 4.0 # minimum distance of emergency stop [m]
+      max_standard_acceleration: 0.5 # supposed maximum acceleration in active cruise control [m/ss]
+      min_standard_acceleration: -1.0 # supposed minimum acceleration (deceleration) in active cruise control
+      standard_idling_time: 0.5 # supposed idling time to react object in active cruise control [s]
+      min_dist_standard: 4.0 # minimum distance in active cruise control [m]
+      obstacle_min_standard_acceleration: -1.5 # supposed minimum acceleration of forward obstacle [m/ss]
+      margin_rate_to_change_vel: 0.3 # margin to insert upper velocity [-]
+
+      # pid parameter for ACC
+      p_coefficient_positive: 0.1 # coefficient P in PID control (used when target dist -current_dist >=0) [-]
+      p_coefficient_negative: 0.3 # coefficient P in PID control (used when target dist -current_dist <0) [-]
+      d_coefficient_positive: 0.0 # coefficient D in PID control (used when delta_dist >=0) [-]
+      d_coefficient_negative: 0.2 # coefficient D in PID control (used when delta_dist <0) [-]
+
+      # parameter for object velocity estimation
+      object_polygon_length_margin: 2.0 # The distance to extend the polygon length the object in pointcloud-object matching [m]
+      object_polygon_width_margin: 0.5 # The distance to extend the polygon width the object in pointcloud-object matching [m]
+      valid_estimated_vel_diff_time: 1.0 # Maximum time difference treated as continuous points in speed estimation using a point cloud [s]
+      valid_vel_que_time: 0.5 # Time width of information used for speed estimation in speed estimation using a point cloud [s]
+      valid_estimated_vel_max: 20.0 # Maximum value of valid speed estimation results in speed estimation using a point cloud [m/s]
+      valid_estimated_vel_min: -20.0 # Minimum value of valid speed estimation results in speed estimation using a point cloud [m/s]
+      thresh_vel_to_stop: 1.5 # Embed a stop line if the maximum speed calculated by ACC is lower than this speed [m/s]
+      lowpass_gain_of_upper_velocity: 0.75 # Lowpass-gain of upper velocity
+      use_rough_velocity_estimation: false # Use rough estimated velocity if the velocity estimation is failed (#### If this parameter is true, the vehicle may collide with the front car. Be careful. ####)
+      rough_velocity_rate: 0.9 # In the rough velocity estimation, the velocity of front car is estimated as self current velocity * this value
diff --git a/planning_launch/config/obstacle_stop_planner.yaml b/planning_launch/config/obstacle_stop_planner.yaml
@@ -0,0 +1,15 @@
+/**:
+  ros__parameters:
+    stop_planner:
+      stop_margin: 5.0 # stop margin distance from obstacle on the path [m]
+      min_behavior_stop_margin: 5.0 # stop margin distance when any other stop point is inserted in stop margin [m]
+      step_length: 1.0 # step length for pointcloud search range [m]
+      extend_distance: 0.0 # extend trajectory to consider after goal obstacle in the extend_distance
+      expand_stop_range: 0.0 # margin of vehicle footprint [m]
+
+    slow_down_planner:
+      slow_down_margin: 5.0 # margin distance from slow down point [m]
+      expand_slow_down_range: 1.0 # offset from vehicle side edge for expanding the search area of the surrounding point cloud [m]
+      max_slow_down_vel: 1.38 # max slow down velocity [m/s]
+      min_slow_down_vel: 0.28 # min slow down velocity [m/s]
+      max_deceleration: 2.0   # max slow down deceleration [m/s2]
diff --git a/.../config/scenario_planning/common/motion_velocity_optimizer/motion_velocity_optimizer.yaml b/.../config/scenario_planning/common/motion_velocity_optimizer/motion_velocity_optimizer.yaml
@@ -6,7 +6,7 @@
 
     # curve parameters
     max_lateral_accel: 0.5             # max lateral acceleration limit [m/ss]
-    min_curve_velocity: 2.74           # min velocity at lateral acceleration limit [m/ss]
+    min_curve_velocity: 2.74           # min velocity at lateral acceleration limit [m/s]
     decel_distance_before_curve: 3.5   # slow speed distance before a curve for lateral acceleration limit
     decel_distance_after_curve: 2.0    # slow speed distance after a curve for lateral acceleration limit
 
@@ -17,6 +17,10 @@
     engage_exit_ratio: 0.5             # exit engage sequence to normal velocity plannning when the velocity exceeds engage_exit_ratio x engage_velocity.
     stop_dist_to_prohibit_engage: 0.5  # if the stop point is in this distance, the speed is set to 0 not to move the vehicle [m]
 
+    # stop velocity
+    stopping_velocity: 2.778  # change target velocity to this value before v=0 point [m/s]
+    stopping_distance: 0.0    # distance for the stopping_velocity [m]. 0 means the stopping velocity is not applied.
+
     extract_ahead_dist: 200.0         # forward trajectory distance used for planning [m]
     extract_behind_dist: 5.0          # backward trajectory distance used for planning [m]
     delta_yaw_threshold: 1.0472       # Allowed delta yaw between ego pose and trajecotory pose [radian]
@@ -31,8 +35,8 @@
     # L2: jerk = 100.0, v_weight = 100000.0, a_weight = 1000.0
     # Linf: jerk = 200.0, v_weight = 100000.0, a_weight = 5000.0
 
-    pseudo_jerk_weight: 100.0 # weight for "smoothness" cost
+    pseudo_jerk_weight: 200.0 # weight for "smoothness" cost
     over_v_weight: 100000.0   # weight for "overspeed limit" cost
-    over_a_weight: 1000.0     # weight for "overaccel limit" cost
+    over_a_weight: 5000.0     # weight for "overaccel limit" cost
 
-    algorithm_type: "L2" # Option : L2, Linf
+    algorithm_type: "Linf" # Option : L2, Linf
diff --git a/...g/lane_driving/motion_planning/obstacle_avoidance_planner/obstacle_avoidance_planner.yaml b/...g/lane_driving/motion_planning/obstacle_avoidance_planner/obstacle_avoidance_planner.yaml
@@ -13,7 +13,7 @@
 
     # clearance for unique points
     clearance_for_straight_line_: 0.05 # minimum optimizing range around straight points
-    clearance_for_joint_: 3.2 # minimum optimizing range around joint points
+    clearance_for_joint_: 0.1 # minimum optimizing range around joint points
     clearance_for_only_smoothing: 0.1 # minimum optimizing range when applygin 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
@@ -59,16 +59,16 @@
 
     # mpt param
     # vehicle param for mpt
-    max_steer_deg: 30.0 # max steering angle [deg]
+    max_steer_deg: 40.0 # max steering angle [deg]
     steer_tau: 0.1 # steering dynamics time constant (1d approzimation) [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]
-    num_fix_points_for_mpt: 8 # number of fixing points around ego vehicle
+    forward_fixing_mpt_distance: 10 # number of fixing points around ego vehicle
 
     # optimization param for mpt
-    is_hard_fixing_terminal_point: true # hard fixing terminal point
+    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

diff --git a/...ing/lane_driving/motion_planning/surround_obstacle_checker/surround_obstacle_checker.yaml b/...ing/lane_driving/motion_planning/surround_obstacle_checker/surround_obstacle_checker.yaml
@@ -0,0 +1,12 @@
+/**:
+  ros__parameters:
+
+    # obstacle check
+    use_pointcloud: true # use pointcloud as obstacle check
+    use_dynamic_object: true # use dynamic object as obstacle check
+    surround_check_distance: 0.5 # if objects exist in this distance, transit to "exist-surrounding-obstacle" status [m]
+    surround_check_recover_distance: 0.8 # if no object exists in this distance, transit to "non-surrounding-obstacle" status [m]
+    state_clear_time: 2.0
+
+    # ego stop state
+    stop_state_ego_speed: 0.1 #[m/s]
diff --git a/planning_launch/launch/mission_planning/mission_planning.launch.xml b/planning_launch/launch/mission_planning/mission_planning.launch.xml
@@ -1,16 +1,7 @@
 <launch>
-  <arg name="input_topic_name" default="/planning/mission_planning/goal"/>
-  <arg name="output_topic_name" default="/planning/mission_planning/route"/>
-  <arg name="map_topic_name" default="/map/vector_map"/>
-  <arg name="visualization_topic_name" default="/planning/mission_planning/route_marker"/>
-  <arg name="checkpoint_topic_name" default="/planning/mission_planning/checkpoint"/>
-  <node pkg="mission_planner" exec="mission_planner" name="mission_planner" output="screen">
-    <param name="map_frame" value="map"/>
-    <param name="base_link_frame" value="base_link"/>
-    <remap from="input/goal_pose" to="$(var input_topic_name)"/>
-    <remap from="input/checkpoint" to="$(var checkpoint_topic_name)"/>
-    <remap from="input/vector_map" to="$(var map_topic_name)"/>
-    <remap from="output/route" to="$(var output_topic_name)"/>
-    <remap from="debug/route_marker" to="$(var visualization_topic_name)"/>
-  </node>
+  <include file="$(find-pkg-share mission_planner)/launch/mission_planner.launch.xml">
+  </include>
+
+  <include file="$(find-pkg-share mission_planner)/launch/goal_pose_visualizer.launch.xml">
+  </include>
 </launch>
diff --git a/...unch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml b/...unch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml
@@ -23,24 +23,34 @@
     <param name="max_accel" value="-5.0" />
     <param name="lane_change_prepare_duration" value="4.0" />
     <param name="lane_changing_duration" value="8.0" />
+    <param name="backward_length_buffer_for_end_of_lane" value="5.0" />
+    <param name="lane_change_finish_judge_buffer" value="3.0" />
     <param name="minimum_lane_change_length" value="12.0" />
+    <param name="minimum_lane_change_velocity" value="5.6" />
     <param name="prediction_duration" value="8.0" />
     <param name="prediction_time_resolution" value="0.5" />
     <param name="drivable_area_resolution" value="0.1" />
     <param name="drivable_area_width" value="100.0" />
     <param name="drivable_area_height" value="50.0" />
-    <param name="static_obstale_velocity_thresh" value="0.1" />
-    <param name="enable_abort_lane_change" value="true" />
+    <param name="static_obstacle_velocity_thresh" value="1.5" />
+    <param name="maximum_deceleration" value="1.0" />
+    <param name="enable_abort_lane_change" value="false" />
+    <param name="enable_collision_check_at_prepare_phase" value="false" />
+    <param name="use_predicted_path_outside_lanelet" value="false" />
+    <param name="use_all_predicted_path" value="false" />
+    <param name="refine_goal_search_radius_range" value="7.5" />
   </node>
 
-  <executable cmd="ros2 topic pub /planning/scenario_planning/lane_driving/lane_change_approval std_msgs/msg/Bool 'data: true' -r 10"/>
+  <executable cmd="ros2 topic pub /planning/scenario_planning/lane_driving/lane_change_approval
+                   autoware_planning_msgs/msg/LaneChangeCommand 'command: true' -r 10"/>
 
   <node pkg="turn_signal_decider" exec="turn_signal_decider" name="turn_signal_decider" output="screen">
     <remap from="input/path_with_lane_id" to="path_with_lane_id" />
     <remap from="input/vector_map" to="$(var map_topic_name)" />
     <remap from="output/turn_signal_cmd" to="/planning/turn_signal_decider/turn_signal_cmd" />
     <param name="lane_change_search_distance" value="30.0" />
     <param name="intersection_search_distance" value="30.0" />
+    <param from="$(var vehicle_param_file)" />
   </node>
 
   <include file="$(find-pkg-share behavior_velocity_planner)/launch/behavior_velocity_planner.launch.xml">

diff --git a/...g_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml b/...g_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml
@@ -1,6 +1,7 @@
 <launch>
   <arg name="input_path_topic" default="/planning/scenario_planning/lane_driving/behavior_planning/path" />
   <arg name="vehicle_param_file" />
+  <arg name="use_surround_obstacle_check" default="true" />
 
   <!-- path planning for avoiding obstacle with dynamic object info -->
   <group>
@@ -14,23 +15,34 @@
 
 
   <!-- surround obstacle checker (Do not start if there are pedestrian/bicycles around ego-car) -->
-  <group>
+  <group if="$(var use_surround_obstacle_check)">
     <include file="$(find-pkg-share surround_obstacle_checker)/launch/surround_obstacle_checker.launch.xml">
       <arg name="input_trajectory" value="obstacle_avoidance_planner/trajectory" />
       <arg name="output_trajectory" value="surround_obstacle_checker/trajectory" />
       <arg name="vehicle_param_file" value="$(var vehicle_param_file)" />
+      <arg name="param_path" value="$(find-pkg-share planning_launch)/config/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/surround_obstacle_checker.yaml"/>
     </include>
   </group>
 
+  <!-- skip surround obstacle checker -->
+  <group unless="$(var use_surround_obstacle_check)">
+    <node pkg="topic_tools" exec="relay" name="skip_surround_obstacle_check_relay" output="log" >
+      <param name="input_topic" value="obstacle_avoidance_planner/trajectory" />
+      <param name="output_topic" value="surround_obstacle_checker/trajectory" />
+      <param name="type" value="autoware_planning_msgs/msg/Trajectory" />
+    </node>
+  </group>
 
   <!-- stop velocity planning with obstacle pointcloud info -->
-
   <group>
     <include file="$(find-pkg-share obstacle_stop_planner)/launch/obstacle_stop_planner.launch.xml">
       <arg name="input_trajectory" value="surround_obstacle_checker/trajectory" />
       <arg name="input_pointcloud" value="/sensing/lidar/no_ground/pointcloud" />
       <arg name="output_trajectory" value="/planning/scenario_planning/lane_driving/trajectory" />
       <arg name="vehicle_param_file" value="$(var vehicle_param_file)" />
+      <arg name="acc_param_file" value="$(find-pkg-share planning_launch)/config/adaptive_cruise_control.yaml" />
+      <arg name="param_file" value="$(find-pkg-share planning_launch)/config/obstacle_stop_planner.yaml" />
+      <arg name="enable_slow_down" value="false" />
     </include>
   </group>