feat(mission_planning): add pit-in control (rebased)

.cspell.json

@@ -137,6 +137,7 @@
     "tempature",
     "rsample",
     "coeffs",
-    "softplus"
+    "softplus",
+    "mpss"
   ]
 }

.gitignore

@@ -0,0 +1 @@
+.vscode

aichallenge/.gitignore

@@ -0,0 +1,2 @@
+capture
+result-details.json

aichallenge/workspace/.gitignore

@@ -3,4 +3,5 @@
 /log
 
 .vscode
-*.code-workspace
+*.code-workspace
+result-details.json

aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/launch/aichallenge_submit.launch.xml

@@ -22,6 +22,6 @@
 
   <!-- place a goal pose anywhere you like-->
   <node pkg="goal_pose_setter" exec="goal_pose_setter_node" name="goal_pose_setter" output="screen">
-    <param from="$(find-pkg-share goal_pose_setter)/config/default_goal_pose.param.yaml" />
+    <param from="$(find-pkg-share booars_launch)/config/planning/goal_pose.param.yaml" />
   </node>
 </launch>

aichallenge/workspace/src/aichallenge_submit/booars_launch/config/planning/costmap_generator.param.yaml

@@ -4,7 +4,7 @@
     map_frame_id: "map"
     costmap_center_frame_id: "base_link"
     costmap_width: 50.0     # [m]
-    costmap_resolution: 0.2 # [m/cell]
+    costmap_resolution: 0.1 # [m/cell]
     multi_layered_costmap:
       layers:
         - "cached_lanelet_layer"
@@ -13,7 +13,7 @@
         type: "cached_lanelet"
         map_topic: "/map/vector_map"
         costmap_topic: "~/debug/cached_costmap"
-        inflation_radius: 1.8 # [m]
+        inflation_radius: 1.4 # [m]
         cached_costmap:
           min_x: 89607.0  # [m]
           max_x: 89687.0  # [m]
@@ -23,4 +23,4 @@
       predicted_object_layer:
         type: "predicted_object"
         predicted_objects_topic: "/perception/object_recognition/objects"
-        distance_threshold: 1.3
+        distance_threshold: 1.2

aichallenge/workspace/src/aichallenge_submit/booars_launch/config/planning/goal_pose.param.yaml

@@ -0,0 +1,32 @@
+/**:
+  ros__parameters:
+    goal.position.x: 89653.7
+    goal.position.y: 43122.5
+    goal.position.z: 0.0
+    goal.orientation.x: 0.0
+    goal.orientation.y: 0.0
+    goal.orientation.z: -0.971732
+    goal.orientation.w: 0.236088
+
+    half_goal.position.x: 89657.0
+    half_goal.position.y: 43175.0
+    half_goal.position.z: -28.0
+    half_goal.orientation.x: 0.0
+    half_goal.orientation.y: 0.0
+    half_goal.orientation.z: -0.9
+    half_goal.orientation.w: 0.25
+
+    pit_goal.position.x: 89626.3671875
+    pit_goal.position.y: 43134.921875
+    pit_goal.position.z: 42.10000228881836
+    pit_goal.orientation.x: 0.0
+    pit_goal.orientation.y: 0.0
+    pit_goal.orientation.z: -0.8788172006607056
+    pit_goal.orientation.w: -0.47715866565704346
+    # ゴールまでの距離がこの値以下になると次のゴールを配信する
+    goal_range: 10.0
+    # ピットインを有効化するか
+    enable_pit: true
+    # このしきい値以下になるとピットイン
+    # pit_in_threshold: 100
+    pit_in_threshold: 700

aichallenge/workspace/src/aichallenge_submit/booars_launch/config/planning/mission_planner.param.yaml

@@ -0,0 +1,13 @@
+/**:
+  ros__parameters:
+    map_frame: map
+    arrival_check_angle_deg: 180.0
+    arrival_check_distance: 30.0
+    arrival_check_duration: 1.0
+    goal_angle_threshold_deg: 90.0
+    enable_correct_goal_pose: false
+    reroute_time_threshold: 10.0
+    minimum_reroute_length: 30.0
+    consider_no_drivable_lanes: false # This flag is for considering no_drivable_lanes in planning or not.
+    check_footprint_inside_lanes: false
+    allow_reroute_in_autonomous_mode: true

aichallenge/workspace/src/aichallenge_submit/booars_launch/config/planning/mppi_controller.param.yaml

@@ -2,25 +2,25 @@
   ros__parameters:
       # mppi
       horizon : 25
-      num_samples : 3000
-      u_min : [-2.0, -0.35] # accel(m/s2), steer angle(rad)
-      u_max : [2.0, 0.35]
-      sigmas : [0.5, 0.25] # sample range
+      num_samples : 4000
+      u_min : [-4.0, -0.35] # accel(m/s2), steer angle(rad)
+      u_max : [3.0, 0.35]
+      sigmas : [2.0, 0.35] # sample range
       lambda : 1.0 
       auto_lambda : false
       # reference path
       DL : 0.1
       lookahead_distance : 0.1
       reference_path_interval : 0.8
       # cost weights
-      Qc : 20.0
+      Qc : 10.0
       Ql : 1.0
-      Qv : 2.0
-      Qo : 10000.0
+      Qv : 4.0
+      Qo : 1000.0
       Qin : 0.01
-      Qdin : 0.5
+      Qdin : 0.0
       # model param
       delta_t : 0.1
-      vehicle_L : 1.0
-      V_MAX : 8.0
+      vehicle_L : 1.08
+      V_MAX : 8.33
 

aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/control.launch.xml

@@ -12,11 +12,11 @@
 
     <node pkg="simple_pure_pursuit" exec="simple_pure_pursuit" name="simple_pure_pursuit_node"
       output="screen" unless="$(var use_stanley)">
-      <param name="use_external_target_vel" value="true" />
+      <param name="use_external_target_vel" value="false" />
       <param name="external_target_vel" value="8.0" />
       <param name="lookahead_gain" value="0.24" />
-      <param name="lookahead_min_distance" value="3.0" />
-      <param name="speed_proportional_gain" value="1.0" />
+      <param name="lookahead_min_distance" value="2.0" />
+      <param name="speed_proportional_gain" value="2.0" />
       <param name="steering_tire_angle_gain" value="$(var steering_tire_angle_gain_var)"/>
 
       <remap from="input/kinematics" to="/localization/kinematic_state" />

aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/planning.launch.xml

@@ -14,7 +14,7 @@
         <remap from="input/vector_map" to="/map/vector_map"/>
         <!-- <remap from="/localization/kinematic_state" to="/awsim/ground_truth/localization/kinematic_state"/> -->
         <remap from="debug/route_marker" to="/planning/mission_planning/route_marker"/>
-        <param from="$(find-pkg-share aichallenge_submit_launch)/config/planning/mission_planning/mission_planner/mission_planner.param.yaml"/>
+        <param from="$(find-pkg-share booars_launch)/config/planning/mission_planner.param.yaml"/>
       </node>
 
       <!-- goal_pose_visualizer -->
@@ -98,6 +98,14 @@
       <node pkg="path_to_trajectory" exec="path_to_trajectory_node" name="path_to_trajectory" output="screen">
         <remap from="input" to="/planning/scenario_planning/lane_driving/behavior_planning/path_with_lane_id"/>
         <remap from="output" to="/planning/scenario_planning/trajectory"/>
+        <!-- 減速度 -->
+        <param name="deceleration" value="-14.0"/>
+        <!-- ピットの何メートル前で速度を0にするか -->
+        <param name="stop_offset" value="1.5"/>
+        <!-- 最高速度 -->
+        <param name="max_speed" value="30.0"/>
+        <!-- trajectoryの点の間隔 -->
+        <param name="traj_width" value="1.0"/>
       </node>
 
     </group>