Merge pull request #23 from booars/feat/MPPI-costmap-integ

Feat/mppi costmap integ

.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

@@ -15,4 +15,4 @@
       predicted_object_layer:
         type: "predicted_object"
         predicted_objects_topic: "/perception/object_recognition/objects"
-        distance_threshold: 1.0
+        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

@@ -111,5 +111,18 @@
         <!-- <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/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
+