Merge branch 'main' into feat/planning/cached_lanelet_costmap

.cspell.json

@@ -104,6 +104,28 @@
     "wpedantic",
     "xacro",
     "xyzrpy",
-    "zcvf"
+    "zcvf",
+    "mppi",
+    "Qdin",
+    "Kohei",
+    "MPPI",
+    "Savitzky",
+    "satitzky",
+    "Golay",
+    "Vandermonde",
+    "michikuni",
+    "eguchi",
+    "vander",
+    "pinv",
+    "dtheta",
+    "cind",
+    "dind",
+    "ncourse",
+    "argmin",
+    "reparameting",
+    "tempature",
+    "rsample",
+    "coeffs",
+    "softplus"
   ]
 }

.github/CODEOWNERS

@@ -12,3 +12,4 @@ aichallenge/workspace/src/aichallenge_submit/racing_kart_description/** @Autumn6
 aichallenge/workspace/src/aichallenge_submit/racing_kart_sensor_kit_description/** @booars/aic2024-developers
 aichallenge/workspace/src/aichallenge_submit/simple_pure_pursuit/** @sitahara
 aichallenge/workspace/src/aichallenge_system/aichallenge_system_launch/** @booars/aic2024-developers
+aichallenge/workspace/src/aichallenge_submit/mppi_controller/** @tamago117

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

@@ -0,0 +1,18 @@
+/**:
+  ros__parameters:
+    update_rate: 10.0
+    map_frame_id: "map"
+    costmap_center_frame_id: "base_link"
+    costmap_width: 50.0
+    costmap_resolution: 0.2
+    multi_layered_costmap:
+      layers:
+        - "lanelet_layer"
+        - "predicted_object_layer"
+      lanelet_layer:
+        type: "lanelet"
+        map_topic: "/map/vector_map"
+      predicted_object_layer:
+        type: "predicted_object"
+        predicted_objects_topic: "/perception/object_recognition/objects"
+        distance_threshold: 1.2

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

@@ -5,12 +5,13 @@
     <node pkg="simple_pure_pursuit" exec="simple_pure_pursuit" name="simple_pure_pursuit_node" output="screen">
       <param name="use_external_target_vel" value="true"/>
       <param name="external_target_vel" value="8.0"/>
-      <param name="lookahead_gain" value="0.3"/>
-      <param name="lookahead_min_distance" value="4.0"/>
+      <param name="lookahead_gain" value="0.4"/>
+      <param name="lookahead_min_distance" value="3.0"/>
       <param name="speed_proportional_gain" value="1.0"/>
 
       <remap from="input/kinematics" to="/localization/kinematic_state"/>
-      <remap from="input/trajectory" to="/planning/scenario_planning/trajectory"/>
+      <!-- <remap from="input/trajectory" to="/planning/scenario_planning/trajectory"/> -->
+      <remap from="input/trajectory" to="/planning/output/mppi_planned_path"/>
       <remap from="output/control_cmd" to="/control/command/control_cmd"/>
     </node>
   </group>

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

@@ -100,5 +100,29 @@
       </node>
 
     </group>
+
+    <!-- costmap -->
+    <group>
+      <push-ros-namespace namespace="costmap"/>
+
+      <!-- Costmap Generator -->
+      <include file="$(find-pkg-share booars_costmap_generator)/launch/costmap_generator.launch.xml">
+        <arg name="config_file" value="$(find-pkg-share booars_launch)/config/planning/costmap_generator.param.yaml"/>
+        <!-- <arg name="costmap_topic" value="~/costmap"/> -->
+      </include>
+    </group>
+
+    <!-- Local Planning -->
+    <node pkg="mppi_controller" exec="mppi_controller_node" name="mppi_controller_node" output="screen">
+      <param from="$(find-pkg-share mppi_controller)/config/mppi_controller.param.yaml"/>
+      <remap from="input/kinematics" to="/localization/kinematic_state"/>
+      <remap from="input/reference_trajectory" to="/planning/scenario_planning/trajectory"/>
+      <!-- cost map -->
+      <remap from="input/costmap" to="/planning/costmap/costmap_generator/costmap"/>
+      <remap from="output/control_cmd" to="output/mppi_control_cmd"/>
+      <remap from="output/planned_path" to="output/mppi_planned_path"/>
+
+    </node>
+
   </group>
 </launch>

aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_generator/launch/costmap_generator.launch.xml

@@ -1,7 +1,10 @@
 <launch>
   <arg name="config_file" default="$(find-pkg-share booars_costmap_generator)/config/costmap_generator.param.yaml"/>
 
+  <arg name="costmap_topic" default="~/costmap"/>
+
   <node pkg="booars_costmap_generator" exec="costmap_generator_node" name="costmap_generator" output="screen">
     <param from="$(var config_file)"/>
+    <remap from="~/output/costmap" to="$(var costmap_topic)"/>
   </node>
 </launch>

aichallenge/workspace/src/aichallenge_submit/mppi_controller/.gitignore

@@ -0,0 +1 @@
+*.pyc

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

@@ -0,0 +1,26 @@
+/**:
+  ros__parameters:
+      # mppi
+      horizon : 25
+      num_samples : 3000
+      u_min : [-2.0, -0.25] # accel(m/s2), steer angle(rad)
+      u_max : [2.0, 0.25]
+      sigmas : [0.5, 0.15] # sample range
+      lambda : 1.0 
+      auto_lambda : false
+      # reference path
+      DL : 0.1
+      lookahead_distance : 0.3
+      reference_path_interval : 0.85
+      # cost weights
+      Qc : 20.0
+      Ql : 1.0
+      Qv : 2.0
+      Qo : 10000.0
+      Qin : 0.01
+      Qdin : 0.5
+      # model param
+      delta_t : 0.1
+      vehicle_L : 1.0
+      V_MAX : 8.0
+