Feat/mppi costmap integ

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.0

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

@@ -100,5 +100,28 @@
       </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/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.1] # sample range
+      lambda : 1.0 
+      auto_lambda : false
+      # reference path
+      DL : 0.1
+      loolahead_distance : 3.0
+      reference_path_interval : 0.85
+      # cost weights
+      Qc : 2.0
+      Ql : 3.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
+