diff --git a/aichallenge/workspace/src/aichallenge_submit/booars_launch/config/planning/costmap_generator.param.yaml b/aichallenge/workspace/src/aichallenge_submit/booars_launch/config/planning/costmap_generator.param.yaml
new file mode 100644
index 00000000..761d16e7
--- /dev/null
+++ b/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
diff --git a/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/planning.launch.xml b/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/planning.launch.xml
index 2c8c989f..483d4cd0 100644
--- a/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/planning.launch.xml
+++ b/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/planning.launch.xml
@@ -100,5 +100,16 @@
       </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>
   </group>
 </launch>
\ No newline at end of file
diff --git a/aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_generator/launch/costmap_generator.launch.xml b/aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_generator/launch/costmap_generator.launch.xml
index 9501d94e..0dea8897 100644
--- a/aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_generator/launch/costmap_generator.launch.xml
+++ b/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>
\ No newline at end of file
diff --git a/aichallenge/workspace/src/aichallenge_system/aichallenge_system_launch/config/autoware.rviz b/aichallenge/workspace/src/aichallenge_system/aichallenge_system_launch/config/autoware.rviz
index 1539328f..2d248d76 100644
--- a/aichallenge/workspace/src/aichallenge_system/aichallenge_system_launch/config/autoware.rviz
+++ b/aichallenge/workspace/src/aichallenge_system/aichallenge_system_launch/config/autoware.rviz
@@ -3,9 +3,11 @@ Panels:
     Help Height: 0
     Name: Displays
     Property Tree Widget:
-      Expanded: ~
+      Expanded:
+        - /Sensing1/LiDAR1/ConcatenatePointCloud1/Autocompute Value Bounds1
+        - /Planning1
       Splitter Ratio: 0.557669460773468
-    Tree Height: 397
+    Tree Height: 242
   - Class: rviz_common/Selection
     Name: Selection
   - Class: rviz_common/Tool Properties
@@ -13,7 +15,8 @@ Panels:
     Name: Tool Properties
     Splitter Ratio: 0.5886790156364441
   - Class: rviz_common/Views
-    Expanded: ~
+    Expanded:
+      - /Current View1
     Name: Views
     Splitter Ratio: 0.5
   - Class: tier4_localization_rviz_plugin/InitialPoseButtonPanel
@@ -39,7 +42,8 @@ Visualization Manager:
           Show Arrows: true
           Show Axes: true
           Show Names: true
-          Tree: {}
+          Tree:
+            {}
           Update Interval: 0
           Value: false
         - Alpha: 0.5
@@ -64,9 +68,12 @@ Visualization Manager:
           Displays:
             - Class: rviz_plugins/SteeringAngle
               Enabled: true
+              Left: 85
+              Length: 171
               Name: SteeringAngle
               Scale: 17
               Text Color: 25; 255; 240
+              Top: 85
               Topic:
                 Depth: 5
                 Durability Policy: Volatile
@@ -74,12 +81,15 @@ Visualization Manager:
                 Reliability Policy: Reliable
                 Value: /vehicle/status/steering_status
               Value: true
+              Value Scale: 0.14999249577522278
               Value height offset: 0
             - Class: rviz_plugins/ConsoleMeter
               Enabled: true
+              Left: 341
+              Length: 171
               Name: ConsoleMeter
-              Scale: 3
               Text Color: 25; 255; 240
+              Top: 85
               Topic:
                 Depth: 5
                 Durability Policy: Volatile
@@ -87,8 +97,9 @@ Visualization Manager:
                 Reliability Policy: Reliable
                 Value: /vehicle/status/velocity_status
               Value: true
+              Value Scale: 0.14999249577522278
               Value height offset: 0
-            - Alpha: 0.999
+            - Alpha: 0.9990000128746033
               Class: rviz_plugins/VelocityHistory
               Color Border Vel Max: 3
               Constant Color:
@@ -157,8 +168,8 @@ Visualization Manager:
               Max Alpha: 0.5
               Max Range: 100
               Max Wave Alpha: 0.5
-              Min Alpha: 0.01
-              Min Wave Alpha: 0.01
+              Min Alpha: 0.009999999776482582
+              Min Wave Alpha: 0.009999999776482582
               Name: PolarGridDisplay
               Reference Frame: base_link
               Value: true
@@ -166,18 +177,20 @@ Visualization Manager:
               Wave Velocity: 40
             - Class: rviz_plugins/MaxVelocity
               Enabled: true
+              Left: 397
+              Length: 64
               Name: MaxVelocity
               Text Color: 255; 255; 255
-              Topic:
-                Depth: 5
-                Durability Policy: Volatile
-                History Policy: Keep Last
-                Reliability Policy: Reliable
-                Value: /planning/scenario_planning/current_max_velocity
+              Top: 187
+              Topic: /planning/scenario_planning/current_max_velocity
               Value: true
+              Value Scale: 0.25
             - Class: rviz_plugins/TurnSignal
               Enabled: true
+              Height: 171
+              Left: 131
               Name: TurnSignal
+              Top: 233
               Topic:
                 Depth: 5
                 Durability Policy: Volatile
@@ -185,6 +198,7 @@ Visualization Manager:
                 Reliability Policy: Reliable
                 Value: /vehicle/status/turn_indicators_status
               Value: true
+              Width: 341
           Enabled: true
           Name: Vehicle
       Enabled: true
@@ -213,11 +227,12 @@ Visualization Manager:
           Position Transformer: XYZ
           Selectable: false
           Size (Pixels): 1
-          Size (m): 0.02
+          Size (m): 0.019999999552965164
           Style: Points
           Topic:
             Depth: 5
             Durability Policy: Transient Local
+            Filter size: 10
             History Policy: Keep Last
             Reliability Policy: Reliable
             Value: /map/pointcloud_map
@@ -248,7 +263,7 @@ Visualization Manager:
       Displays:
         - Class: rviz_common/Group
           Displays:
-            - Alpha: 0.4
+            - Alpha: 0.4000000059604645
               Autocompute Intensity Bounds: true
               Autocompute Value Bounds:
                 Max Value: 5
@@ -270,18 +285,19 @@ Visualization Manager:
               Position Transformer: XYZ
               Selectable: false
               Size (Pixels): 1
-              Size (m): 0.02
+              Size (m): 0.019999999552965164
               Style: Points
               Topic:
                 Depth: 5
                 Durability Policy: Volatile
+                Filter size: 10
                 History Policy: Keep Last
                 Reliability Policy: Best Effort
                 Value: /sensing/lidar/concatenated/pointcloud
               Use Fixed Frame: false
               Use rainbow: true
               Value: true
-            - Alpha: 0.999
+            - Alpha: 0.9990000128746033
               Class: rviz_default_plugins/Polygon
               Color: 25; 255; 0
               Enabled: false
@@ -289,6 +305,7 @@ Visualization Manager:
               Topic:
                 Depth: 5
                 Durability Policy: Volatile
+                Filter size: 10
                 History Policy: Keep Last
                 Reliability Policy: Reliable
                 Value: /sensing/lidar/crop_box_filter/crop_box_polygon
@@ -327,6 +344,7 @@ Visualization Manager:
               Topic:
                 Depth: 5
                 Durability Policy: Volatile
+                Filter size: 10
                 History Policy: Keep Last
                 Reliability Policy: Reliable
                 Value: /sensing/gnss/pose_with_covariance
@@ -339,7 +357,7 @@ Visualization Manager:
       Displays:
         - Class: rviz_common/Group
           Displays:
-            - Alpha: 0.999
+            - Alpha: 0.9990000128746033
               Axes Length: 1
               Axes Radius: 0.10000000149011612
               Class: rviz_default_plugins/PoseWithCovariance
@@ -369,11 +387,12 @@ Visualization Manager:
               Topic:
                 Depth: 5
                 Durability Policy: Volatile
+                Filter size: 10
                 History Policy: Keep Last
                 Reliability Policy: Reliable
                 Value: /localization/pose_estimator/initial_pose_with_covariance
-              Value: true
-            - Alpha: 0.999
+              Value: false
+            - Alpha: 0.9990000128746033
               Axes Length: 1
               Axes Radius: 0.10000000149011612
               Class: rviz_default_plugins/PoseWithCovariance
@@ -403,27 +422,29 @@ Visualization Manager:
               Topic:
                 Depth: 5
                 Durability Policy: Volatile
+                Filter size: 10
                 History Policy: Keep Last
                 Reliability Policy: Reliable
                 Value: /localization/pose_estimator/pose_with_covariance
-              Value: true
+              Value: false
             - Buffer Size: 200
               Class: rviz_plugins::PoseHistory
               Enabled: false
               Line:
+                Alpha: 0.9990000128746033
                 Color: 170; 255; 127
                 Value: true
                 Width: 0.10000000149011612
-                Alpha: 0.999
               Name: PoseHistory
               Topic:
                 Depth: 5
                 Durability Policy: Volatile
+                Filter size: 10
                 History Policy: Keep Last
                 Reliability Policy: Reliable
                 Value: /localization/pose_estimator/pose
-              Value: true
-            - Alpha: 0.999
+              Value: false
+            - Alpha: 0.9990000128746033
               Autocompute Intensity Bounds: true
               Autocompute Value Bounds:
                 Max Value: 10
@@ -450,13 +471,14 @@ Visualization Manager:
               Topic:
                 Depth: 5
                 Durability Policy: Volatile
+                Filter size: 10
                 History Policy: Keep Last
                 Reliability Policy: Best Effort
                 Value: /localization/util/downsample/pointcloud
               Use Fixed Frame: true
               Use rainbow: true
               Value: false
-            - Alpha: 0.999
+            - Alpha: 0.9990000128746033
               Autocompute Intensity Bounds: true
               Autocompute Value Bounds:
                 Max Value: 10
@@ -483,6 +505,7 @@ Visualization Manager:
               Topic:
                 Depth: 5
                 Durability Policy: Volatile
+                Filter size: 10
                 History Policy: Keep Last
                 Reliability Policy: Reliable
                 Value: /localization/pose_estimator/points_aligned
@@ -492,7 +515,8 @@ Visualization Manager:
             - Class: rviz_default_plugins/MarkerArray
               Enabled: true
               Name: MonteCarloInitialPose
-              Namespaces: {}
+              Namespaces:
+                {}
               Topic:
                 Depth: 5
                 Durability Policy: Volatile
@@ -508,14 +532,15 @@ Visualization Manager:
               Class: rviz_plugins::PoseHistory
               Enabled: true
               Line:
+                Alpha: 0.9990000128746033
                 Color: 0; 255; 255
                 Value: true
                 Width: 0.10000000149011612
-                Alpha: 0.999
               Name: PoseHistory
               Topic:
                 Depth: 5
                 Durability Policy: Volatile
+                Filter size: 10
                 History Policy: Keep Last
                 Reliability Policy: Reliable
                 Value: /localization/pose_twist_fusion_filter/pose
@@ -528,7 +553,7 @@ Visualization Manager:
       Displays:
         - Class: rviz_common/Group
           Displays:
-            - Alpha: 0.999
+            - Alpha: 0.9990000128746033
               Autocompute Intensity Bounds: true
               Autocompute Value Bounds:
                 Max Value: 15
@@ -550,11 +575,12 @@ Visualization Manager:
               Position Transformer: XYZ
               Selectable: false
               Size (Pixels): 3
-              Size (m): 0.02
+              Size (m): 0.019999999552965164
               Style: Points
               Topic:
                 Depth: 5
                 Durability Policy: Volatile
+                Filter size: 10
                 History Policy: Keep Last
                 Reliability Policy: Best Effort
                 Value: /perception/obstacle_segmentation/pointcloud
@@ -568,16 +594,16 @@ Visualization Manager:
             - Class: rviz_common/Group
               Displays:
                 - BUS:
-                    Alpha: 0.999
+                    Alpha: 0.9990000128746033
                     Color: 30; 144; 255
                   CAR:
-                    Alpha: 0.999
+                    Alpha: 0.9990000128746033
                     Color: 30; 144; 255
                   CYCLIST:
-                    Alpha: 0.999
+                    Alpha: 0.9990000128746033
                     Color: 119; 11; 32
                   Class: autoware_auto_perception_rviz_plugin/DetectedObjects
-                  Display 3d polygon: true
+                  Display Acceleration: true
                   Display Label: true
                   Display PoseWithCovariance: true
                   Display Predicted Path Confidence: true
@@ -585,21 +611,23 @@ Visualization Manager:
                   Display Twist: true
                   Display UUID: true
                   Display Velocity: true
-                  Line Width: 0.03
                   Enabled: true
+                  Line Width: 0.029999999329447746
                   MOTORCYCLE:
-                    Alpha: 0.999
+                    Alpha: 0.9990000128746033
                     Color: 119; 11; 32
                   Name: DetectedObjects
-                  Namespaces: {}
+                  Namespaces:
+                    {}
                   PEDESTRIAN:
-                    Alpha: 0.999
+                    Alpha: 0.9990000128746033
                     Color: 255; 192; 203
+                  Polygon Type: 3d
                   TRAILER:
-                    Alpha: 0.999
+                    Alpha: 0.9990000128746033
                     Color: 30; 144; 255
                   TRUCK:
-                    Alpha: 0.999
+                    Alpha: 0.9990000128746033
                     Color: 30; 144; 255
                   Topic:
                     Depth: 5
@@ -608,24 +636,25 @@ Visualization Manager:
                     Reliability Policy: Best Effort
                     Value: /perception/object_recognition/detection/objects
                   UNKNOWN:
-                    Alpha: 0.999
+                    Alpha: 0.9990000128746033
                     Color: 255; 255; 255
                   Value: true
+                  Visualization Type: Normal
               Enabled: false
               Name: Detection
             - Class: rviz_common/Group
               Displays:
                 - BUS:
-                    Alpha: 0.999
+                    Alpha: 0.9990000128746033
                     Color: 30; 144; 255
                   CAR:
-                    Alpha: 0.999
+                    Alpha: 0.9990000128746033
                     Color: 30; 144; 255
                   CYCLIST:
-                    Alpha: 0.999
+                    Alpha: 0.9990000128746033
                     Color: 119; 11; 32
                   Class: autoware_auto_perception_rviz_plugin/TrackedObjects
-                  Display 3d polygon: true
+                  Display Acceleration: true
                   Display Label: true
                   Display PoseWithCovariance: true
                   Display Predicted Path Confidence: true
@@ -633,21 +662,23 @@ Visualization Manager:
                   Display Twist: true
                   Display UUID: true
                   Display Velocity: true
-                  Line Width: 0.03
                   Enabled: true
+                  Line Width: 0.029999999329447746
                   MOTORCYCLE:
-                    Alpha: 0.999
+                    Alpha: 0.9990000128746033
                     Color: 119; 11; 32
                   Name: TrackedObjects
-                  Namespaces: {}
+                  Namespaces:
+                    {}
                   PEDESTRIAN:
-                    Alpha: 0.999
+                    Alpha: 0.9990000128746033
                     Color: 255; 192; 203
+                  Polygon Type: 3d
                   TRAILER:
-                    Alpha: 0.999
+                    Alpha: 0.9990000128746033
                     Color: 30; 144; 255
                   TRUCK:
-                    Alpha: 0.999
+                    Alpha: 0.9990000128746033
                     Color: 30; 144; 255
                   Topic:
                     Depth: 5
@@ -656,24 +687,25 @@ Visualization Manager:
                     Reliability Policy: Best Effort
                     Value: /perception/object_recognition/tracking/objects
                   UNKNOWN:
-                    Alpha: 0.999
+                    Alpha: 0.9990000128746033
                     Color: 255; 255; 255
                   Value: true
+                  Visualization Type: Normal
               Enabled: false
               Name: Tracking
             - Class: rviz_common/Group
               Displays:
                 - BUS:
-                    Alpha: 0.999
+                    Alpha: 0.9990000128746033
                     Color: 30; 144; 255
                   CAR:
-                    Alpha: 0.999
+                    Alpha: 0.9990000128746033
                     Color: 30; 144; 255
                   CYCLIST:
-                    Alpha: 0.999
+                    Alpha: 0.9990000128746033
                     Color: 119; 11; 32
                   Class: autoware_auto_perception_rviz_plugin/PredictedObjects
-                  Display 3d polygon: true
+                  Display Acceleration: true
                   Display Label: true
                   Display PoseWithCovariance: false
                   Display Predicted Path Confidence: true
@@ -681,21 +713,28 @@ Visualization Manager:
                   Display Twist: true
                   Display UUID: true
                   Display Velocity: true
-                  Line Width: 0.03
                   Enabled: true
+                  Line Width: 0.029999999329447746
                   MOTORCYCLE:
-                    Alpha: 0.999
+                    Alpha: 0.9990000128746033
                     Color: 119; 11; 32
                   Name: PredictedObjects
-                  Namespaces: {}
+                  Namespaces:
+                    acceleration: true
+                    label: true
+                    shape: true
+                    twist: true
+                    uuid: true
+                    velocity: true
                   PEDESTRIAN:
-                    Alpha: 0.999
+                    Alpha: 0.9990000128746033
                     Color: 255; 192; 203
+                  Polygon Type: 3d
                   TRAILER:
-                    Alpha: 0.999
+                    Alpha: 0.9990000128746033
                     Color: 30; 144; 255
                   TRUCK:
-                    Alpha: 0.999
+                    Alpha: 0.9990000128746033
                     Color: 30; 144; 255
                   Topic:
                     Depth: 5
@@ -704,20 +743,22 @@ Visualization Manager:
                     Reliability Policy: Best Effort
                     Value: /perception/object_recognition/objects
                   UNKNOWN:
-                    Alpha: 0.999
+                    Alpha: 0.9990000128746033
                     Color: 255; 255; 255
                   Value: true
+                  Visualization Type: Normal
                 - Class: rviz_default_plugins/MarkerArray
                   Enabled: false
                   Name: Maneuver
-                  Namespaces: {}
+                  Namespaces:
+                    {}
                   Topic:
                     Depth: 5
                     Durability Policy: Volatile
                     History Policy: Keep Last
                     Reliability Policy: Best Effort
                     Value: /perception/object_recognition/prediction/maneuver
-                  Value: true
+                  Value: false
               Enabled: true
               Name: Prediction
           Enabled: true
@@ -741,7 +782,8 @@ Visualization Manager:
             - Class: rviz_default_plugins/MarkerArray
               Enabled: true
               Name: MapBasedDetectionResult
-              Namespaces: {}
+              Namespaces:
+                {}
               Topic:
                 Depth: 5
                 Durability Policy: Volatile
@@ -798,7 +840,7 @@ Visualization Manager:
                 Reliability Policy: Reliable
                 Value: /planning/mission_planning/route_marker
               Value: true
-            - Alpha: 0.999
+            - Alpha: 0.9990000128746033
               Axes Length: 1
               Axes Radius: 0.30000001192092896
               Class: rviz_default_plugins/Pose
@@ -813,6 +855,7 @@ Visualization Manager:
               Topic:
                 Depth: 5
                 Durability Policy: Volatile
+                Filter size: 10
                 History Policy: Keep Last
                 Reliability Policy: Reliable
                 Value: /planning/mission_planning/echo_back_goal_pose
@@ -828,18 +871,36 @@ Visualization Manager:
               Topic:
                 Depth: 5
                 Durability Policy: Volatile
+                Filter size: 10
                 History Policy: Keep Last
                 Reliability Policy: Reliable
                 Value: /planning/scenario_planning/trajectory
               Value: true
+              View Footprint:
+                Alpha: 1
+                Color: 230; 230; 50
+                Offset from BaseLink: 0
+                Rear Overhang: 1.0299999713897705
+                Value: false
+                Vehicle Length: 4.769999980926514
+                Vehicle Width: 1.8300000429153442
               View Path:
-                Alpha: 0.999
+                Alpha: 0.9990000128746033
                 Color: 0; 0; 0
                 Constant Color: false
                 Value: true
                 Width: 2
+              View Point:
+                Alpha: 1
+                Color: 0; 60; 255
+                Offset: 0
+                Radius: 0.10000000149011612
+                Value: false
+              View Text Velocity:
+                Scale: 0.30000001192092896
+                Value: false
               View Velocity:
-                Alpha: 0.999
+                Alpha: 0.9990000128746033
                 Color: 0; 0; 0
                 Constant Color: false
                 Scale: 0.30000001192092896
@@ -855,16 +916,39 @@ Visualization Manager:
                       Topic:
                         Depth: 5
                         Durability Policy: Volatile
+                        Filter size: 10
                         History Policy: Keep Last
                         Reliability Policy: Reliable
                         Value: /planning/scenario_planning/lane_driving/behavior_planning/path
                       Value: true
+                      View Drivable Area:
+                        Alpha: 0.9990000128746033
+                        Color: 0; 148; 205
+                        Value: true
+                        Width: 0.30000001192092896
+                      View Footprint:
+                        Alpha: 1
+                        Color: 230; 230; 50
+                        Offset from BaseLink: 0
+                        Rear Overhang: 1.0299999713897705
+                        Value: false
+                        Vehicle Length: 4.769999980926514
+                        Vehicle Width: 1.8300000429153442
                       View Path:
                         Alpha: 0.4000000059604645
                         Color: 0; 0; 0
                         Constant Color: false
                         Value: true
                         Width: 2
+                      View Point:
+                        Alpha: 1
+                        Color: 0; 60; 255
+                        Offset: 0
+                        Radius: 0.10000000149011612
+                        Value: false
+                      View Text Velocity:
+                        Scale: 0.30000001192092896
+                        Value: false
                       View Velocity:
                         Alpha: 0.4000000059604645
                         Color: 0; 0; 0
@@ -878,16 +962,39 @@ Visualization Manager:
                       Topic:
                         Depth: 5
                         Durability Policy: Volatile
+                        Filter size: 10
                         History Policy: Keep Last
                         Reliability Policy: Reliable
                         Value: /planning/path_candidate/lane_change
                       Value: true
+                      View Drivable Area:
+                        Alpha: 0.9990000128746033
+                        Color: 0; 148; 205
+                        Value: true
+                        Width: 0.30000001192092896
+                      View Footprint:
+                        Alpha: 1
+                        Color: 230; 230; 50
+                        Offset from BaseLink: 0
+                        Rear Overhang: 1.0299999713897705
+                        Value: false
+                        Vehicle Length: 4.769999980926514
+                        Vehicle Width: 1.8300000429153442
                       View Path:
                         Alpha: 0.30000001192092896
                         Color: 115; 210; 22
                         Constant Color: false
                         Value: true
                         Width: 2
+                      View Point:
+                        Alpha: 1
+                        Color: 0; 60; 255
+                        Offset: 0
+                        Radius: 0.10000000149011612
+                        Value: false
+                      View Text Velocity:
+                        Scale: 0.30000001192092896
+                        Value: false
                       View Velocity:
                         Alpha: 0.30000001192092896
                         Color: 0; 0; 0
@@ -901,16 +1008,39 @@ Visualization Manager:
                       Topic:
                         Depth: 5
                         Durability Policy: Volatile
+                        Filter size: 10
                         History Policy: Keep Last
                         Reliability Policy: Reliable
                         Value: /planning/path_candidate/ext_request_lane_change_right
                       Value: true
+                      View Drivable Area:
+                        Alpha: 0.9990000128746033
+                        Color: 0; 148; 205
+                        Value: true
+                        Width: 0.30000001192092896
+                      View Footprint:
+                        Alpha: 1
+                        Color: 230; 230; 50
+                        Offset from BaseLink: 0
+                        Rear Overhang: 1.0299999713897705
+                        Value: false
+                        Vehicle Length: 4.769999980926514
+                        Vehicle Width: 1.8300000429153442
                       View Path:
                         Alpha: 0.30000001192092896
                         Color: 115; 210; 22
                         Constant Color: false
                         Value: true
                         Width: 2
+                      View Point:
+                        Alpha: 1
+                        Color: 0; 60; 255
+                        Offset: 0
+                        Radius: 0.10000000149011612
+                        Value: false
+                      View Text Velocity:
+                        Scale: 0.30000001192092896
+                        Value: false
                       View Velocity:
                         Alpha: 0.30000001192092896
                         Color: 0; 0; 0
@@ -924,16 +1054,39 @@ Visualization Manager:
                       Topic:
                         Depth: 5
                         Durability Policy: Volatile
+                        Filter size: 10
                         History Policy: Keep Last
                         Reliability Policy: Reliable
                         Value: /planning/path_candidate/ext_request_lane_change_left
                       Value: true
+                      View Drivable Area:
+                        Alpha: 0.9990000128746033
+                        Color: 0; 148; 205
+                        Value: true
+                        Width: 0.30000001192092896
+                      View Footprint:
+                        Alpha: 1
+                        Color: 230; 230; 50
+                        Offset from BaseLink: 0
+                        Rear Overhang: 1.0299999713897705
+                        Value: false
+                        Vehicle Length: 4.769999980926514
+                        Vehicle Width: 1.8300000429153442
                       View Path:
                         Alpha: 0.30000001192092896
                         Color: 115; 210; 22
                         Constant Color: false
                         Value: true
                         Width: 2
+                      View Point:
+                        Alpha: 1
+                        Color: 0; 60; 255
+                        Offset: 0
+                        Radius: 0.10000000149011612
+                        Value: false
+                      View Text Velocity:
+                        Scale: 0.30000001192092896
+                        Value: false
                       View Velocity:
                         Alpha: 0.30000001192092896
                         Color: 0; 0; 0
@@ -947,16 +1100,39 @@ Visualization Manager:
                       Topic:
                         Depth: 5
                         Durability Policy: Volatile
+                        Filter size: 10
                         History Policy: Keep Last
                         Reliability Policy: Reliable
                         Value: /planning/path_candidate/avoidance
                       Value: true
+                      View Drivable Area:
+                        Alpha: 0.9990000128746033
+                        Color: 0; 148; 205
+                        Value: true
+                        Width: 0.30000001192092896
+                      View Footprint:
+                        Alpha: 1
+                        Color: 230; 230; 50
+                        Offset from BaseLink: 0
+                        Rear Overhang: 1.0299999713897705
+                        Value: false
+                        Vehicle Length: 4.769999980926514
+                        Vehicle Width: 1.8300000429153442
                       View Path:
                         Alpha: 0.30000001192092896
                         Color: 115; 210; 22
                         Constant Color: false
                         Value: true
                         Width: 2
+                      View Point:
+                        Alpha: 1
+                        Color: 0; 60; 255
+                        Offset: 0
+                        Radius: 0.10000000149011612
+                        Value: false
+                      View Text Velocity:
+                        Scale: 0.30000001192092896
+                        Value: false
                       View Velocity:
                         Alpha: 0.30000001192092896
                         Color: 0; 0; 0
@@ -970,16 +1146,39 @@ Visualization Manager:
                       Topic:
                         Depth: 5
                         Durability Policy: Volatile
+                        Filter size: 10
                         History Policy: Keep Last
                         Reliability Policy: Reliable
                         Value: /planning/path_candidate/pull_out
                       Value: true
+                      View Drivable Area:
+                        Alpha: 0.9990000128746033
+                        Color: 0; 148; 205
+                        Value: true
+                        Width: 0.30000001192092896
+                      View Footprint:
+                        Alpha: 1
+                        Color: 230; 230; 50
+                        Offset from BaseLink: 0
+                        Rear Overhang: 1.0299999713897705
+                        Value: false
+                        Vehicle Length: 4.769999980926514
+                        Vehicle Width: 1.8300000429153442
                       View Path:
                         Alpha: 0.30000001192092896
                         Color: 115; 210; 22
                         Constant Color: false
                         Value: true
                         Width: 2
+                      View Point:
+                        Alpha: 1
+                        Color: 0; 60; 255
+                        Offset: 0
+                        Radius: 0.10000000149011612
+                        Value: false
+                      View Text Velocity:
+                        Scale: 0.30000001192092896
+                        Value: false
                       View Velocity:
                         Alpha: 0.30000001192092896
                         Color: 0; 0; 0
@@ -993,16 +1192,39 @@ Visualization Manager:
                       Topic:
                         Depth: 5
                         Durability Policy: Volatile
+                        Filter size: 10
                         History Policy: Keep Last
                         Reliability Policy: Reliable
                         Value: /planning/path_candidate/pull_over
                       Value: true
+                      View Drivable Area:
+                        Alpha: 0.9990000128746033
+                        Color: 0; 148; 205
+                        Value: true
+                        Width: 0.30000001192092896
+                      View Footprint:
+                        Alpha: 1
+                        Color: 230; 230; 50
+                        Offset from BaseLink: 0
+                        Rear Overhang: 1.0299999713897705
+                        Value: false
+                        Vehicle Length: 4.769999980926514
+                        Vehicle Width: 1.8300000429153442
                       View Path:
                         Alpha: 0.30000001192092896
                         Color: 115; 210; 22
                         Constant Color: false
                         Value: true
                         Width: 2
+                      View Point:
+                        Alpha: 1
+                        Color: 0; 60; 255
+                        Offset: 0
+                        Radius: 0.10000000149011612
+                        Value: false
+                      View Text Velocity:
+                        Scale: 0.30000001192092896
+                        Value: false
                       View Velocity:
                         Alpha: 0.30000001192092896
                         Color: 0; 0; 0
@@ -1018,7 +1240,6 @@ Visualization Manager:
                       Topic:
                         Depth: 5
                         Durability Policy: Volatile
-                        Filter size: 10
                         History Policy: Keep Last
                         Reliability Policy: Reliable
                         Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/bound
@@ -1414,18 +1635,36 @@ Visualization Manager:
                       Topic:
                         Depth: 5
                         Durability Policy: Volatile
+                        Filter size: 10
                         History Policy: Keep Last
                         Reliability Policy: Reliable
                         Value: /planning/scenario_planning/lane_driving/trajectory
                       Value: false
+                      View Footprint:
+                        Alpha: 1
+                        Color: 230; 230; 50
+                        Offset from BaseLink: 0
+                        Rear Overhang: 1.0299999713897705
+                        Value: false
+                        Vehicle Length: 4.769999980926514
+                        Vehicle Width: 1.8300000429153442
                       View Path:
-                        Alpha: 0.999
+                        Alpha: 0.9990000128746033
                         Color: 0; 0; 0
                         Constant Color: false
                         Value: true
                         Width: 2
+                      View Point:
+                        Alpha: 1
+                        Color: 0; 60; 255
+                        Offset: 0
+                        Radius: 0.10000000149011612
+                        Value: false
+                      View Text Velocity:
+                        Scale: 0.30000001192092896
+                        Value: false
                       View Velocity:
-                        Alpha: 0.999
+                        Alpha: 0.9990000128746033
                         Color: 0; 0; 0
                         Constant Color: false
                         Scale: 0.30000001192092896
@@ -1608,6 +1847,7 @@ Visualization Manager:
                   Topic:
                     Depth: 5
                     Durability Policy: Volatile
+                    Filter size: 10
                     History Policy: Keep Last
                     Reliability Policy: Reliable
                     Value: /planning/scenario_planning/parking/costmap_generator/occupancy_grid
@@ -1619,7 +1859,7 @@ Visualization Manager:
                     Value: /planning/scenario_planning/parking/costmap_generator/occupancy_grid_updates
                   Use Timestamp: false
                   Value: false
-                - Alpha: 0.999
+                - Alpha: 0.9990000128746033
                   Arrow Length: 0.30000001192092896
                   Axes Length: 0.30000001192092896
                   Axes Radius: 0.009999999776482582
@@ -1635,11 +1875,12 @@ Visualization Manager:
                   Topic:
                     Depth: 5
                     Durability Policy: Volatile
+                    Filter size: 10
                     History Policy: Keep Last
                     Reliability Policy: Reliable
                     Value: /planning/scenario_planning/parking/freespace_planner/debug/partial_pose_array
                   Value: true
-                - Alpha: 0.999
+                - Alpha: 0.9990000128746033
                   Arrow Length: 0.5
                   Axes Length: 0.30000001192092896
                   Axes Radius: 0.009999999776482582
@@ -1655,6 +1896,7 @@ Visualization Manager:
                   Topic:
                     Depth: 5
                     Durability Policy: Volatile
+                    Filter size: 10
                     History Policy: Keep Last
                     Reliability Policy: Reliable
                     Value: /planning/scenario_planning/parking/freespace_planner/debug/pose_array
@@ -1684,7 +1926,8 @@ Visualization Manager:
             - Class: rviz_default_plugins/MarkerArray
               Enabled: true
               Name: PlanningErrorMarker
-              Namespaces: {}
+              Namespaces:
+                {}
               Topic:
                 Depth: 5
                 Durability Policy: Volatile
@@ -1694,6 +1937,31 @@ Visualization Manager:
               Value: true
           Enabled: false
           Name: Diagnostic
+        - Class: rviz_common/Group
+          Displays:
+            - Alpha: 0.699999988079071
+              Class: rviz_default_plugins/Map
+              Color Scheme: map
+              Draw Behind: false
+              Enabled: true
+              Name: Map
+              Topic:
+                Depth: 5
+                Durability Policy: Volatile
+                Filter size: 10
+                History Policy: Keep Last
+                Reliability Policy: Reliable
+                Value: /planning/costmap/costmap_generator/costmap
+              Update Topic:
+                Depth: 5
+                Durability Policy: Volatile
+                History Policy: Keep Last
+                Reliability Policy: Reliable
+                Value: /planning/costmap/costmap_generator/costmap_updates
+              Use Timestamp: false
+              Value: true
+          Enabled: true
+          Name: Costmap
       Enabled: true
       Name: Planning
     - Class: rviz_common/Group
@@ -1705,16 +1973,34 @@ Visualization Manager:
           Topic:
             Depth: 5
             Durability Policy: Volatile
+            Filter size: 10
             History Policy: Keep Last
             Reliability Policy: Reliable
             Value: /control/trajectory_follower/lateral/predicted_trajectory
           Value: true
+          View Footprint:
+            Alpha: 1
+            Color: 230; 230; 50
+            Offset from BaseLink: 0
+            Rear Overhang: 1.0299999713897705
+            Value: false
+            Vehicle Length: 4.769999980926514
+            Vehicle Width: 1.8300000429153442
           View Path:
             Alpha: 1
             Color: 255; 255; 255
             Constant Color: true
             Value: true
             Width: 0.05000000074505806
+          View Point:
+            Alpha: 1
+            Color: 0; 60; 255
+            Offset: 0
+            Radius: 0.10000000149011612
+            Value: false
+          View Text Velocity:
+            Scale: 0.30000001192092896
+            Value: false
           View Velocity:
             Alpha: 1
             Color: 0; 0; 0
@@ -1724,7 +2010,8 @@ Visualization Manager:
         - Class: rviz_default_plugins/MarkerArray
           Enabled: false
           Name: Debug/MPC
-          Namespaces: {}
+          Namespaces:
+            {}
           Topic:
             Depth: 5
             Durability Policy: Volatile
@@ -1735,7 +2022,8 @@ Visualization Manager:
         - Class: rviz_default_plugins/MarkerArray
           Enabled: false
           Name: Debug/PurePursuit
-          Namespaces: {}
+          Namespaces:
+            {}
           Topic:
             Depth: 5
             Durability Policy: Volatile
@@ -1797,15 +2085,15 @@ Visualization Manager:
     - Class: rviz_default_plugins/Measure
       Line color: 128; 128; 0
     - Class: rviz_default_plugins/SetInitialPose
-      Theta std deviation: 0.2617993950843811
+      Covariance x: 0.25
+      Covariance y: 0.25
+      Covariance yaw: 0.06853891909122467
       Topic:
         Depth: 5
         Durability Policy: Volatile
         History Policy: Keep Last
         Reliability Policy: Reliable
         Value: /initialpose
-      X std deviation: 0.5
-      Y std deviation: 0.5
     - Class: rviz_default_plugins/SetGoal
       Topic:
         Depth: 5
@@ -1813,26 +2101,47 @@ Visualization Manager:
         History Policy: Keep Last
         Reliability Policy: Reliable
         Value: /planning/mission_planning/goal
-    - Class: rviz_plugins/PedestrianInitialPoseTool
+    - Acceleration: 0
+      Class: rviz_plugins/PedestrianInitialPoseTool
+      Interactive: false
+      Max velocity: 33.29999923706055
+      Min velocity: -33.29999923706055
       Pose Topic: /simulation/dummy_perception_publisher/object_info
+      Target Frame: <Fixed Frame>
       Theta std deviation: 0.0872664600610733
       Velocity: 0
       X std deviation: 0.029999999329447746
       Y std deviation: 0.029999999329447746
       Z position: 1
       Z std deviation: 0.029999999329447746
-    - Class: rviz_plugins/CarInitialPoseTool
+    - Acceleration: 0
+      Class: rviz_plugins/CarInitialPoseTool
+      H vehicle height: 2
+      Interactive: false
+      L vehicle length: 4
+      Max velocity: 33.29999923706055
+      Min velocity: -33.29999923706055
       Pose Topic: /simulation/dummy_perception_publisher/object_info
+      Target Frame: <Fixed Frame>
       Theta std deviation: 0.0872664600610733
       Velocity: 3
+      W vehicle width: 1.7999999523162842
       X std deviation: 0.029999999329447746
       Y std deviation: 0.029999999329447746
       Z position: 0
       Z std deviation: 0.029999999329447746
-    - Class: rviz_plugins/BusInitialPoseTool
+    - Acceleration: 0
+      Class: rviz_plugins/BusInitialPoseTool
+      H vehicle height: 3.5
+      Interactive: false
+      L vehicle length: 10.5
+      Max velocity: 33.29999923706055
+      Min velocity: -33.29999923706055
       Pose Topic: /simulation/dummy_perception_publisher/object_info
+      Target Frame: <Fixed Frame>
       Theta std deviation: 0.0872664600610733
       Velocity: 0
+      W vehicle width: 2.5
       X std deviation: 0.029999999329447746
       Y std deviation: 0.029999999329447746
       Z position: 0
@@ -1845,6 +2154,9 @@ Visualization Manager:
       Z position: 0
     - Class: rviz_plugins/DeleteAllObjectsTool
       Pose Topic: /simulation/dummy_perception_publisher/object_info
+  Transformation:
+    Current:
+      Class: rviz_default_plugins/TF
   Value: true
   Views:
     Current:
@@ -1858,7 +2170,7 @@ Visualization Manager:
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Scale: 10
+      Scale: 10.328941345214844
       Target Frame: viewer
       Value: TopDownOrtho (rviz_default_plugins)
       X: 0
@@ -1908,12 +2220,12 @@ Window Geometry:
     collapsed: false
   Displays:
     collapsed: false
-  Height: 1016
+  Height: 1376
   Hide Left Dock: false
   Hide Right Dock: false
   InitialPoseButtonPanel:
     collapsed: false
-  QMainWindow State: 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
+  QMainWindow State: 000000ff00000000fd00000004000000000000029700000531fc0200000010fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000140000012d000000c700fffffffc000001470000010b000000bb0100001afa000000000100000002fb0000000a0056006900650077007301000000000000033c0000010000fffffffb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000000ffffffff0000008c00fffffffb00000024004100750074006f00770061007200650053007400610074006500500061006e0065006c0100000258000001fd0000015e00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d0065007200610100000682000000eb0000000000000000fb0000000a0049006d0061006700650100000505000002680000000000000000fb0000002c0049006e0069007400690061006c0050006f007300650042007500740074006f006e00500061006e0065006c000000068f000000de0000007100fffffffb0000002c0049006e0069007400690061006c0050006f007300650042007500740074006f006e00500061006e0065006c000000068f000000de0000000000000000fb00000030005200650063006f0067006e006900740069006f006e0052006500730075006c0074004f006e0049006d006100670065000000038a0000010b0000002800fffffffb0000002a004100750074006f0077006100720065004400610074006500540069006d006500500061006e0065006c010000045b0000005e0000003f00fffffffb00000034004100750074006f007700610072006500530063007200650065006e004300610070007400750072006500500061006e0065006c01000004bf000000860000005d00ffffff000000010000015f000006fffc0200000002fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000e7a0000005afc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000e7a0000005afc0100000002fb0000000800540069006d0065010000000000000e7a0000000000000000fb0000000800540069006d00650100000000000004500000000000000000000009df0000053100000004000000040000000800000008fc0000000100000000000000010000000a0054006f006f006c00730300000000ffffffff0000000000000000
   RecognitionResultOnImage:
     collapsed: false
   Selection:
@@ -1922,6 +2234,6 @@ Window Geometry:
     collapsed: false
   Views:
     collapsed: false
-  Width: 1920
+  Width: 3370
   X: 70
   Y: 27