diff --git a/master_launch_使用指南.md b/master_launch_使用指南.md
new file mode 100644
index 0000000..dc8ccf7
--- /dev/null
+++ b/master_launch_使用指南.md
@@ -0,0 +1,227 @@
+# master_launch.py 使用指南
+
+## 启动顺序
+
+```
+t=0s  底盘驱动 + EKF + TF + IMU (origincar_bringup)
+t=2s  激光雷达 (lslidar_driver)
+t=5s  SLAM 建图 (slam_toolbox)
+t=8s  Nav2 导航栈
+t=10s VLM 图生文 + TTS 语音播报
+```
+
+每个阶段依赖前一个阶段就绪，属于**严格时间线启动**，不可调换。
+
+---
+
+## 参数一览
+
+| 参数 | 默认值 | 说明 |
+|---|---|---|
+| `use_base` | true | 底盘驱动 + EKF + TF |
+| `use_lidar` | true | 激光雷达 (lsn10) |
+| `use_slam` | true | slam_toolbox 建图 |
+| `use_nav2` | true | Nav2 导航栈 |
+| `use_vlm` | true | VLM 图生文 + TTS |
+| `use_tts` | true | TTS 语音播报开关 |
+| `akmcar` | true | 阿克曼底盘 (false=差速) |
+| `carto_slam` | false | 使用 Cartographer 替代 EKF |
+| `vlm_host` | http://192.168.10.189:8000 | VLM 推理服务地址 |
+| `use_sim_time` | false | 使用仿真时间 |
+
+---
+
+## 常用命令
+
+### 全部启动
+```bash
+ros2 launch my_robot_bringup master_launch.py
+```
+
+### 只调试底盘
+```bash
+ros2 launch my_robot_bringup master_launch.py use_lidar:=false use_slam:=false use_nav2:=false use_vlm:=false
+```
+
+### 只建图（底盘 + 雷达 + SLAM）
+```bash
+ros2 launch my_robot_bringup master_launch.py use_nav2:=false use_vlm:=false
+```
+
+### 只启动 VLM（调试图生文）
+```bash
+ros2 launch my_robot_bringup master_launch.py use_base:=false use_lidar:=false use_slam:=false use_nav2:=false
+```
+
+### 底盘 + 雷达 + SLAM + 导航（不加 VLM）
+```bash
+ros2 launch my_robot_bringup master_launch.py use_vlm:=false use_tts:=false
+```
+
+### 更换 VLM 服务地址
+```bash
+ros2 launch my_robot_bringup master_launch.py vlm_host:=http://192.168.10.200:8000
+```
+
+### 差分底盘模式
+```bash
+ros2 launch my_robot_bringup master_launch.py akmcar:=false
+```
+
+### 使用 Cartographer 替代 EKF
+```bash
+ros2 launch my_robot_bringup master_launch.py carto_slam:=true
+```
+
+### 使用自定义 SLAM 参数
+```bash
+ros2 launch my_robot_bringup master_launch.py slam_params_file:=/path/to/my_slam.yaml
+```
+
+### 使用自定义 Nav2 参数
+```bash
+ros2 launch my_robot_bringup master_launch.py nav2_params_file:=/path/to/my_nav2.yaml
+```
+
+---
+
+## 前置条件
+
+1. **VLM 服务** — WSL 中先启动 `python vlm_server.py`（如需使用 VLM）
+2. **Portproxy 转发** — Windows 上管理员 PowerShell 执行 `setup_vlm_forward.ps1`（仅需一次，IP 不变则不用重跑）
+3. **重刷环境** — 启动前清理残留进程：
+   ```bash
+   pkill -f ros2; pkill -f espeak; pkill -f ffplay
+   ```
+
+---
+
+## 网络架构（VLM 相关）
+
+```
+机器人 (192.168.10.x)
+    ↓ http://192.168.10.189:8000
+Windows 宿主机 (IP 需固定)
+    ↓ netsh portproxy → 127.0.0.1:8000
+WSL2 vlm_server.py
+```
+
+- Windows IP 不变 → 一切正常
+- Windows IP 变化 → 重跑 `setup_vlm_forward.ps1`，启动时传 `vlm_host:=http://新IP:8000`
+
+
+## 改动的地方
+  master_launch.py 变更内容
+
+  阶段: 1 (t=0s)
+  原来: 底盘 + EKF + TF + IMU
+  现在: 不变
+  ────────────────────────────────────────
+  阶段: 2 (t=2s)
+  原来: 激光雷达
+  现在: 不变
+  ────────────────────────────────────────
+  阶段: 3 (t=4s)
+  原来: —
+  现在: 障碍物检测
+    (obstacle_scanner.launch.py)
+  ────────────────────────────────────────
+  阶段: 4 (t=5s)
+  原来: SLAM Toolbox
+  现在: Map Server + AMCL 定位 (预建地图替代
+    SLAM)
+  ────────────────────────────────────────
+  阶段: 5 (t=7s)
+  原来: Nav2 导航栈
+  现在: Hybrid A* 路径规划 + Pure Pursuit
+    轨迹跟随
+  ────────────────────────────────────────
+  阶段: 6 (t=10s)
+
+● Bash(rm -f /c/Users/DHQ/master_launch_new.py)
+  ⎿  Done
+
+● 修改完成。以下是改动总结：
+
+  master_launch.py 变更内容
+
+  ┌───────────┬───────────────────────┬────────────────────────────────────────────┐
+  │   阶段    │         原来          │                    现在                    │
+  ├───────────┼───────────────────────┼────────────────────────────────────────────┤
+  │ 1 (t=0s)  │ 底盘 + EKF + TF + IMU │ 不变                                       │
+  ├───────────┼───────────────────────┼────────────────────────────────────────────┤
+  │ 2 (t=2s)  │ 激光雷达              │ 不变                                       │
+  ├───────────┼───────────────────────┼────────────────────────────────────────────┤
+  │ 3 (t=4s)  │ —                     │ 障碍物检测 (obstacle_scanner.launch.py)    │
+  ├───────────┼───────────────────────┼────────────────────────────────────────────┤
+  │ 4 (t=5s)  │ SLAM Toolbox          │ Map Server + AMCL 定位 (预建地图替代 SLAM) │
+  ├───────────┼───────────────────────┼────────────────────────────────────────────┤
+  │ 5 (t=7s)  │ Nav2 导航栈           │ Hybrid A* 路径规划 + Pure Pursuit 轨迹跟随 │
+  ├───────────┼───────────────────────┼────────────────────────────────────────────┤
+  │ 6 (t=10s) │ VLM + TTS             │ 不变                                       │
+  └───────────┴───────────────────────┴────────────────────────────────────────────┘
+
+  具体改动点
+
+  1. 移除：cyy_slamtoolbox、gc_navigation2_slamtoolbox、slam_toolbox 节点、nav2_bringup launch
+  2. 新增：obstacle_scanner 节点（发布 /obstacles）
+  3. 新增：map_server + amcl + lifecycle_manager 用于定位
+  4. 新增：grid_astar_theta_node（路径规划，发布 /plan）
+  5. 新增：topology_pure_pursuit_node.py（轨迹跟随，发布 /cmd_vel）
+  6. 话题对齐：planner 的 obstacles_topic 参数覆盖为 /obstacles（与 obstacle_scanner 发布的 topic 一致）
+  7. 原文件已备份为 master_launch.py.bak
+  
+  常用命令
+  source /opt/ros/humble/setup.bash
+  source ~/yiliao_ws/install/setup.bash
+  全部启动
+
+  ros2 launch my_robot_bringup master_launch.py
+
+  只调试底盘
+
+  ros2 launch my_robot_bringup master_launch.py use_lidar:=false use_obstacle:=false use_planner:=false use_vlm:=false
+
+  底盘 + 雷达 + 定位 + 规划跟踪（不开障碍物检测和 VLM）
+
+  ros2 launch my_robot_bringup master_launch.py use_obstacle:=false use_vlm:=false
+
+  只启动 VLM（调试图生文）
+
+  ros2 launch my_robot_bringup master_launch.py use_base:=false use_lidar:=false use_obstacle:=false use_planner:=false
+
+  底盘 + 雷达 + 障碍物检测 + 规划跟踪（不加 VLM）
+
+  ros2 launch my_robot_bringup master_launch.py use_vlm:=false use_tts:=false
+
+  只做障碍物检测（不开规划不开 VLM）
+
+  ros2 launch my_robot_bringup master_launch.py use_planner:=false use_vlm:=false
+
+  只做路径规划+跟踪（不开障碍物检测，靠预建地图）
+
+  ros2 launch my_robot_bringup master_launch.py use_obstacle:=false use_vlm:=false
+
+  更换 VLM 服务地址
+
+  ros2 launch my_robot_bringup master_launch.py vlm_host:=http://192.168.10.200:8000
+
+  差分底盘模式
+
+  ros2 launch my_robot_bringup master_launch.py akmcar:=false
+
+  使用 Cartographer 替代 EKF
+
+  ros2 launch my_robot_bringup master_launch.py carto_slam:=true
+
+  更换地图文件
+
+  ros2 launch my_robot_bringup master_launch.py map_file:=/path/to/my_map.yaml
+
+  更换规划器/跟踪器/AMCL 参数文件
+
+  ros2 launch my_robot_bringup master_launch.py planner_params_file:=/path/to/planner.yaml
+  ros2 launch my_robot_bringup master_launch.py tracker_params_file:=/path/to/tracker.yaml
+  ros2 launch my_robot_bringup master_launch.py amcl_params_file:=/path/to/amcl.yaml
+
+
diff --git a/must_know.md b/must_know.md
index 8d88d6f..044959a 100644
--- a/must_know.md
+++ b/must_know.md
@@ -236,4 +236,7 @@ cd /home/sunrise/yiliao_ws && colcon build --packages-select gc_navigation2_slam
 
 # 查看完整提交历史
 cd /home/sunrise/yiliao_ws && git log --oneline 436e8a2..HEAD
+
+# 启动图生文
+
 ```
diff --git a/src/my_robot_bringup/launch/master_launch.py b/src/my_robot_bringup/launch/master_launch.py
index ee5e81c..722bdf6 100755
--- a/src/my_robot_bringup/launch/master_launch.py
+++ b/src/my_robot_bringup/launch/master_launch.py
@@ -130,21 +130,21 @@ def generate_launch_description():
 
     # ================================================================
     # 阶段 4: USB 摄像头 + 二维码识别  (t=4s)
-    # usb_cam: 驱动 USB 摄像头，发布 /image_raw
+    # car_usb_cam: 使用 hobot_usb_cam 驱动 USB 摄像头，发布 /image_raw
     # qr_detect: 订阅 /image_raw，识别二维码，发布 qr_results
     # ================================================================
-    usb_camera = Node(
-        package='usb_cam',
-        executable='usb_cam_node_exe',
-        name='usb_cam',
-        output='screen',
+    usb_camera = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource([
+            FindPackageShare('car_usb_cam'), '/launch', '/hobot_usb_cam.launch.py'
+        ]),
         condition=IfCondition(use_qr),
-        parameters=[{
-            'video_device': camera_device,
-            'image_size': [640, 480],
-            'pixel_format': 'YUYV',
-            'framerate': 30.0,
-        }],
+        launch_arguments={
+            'usb_video_device': camera_device,
+            'usb_image_width': '640',
+            'usb_image_height': '480',
+            'usb_framerate': '30',
+            'usb_pixel_format': 'yuyv2rgb',
+        }.items(),
     )
 
     qr_detect = Node(
diff --git a/src/origincar_description/CMakeLists.txt b/src/origincar_description/CMakeLists.txt
index 5bc73fc..21934a5 100644
--- a/src/origincar_description/CMakeLists.txt
+++ b/src/origincar_description/CMakeLists.txt
@@ -22,11 +22,7 @@ find_package(ament_cmake REQUIRED)
 # find_package(<dependency> REQUIRED)
 
 install(
-<<<<<<< HEAD
   DIRECTORY launch urdf rviz meshes world 
-=======
-  DIRECTORY launch urdf rviz meshes world
->>>>>>> mo_new
   DESTINATION share/${PROJECT_NAME}
 )
 
diff --git a/src/origincar_msg/CMakeLists.txt b/src/origincar_msg/CMakeLists.txt
index 8697fbf..d64086a 100644
--- a/src/origincar_msg/CMakeLists.txt
+++ b/src/origincar_msg/CMakeLists.txt
@@ -25,6 +25,7 @@ find_package(std_msgs REQUIRED)
 rosidl_generate_interfaces(${PROJECT_NAME}
   "msg/Data.msg"
   "msg/Sign.msg"
+  "srv/Speak.srv"
   DEPENDENCIES std_msgs
   ADD_LINTER_TESTS
 )
diff --git a/src/origincar_msg/srv/Speak.srv b/src/origincar_msg/srv/Speak.srv
new file mode 100644
index 0000000..57fc17e
--- /dev/null
+++ b/src/origincar_msg/srv/Speak.srv
@@ -0,0 +1,4 @@
+string text
+---
+bool success
+string message
diff --git a/src/planner/config/odom_hybrid_astar.yaml b/src/planner/config/odom_hybrid_astar.yaml
index d3c7ce3..0f0537c 100644
--- a/src/planner/config/odom_hybrid_astar.yaml
+++ b/src/planner/config/odom_hybrid_astar.yaml
@@ -46,9 +46,10 @@ grid_astar_theta_planner:
     yaw_bins: 36
     primitive_length: 0.12
     collision_sample_step: 0.03
-    goal_xy_tolerance: 0.06
+    goal_xy_tolerance: 0.10
     goal_yaw_tolerance: 0.174533
     require_goal_yaw: false
+    snap_final_xy_to_goal: true
     use_grid_heuristic: true
     heuristic_weight: 1.35
     goal_yaw_heuristic_weight: 0.0
diff --git a/src/planner/config/odom_pure_pursuit.yaml b/src/planner/config/odom_pure_pursuit.yaml
index d411b26..ebd9e07 100644
--- a/src/planner/config/odom_pure_pursuit.yaml
+++ b/src/planner/config/odom_pure_pursuit.yaml
@@ -14,18 +14,28 @@ topology_pure_pursuit:
     cancel_topic: /navigation_cancel
 
     control_rate: 20.0
-    lookahead_distance: 0.40
+    lookahead_distance: 0.28
+    min_lookahead_distance: 0.10
+    lookahead_curvature_gain: 0.80
+    lookahead_error_gain: 2.00
+    heading_correction_gain: 0.80
+    cross_track_correction_gain: 1.20
+    cross_track_speed_gain: 1.80
+    min_tracking_speed_ratio: 0.35
+    path_reacquire_distance: 0.45
     goal_tolerance: 0.06
     goal_yaw_tolerance: 0.174533
     align_goal_yaw: false
     final_alignment_offset: 0.25
     final_alignment_speed: 0.06
-    linear_speed: 0.60
-    min_linear_speed: 0.08
-    reverse_speed: 0.25
-    slowdown_distance: 0.35
-    curvature_slowdown_gain: 0.10
-    max_angular_speed: 2.80
+    linear_speed: 0.45
+    min_linear_speed: 0.06
+    reverse_speed: 0.18
+    slowdown_distance: 0.45
+    curvature_slowdown_gain: 0.06
+    min_curvature_speed_ratio: 0.70
+    min_goal_slowdown_ratio: 0.35
+    max_angular_speed: 3.20
     min_turning_radius: 0.25
 
     latency_compensation: true
diff --git a/src/planner/config/real_hybrid_astar.yaml b/src/planner/config/real_hybrid_astar.yaml
index 4ded55e..3d64e9f 100644
--- a/src/planner/config/real_hybrid_astar.yaml
+++ b/src/planner/config/real_hybrid_astar.yaml
@@ -47,6 +47,7 @@ grid_astar_theta_planner:
     # Point navigation does not require the car to finish at a specific yaw.
     # Set true when final docking orientation is required.
     require_goal_yaw: false
+    snap_final_xy_to_goal: true
     use_grid_heuristic: true
     heuristic_weight: 1.25
     goal_yaw_heuristic_weight: 0.0
diff --git a/src/planner/config/real_pure_pursuit.yaml b/src/planner/config/real_pure_pursuit.yaml
index a479cf2..63ac86a 100644
--- a/src/planner/config/real_pure_pursuit.yaml
+++ b/src/planner/config/real_pure_pursuit.yaml
@@ -14,16 +14,26 @@ topology_pure_pursuit:
     cancel_topic: /navigation_cancel
 
     control_rate: 20.0
-    lookahead_distance: 0.45
+    lookahead_distance: 0.30
+    min_lookahead_distance: 0.12
+    lookahead_curvature_gain: 0.70
+    lookahead_error_gain: 1.60
+    heading_correction_gain: 0.70
+    cross_track_correction_gain: 1.00
+    cross_track_speed_gain: 1.50
+    min_tracking_speed_ratio: 0.35
+    path_reacquire_distance: 0.45
     goal_tolerance: 0.15
     goal_yaw_tolerance: 0.174533
     align_goal_yaw: false
-    linear_speed: 0.20
+    linear_speed: 0.18
     min_linear_speed: 0.06
-    reverse_speed: 0.10
+    reverse_speed: 0.08
     slowdown_distance: 0.70
-    curvature_slowdown_gain: 0.25
-    max_angular_speed: 1.20
+    curvature_slowdown_gain: 0.12
+    min_curvature_speed_ratio: 0.65
+    min_goal_slowdown_ratio: 0.35
+    max_angular_speed: 1.50
     min_turning_radius: 0.40
 
     latency_compensation: true
diff --git a/src/planner/launch/odom_hybrid_astar.launch.py b/src/planner/launch/odom_hybrid_astar.launch.py
index 84313e0..408ce16 100644
--- a/src/planner/launch/odom_hybrid_astar.launch.py
+++ b/src/planner/launch/odom_hybrid_astar.launch.py
@@ -41,11 +41,21 @@ def generate_launch_description():
     planner_min_turning_radius = LaunchConfiguration("planner_min_turning_radius")
     tracker_min_turning_radius = LaunchConfiguration("tracker_min_turning_radius")
     lookahead_distance = LaunchConfiguration("lookahead_distance")
+    min_lookahead_distance = LaunchConfiguration("min_lookahead_distance")
+    lookahead_curvature_gain = LaunchConfiguration("lookahead_curvature_gain")
+    lookahead_error_gain = LaunchConfiguration("lookahead_error_gain")
+    heading_correction_gain = LaunchConfiguration("heading_correction_gain")
+    cross_track_correction_gain = LaunchConfiguration("cross_track_correction_gain")
+    cross_track_speed_gain = LaunchConfiguration("cross_track_speed_gain")
+    min_tracking_speed_ratio = LaunchConfiguration("min_tracking_speed_ratio")
+    path_reacquire_distance = LaunchConfiguration("path_reacquire_distance")
     linear_speed = LaunchConfiguration("linear_speed")
     min_linear_speed = LaunchConfiguration("min_linear_speed")
     reverse_speed = LaunchConfiguration("reverse_speed")
     slowdown_distance = LaunchConfiguration("slowdown_distance")
     curvature_slowdown_gain = LaunchConfiguration("curvature_slowdown_gain")
+    min_curvature_speed_ratio = LaunchConfiguration("min_curvature_speed_ratio")
+    min_goal_slowdown_ratio = LaunchConfiguration("min_goal_slowdown_ratio")
     max_angular_speed = LaunchConfiguration("max_angular_speed")
     goal_tolerance = LaunchConfiguration("goal_tolerance")
     goal_yaw_tolerance = LaunchConfiguration("goal_yaw_tolerance")
@@ -54,6 +64,7 @@ def generate_launch_description():
     final_alignment_speed = LaunchConfiguration("final_alignment_speed")
     require_goal_yaw = LaunchConfiguration("require_goal_yaw")
     planner_goal_xy_tolerance = LaunchConfiguration("planner_goal_xy_tolerance")
+    snap_final_xy_to_goal = LaunchConfiguration("snap_final_xy_to_goal")
     use_grid_heuristic = LaunchConfiguration("use_grid_heuristic")
     heuristic_weight = LaunchConfiguration("heuristic_weight")
     goal_yaw_heuristic_weight = LaunchConfiguration("goal_yaw_heuristic_weight")
@@ -83,6 +94,30 @@ def generate_launch_description():
         tracker_min_turning_radius, value_type=float
     )
     lookahead_distance_value = ParameterValue(lookahead_distance, value_type=float)
+    min_lookahead_distance_value = ParameterValue(
+        min_lookahead_distance, value_type=float
+    )
+    lookahead_curvature_gain_value = ParameterValue(
+        lookahead_curvature_gain, value_type=float
+    )
+    lookahead_error_gain_value = ParameterValue(
+        lookahead_error_gain, value_type=float
+    )
+    heading_correction_gain_value = ParameterValue(
+        heading_correction_gain, value_type=float
+    )
+    cross_track_correction_gain_value = ParameterValue(
+        cross_track_correction_gain, value_type=float
+    )
+    cross_track_speed_gain_value = ParameterValue(
+        cross_track_speed_gain, value_type=float
+    )
+    min_tracking_speed_ratio_value = ParameterValue(
+        min_tracking_speed_ratio, value_type=float
+    )
+    path_reacquire_distance_value = ParameterValue(
+        path_reacquire_distance, value_type=float
+    )
     linear_speed_value = ParameterValue(linear_speed, value_type=float)
     min_linear_speed_value = ParameterValue(min_linear_speed, value_type=float)
     reverse_speed_value = ParameterValue(reverse_speed, value_type=float)
@@ -90,6 +125,12 @@ def generate_launch_description():
     curvature_slowdown_gain_value = ParameterValue(
         curvature_slowdown_gain, value_type=float
     )
+    min_curvature_speed_ratio_value = ParameterValue(
+        min_curvature_speed_ratio, value_type=float
+    )
+    min_goal_slowdown_ratio_value = ParameterValue(
+        min_goal_slowdown_ratio, value_type=float
+    )
     max_angular_speed_value = ParameterValue(max_angular_speed, value_type=float)
     goal_tolerance_value = ParameterValue(goal_tolerance, value_type=float)
     goal_yaw_tolerance_value = ParameterValue(goal_yaw_tolerance, value_type=float)
@@ -104,6 +145,9 @@ def generate_launch_description():
     planner_goal_xy_tolerance_value = ParameterValue(
         planner_goal_xy_tolerance, value_type=float
     )
+    snap_final_xy_to_goal_value = ParameterValue(
+        snap_final_xy_to_goal, value_type=bool
+    )
     use_grid_heuristic_value = ParameterValue(use_grid_heuristic, value_type=bool)
     heuristic_weight_value = ParameterValue(heuristic_weight, value_type=float)
     goal_yaw_heuristic_weight_value = ParameterValue(
@@ -225,6 +269,7 @@ def generate_launch_description():
             "min_turning_radius": planner_min_turning_radius_value,
             "require_goal_yaw": require_goal_yaw_value,
             "goal_xy_tolerance": planner_goal_xy_tolerance_value,
+            "snap_final_xy_to_goal": snap_final_xy_to_goal_value,
             "use_grid_heuristic": use_grid_heuristic_value,
             "heuristic_weight": heuristic_weight_value,
             "goal_yaw_heuristic_weight": goal_yaw_heuristic_weight_value,
@@ -251,11 +296,21 @@ def generate_launch_description():
             "cmd_vel_topic": "/planner_cmd_vel",
             "min_turning_radius": tracker_min_turning_radius_value,
             "lookahead_distance": lookahead_distance_value,
+            "min_lookahead_distance": min_lookahead_distance_value,
+            "lookahead_curvature_gain": lookahead_curvature_gain_value,
+            "lookahead_error_gain": lookahead_error_gain_value,
+            "heading_correction_gain": heading_correction_gain_value,
+            "cross_track_correction_gain": cross_track_correction_gain_value,
+            "cross_track_speed_gain": cross_track_speed_gain_value,
+            "min_tracking_speed_ratio": min_tracking_speed_ratio_value,
+            "path_reacquire_distance": path_reacquire_distance_value,
             "linear_speed": linear_speed_value,
             "min_linear_speed": min_linear_speed_value,
             "reverse_speed": reverse_speed_value,
             "slowdown_distance": slowdown_distance_value,
             "curvature_slowdown_gain": curvature_slowdown_gain_value,
+            "min_curvature_speed_ratio": min_curvature_speed_ratio_value,
+            "min_goal_slowdown_ratio": min_goal_slowdown_ratio_value,
             "max_angular_speed": max_angular_speed_value,
             "goal_tolerance": goal_tolerance_value,
             "goal_yaw_tolerance": goal_yaw_tolerance_value,
@@ -308,15 +363,25 @@ def generate_launch_description():
         ),
         DeclareLaunchArgument(
             "lookahead_distance",
-            default_value="0.40",
+            default_value="0.28",
             description="Pure Pursuit lookahead in meters.",
         ),
-        DeclareLaunchArgument("linear_speed", default_value="0.60"),
-        DeclareLaunchArgument("min_linear_speed", default_value="0.08"),
-        DeclareLaunchArgument("reverse_speed", default_value="0.25"),
-        DeclareLaunchArgument("slowdown_distance", default_value="0.35"),
-        DeclareLaunchArgument("curvature_slowdown_gain", default_value="0.10"),
-        DeclareLaunchArgument("max_angular_speed", default_value="2.80"),
+        DeclareLaunchArgument("min_lookahead_distance", default_value="0.10"),
+        DeclareLaunchArgument("lookahead_curvature_gain", default_value="0.80"),
+        DeclareLaunchArgument("lookahead_error_gain", default_value="2.00"),
+        DeclareLaunchArgument("heading_correction_gain", default_value="0.80"),
+        DeclareLaunchArgument("cross_track_correction_gain", default_value="1.20"),
+        DeclareLaunchArgument("cross_track_speed_gain", default_value="1.80"),
+        DeclareLaunchArgument("min_tracking_speed_ratio", default_value="0.35"),
+        DeclareLaunchArgument("path_reacquire_distance", default_value="0.45"),
+        DeclareLaunchArgument("linear_speed", default_value="0.45"),
+        DeclareLaunchArgument("min_linear_speed", default_value="0.06"),
+        DeclareLaunchArgument("reverse_speed", default_value="0.18"),
+        DeclareLaunchArgument("slowdown_distance", default_value="0.45"),
+        DeclareLaunchArgument("curvature_slowdown_gain", default_value="0.06"),
+        DeclareLaunchArgument("min_curvature_speed_ratio", default_value="0.70"),
+        DeclareLaunchArgument("min_goal_slowdown_ratio", default_value="0.35"),
+        DeclareLaunchArgument("max_angular_speed", default_value="3.20"),
         DeclareLaunchArgument("goal_tolerance", default_value="0.06"),
         DeclareLaunchArgument("goal_yaw_tolerance", default_value="0.174533"),
         DeclareLaunchArgument("align_goal_yaw", default_value="false"),
@@ -329,9 +394,14 @@ def generate_launch_description():
         ),
         DeclareLaunchArgument(
             "planner_goal_xy_tolerance",
-            default_value="0.06",
+            default_value="0.10",
             description="Hybrid A* final XY tolerance in meters.",
         ),
+        DeclareLaunchArgument(
+            "snap_final_xy_to_goal",
+            default_value="true",
+            description="Publish the final path XY exactly at the requested goal XY without forcing yaw.",
+        ),
         DeclareLaunchArgument(
             "use_grid_heuristic",
             default_value="true",
diff --git a/src/planner/launch/real_hybrid_astar.launch.py b/src/planner/launch/real_hybrid_astar.launch.py
index 89f8dc0..b69ee38 100644
--- a/src/planner/launch/real_hybrid_astar.launch.py
+++ b/src/planner/launch/real_hybrid_astar.launch.py
@@ -32,9 +32,27 @@ def generate_launch_description():
     enable_motion = LaunchConfiguration("enable_motion")
     wheelbase = LaunchConfiguration("wheelbase")
     max_steering_angle = LaunchConfiguration("max_steering_angle")
+    lookahead_distance = LaunchConfiguration("lookahead_distance")
+    min_lookahead_distance = LaunchConfiguration("min_lookahead_distance")
+    lookahead_curvature_gain = LaunchConfiguration("lookahead_curvature_gain")
+    lookahead_error_gain = LaunchConfiguration("lookahead_error_gain")
+    heading_correction_gain = LaunchConfiguration("heading_correction_gain")
+    cross_track_correction_gain = LaunchConfiguration("cross_track_correction_gain")
+    cross_track_speed_gain = LaunchConfiguration("cross_track_speed_gain")
+    min_tracking_speed_ratio = LaunchConfiguration("min_tracking_speed_ratio")
+    path_reacquire_distance = LaunchConfiguration("path_reacquire_distance")
+    linear_speed = LaunchConfiguration("linear_speed")
+    min_linear_speed = LaunchConfiguration("min_linear_speed")
+    reverse_speed = LaunchConfiguration("reverse_speed")
+    slowdown_distance = LaunchConfiguration("slowdown_distance")
+    curvature_slowdown_gain = LaunchConfiguration("curvature_slowdown_gain")
+    min_curvature_speed_ratio = LaunchConfiguration("min_curvature_speed_ratio")
+    min_goal_slowdown_ratio = LaunchConfiguration("min_goal_slowdown_ratio")
+    max_angular_speed = LaunchConfiguration("max_angular_speed")
     use_grid_heuristic = LaunchConfiguration("use_grid_heuristic")
     heuristic_weight = LaunchConfiguration("heuristic_weight")
     goal_yaw_heuristic_weight = LaunchConfiguration("goal_yaw_heuristic_weight")
+    snap_final_xy_to_goal = LaunchConfiguration("snap_final_xy_to_goal")
     state_log_period = LaunchConfiguration("state_log_period")
     search_progress_log_period_ms = LaunchConfiguration("search_progress_log_period_ms")
     use_sim_time = LaunchConfiguration("use_sim_time")
@@ -42,11 +60,53 @@ def generate_launch_description():
     enable_motion_value = ParameterValue(enable_motion, value_type=bool)
     wheelbase_value = ParameterValue(wheelbase, value_type=float)
     max_steering_angle_value = ParameterValue(max_steering_angle, value_type=float)
+    lookahead_distance_value = ParameterValue(lookahead_distance, value_type=float)
+    min_lookahead_distance_value = ParameterValue(
+        min_lookahead_distance, value_type=float
+    )
+    lookahead_curvature_gain_value = ParameterValue(
+        lookahead_curvature_gain, value_type=float
+    )
+    lookahead_error_gain_value = ParameterValue(
+        lookahead_error_gain, value_type=float
+    )
+    heading_correction_gain_value = ParameterValue(
+        heading_correction_gain, value_type=float
+    )
+    cross_track_correction_gain_value = ParameterValue(
+        cross_track_correction_gain, value_type=float
+    )
+    cross_track_speed_gain_value = ParameterValue(
+        cross_track_speed_gain, value_type=float
+    )
+    min_tracking_speed_ratio_value = ParameterValue(
+        min_tracking_speed_ratio, value_type=float
+    )
+    path_reacquire_distance_value = ParameterValue(
+        path_reacquire_distance, value_type=float
+    )
+    linear_speed_value = ParameterValue(linear_speed, value_type=float)
+    min_linear_speed_value = ParameterValue(min_linear_speed, value_type=float)
+    reverse_speed_value = ParameterValue(reverse_speed, value_type=float)
+    slowdown_distance_value = ParameterValue(slowdown_distance, value_type=float)
+    curvature_slowdown_gain_value = ParameterValue(
+        curvature_slowdown_gain, value_type=float
+    )
+    min_curvature_speed_ratio_value = ParameterValue(
+        min_curvature_speed_ratio, value_type=float
+    )
+    min_goal_slowdown_ratio_value = ParameterValue(
+        min_goal_slowdown_ratio, value_type=float
+    )
+    max_angular_speed_value = ParameterValue(max_angular_speed, value_type=float)
     use_grid_heuristic_value = ParameterValue(use_grid_heuristic, value_type=bool)
     heuristic_weight_value = ParameterValue(heuristic_weight, value_type=float)
     goal_yaw_heuristic_weight_value = ParameterValue(
         goal_yaw_heuristic_weight, value_type=float
     )
+    snap_final_xy_to_goal_value = ParameterValue(
+        snap_final_xy_to_goal, value_type=bool
+    )
     state_log_period_value = ParameterValue(state_log_period, value_type=float)
     search_progress_log_period_ms_value = ParameterValue(
         search_progress_log_period_ms, value_type=float
@@ -168,6 +228,7 @@ def generate_launch_description():
             "use_grid_heuristic": use_grid_heuristic_value,
             "heuristic_weight": heuristic_weight_value,
             "goal_yaw_heuristic_weight": goal_yaw_heuristic_weight_value,
+            "snap_final_xy_to_goal": snap_final_xy_to_goal_value,
             "state_log_period": state_log_period_value,
             "search_progress_log_period_ms": search_progress_log_period_ms_value,
         }],
@@ -182,6 +243,23 @@ def generate_launch_description():
             "use_sim_time": use_sim_time_value,
             "publish_cmd_vel": enable_motion_value,
             "cmd_vel_topic": "/planner_cmd_vel",
+            "lookahead_distance": lookahead_distance_value,
+            "min_lookahead_distance": min_lookahead_distance_value,
+            "lookahead_curvature_gain": lookahead_curvature_gain_value,
+            "lookahead_error_gain": lookahead_error_gain_value,
+            "heading_correction_gain": heading_correction_gain_value,
+            "cross_track_correction_gain": cross_track_correction_gain_value,
+            "cross_track_speed_gain": cross_track_speed_gain_value,
+            "min_tracking_speed_ratio": min_tracking_speed_ratio_value,
+            "path_reacquire_distance": path_reacquire_distance_value,
+            "linear_speed": linear_speed_value,
+            "min_linear_speed": min_linear_speed_value,
+            "reverse_speed": reverse_speed_value,
+            "slowdown_distance": slowdown_distance_value,
+            "curvature_slowdown_gain": curvature_slowdown_gain_value,
+            "min_curvature_speed_ratio": min_curvature_speed_ratio_value,
+            "min_goal_slowdown_ratio": min_goal_slowdown_ratio_value,
+            "max_angular_speed": max_angular_speed_value,
             "state_log_period": state_log_period_value,
         }],
     )
@@ -211,6 +289,23 @@ def generate_launch_description():
         ),
         DeclareLaunchArgument("wheelbase", default_value="0.143"),
         DeclareLaunchArgument("max_steering_angle", default_value="0.60"),
+        DeclareLaunchArgument("lookahead_distance", default_value="0.30"),
+        DeclareLaunchArgument("min_lookahead_distance", default_value="0.12"),
+        DeclareLaunchArgument("lookahead_curvature_gain", default_value="0.70"),
+        DeclareLaunchArgument("lookahead_error_gain", default_value="1.60"),
+        DeclareLaunchArgument("heading_correction_gain", default_value="0.70"),
+        DeclareLaunchArgument("cross_track_correction_gain", default_value="1.00"),
+        DeclareLaunchArgument("cross_track_speed_gain", default_value="1.50"),
+        DeclareLaunchArgument("min_tracking_speed_ratio", default_value="0.35"),
+        DeclareLaunchArgument("path_reacquire_distance", default_value="0.45"),
+        DeclareLaunchArgument("linear_speed", default_value="0.18"),
+        DeclareLaunchArgument("min_linear_speed", default_value="0.06"),
+        DeclareLaunchArgument("reverse_speed", default_value="0.08"),
+        DeclareLaunchArgument("slowdown_distance", default_value="0.70"),
+        DeclareLaunchArgument("curvature_slowdown_gain", default_value="0.12"),
+        DeclareLaunchArgument("min_curvature_speed_ratio", default_value="0.65"),
+        DeclareLaunchArgument("min_goal_slowdown_ratio", default_value="0.35"),
+        DeclareLaunchArgument("max_angular_speed", default_value="1.50"),
         DeclareLaunchArgument(
             "use_grid_heuristic",
             default_value="true",
@@ -226,6 +321,11 @@ def generate_launch_description():
             default_value="0.0",
             description="Weight for final yaw guidance in the Hybrid A* heuristic.",
         ),
+        DeclareLaunchArgument(
+            "snap_final_xy_to_goal",
+            default_value="true",
+            description="Publish the final path XY exactly at the requested goal XY without forcing yaw.",
+        ),
         DeclareLaunchArgument(
             "state_log_period",
             default_value="1.0",
diff --git a/src/planner/scripts/__pycache__/ackermann_cmd_bridge.cpython-313.pyc b/src/planner/scripts/__pycache__/ackermann_cmd_bridge.cpython-313.pyc
new file mode 100644
index 0000000..a4c0cd6
Binary files /dev/null and b/src/planner/scripts/__pycache__/ackermann_cmd_bridge.cpython-313.pyc differ
diff --git a/src/planner/scripts/__pycache__/evaluate_navigation_test.cpython-313.pyc b/src/planner/scripts/__pycache__/evaluate_navigation_test.cpython-313.pyc
new file mode 100644
index 0000000..d554516
Binary files /dev/null and b/src/planner/scripts/__pycache__/evaluate_navigation_test.cpython-313.pyc differ
diff --git a/src/planner/scripts/__pycache__/navigation_test_recorder.cpython-313.pyc b/src/planner/scripts/__pycache__/navigation_test_recorder.cpython-313.pyc
new file mode 100644
index 0000000..a885a27
Binary files /dev/null and b/src/planner/scripts/__pycache__/navigation_test_recorder.cpython-313.pyc differ
diff --git a/src/planner/scripts/__pycache__/odom_map_tf.cpython-313.pyc b/src/planner/scripts/__pycache__/odom_map_tf.cpython-313.pyc
new file mode 100644
index 0000000..9dbd556
Binary files /dev/null and b/src/planner/scripts/__pycache__/odom_map_tf.cpython-313.pyc differ
diff --git a/src/planner/scripts/__pycache__/topology_pure_pursuit_node.cpython-313.pyc b/src/planner/scripts/__pycache__/topology_pure_pursuit_node.cpython-313.pyc
new file mode 100644
index 0000000..56b37c1
Binary files /dev/null and b/src/planner/scripts/__pycache__/topology_pure_pursuit_node.cpython-313.pyc differ
diff --git a/src/planner/scripts/goal_reach_debug_collector.py b/src/planner/scripts/goal_reach_debug_collector.py
index fd760a4..11f5009 100755
--- a/src/planner/scripts/goal_reach_debug_collector.py
+++ b/src/planner/scripts/goal_reach_debug_collector.py
@@ -142,6 +142,7 @@ class GoalReachDebugCollector(Node):
         self.finished = False
 
         self.latest_odom = None
+        self.latest_odom_received_ns = 0
         self.latest_goal = None
         self.latest_plan_points = []
         self.latest_cmd = None
@@ -208,6 +209,7 @@ class GoalReachDebugCollector(Node):
 
     def _odom_cb(self, msg):
         self.latest_odom = msg
+        self.latest_odom_received_ns = self.get_clock().now().nanoseconds
 
     def _goal_cb(self, msg):
         goal = {
@@ -348,18 +350,21 @@ class GoalReachDebugCollector(Node):
             return None
 
         msg = self.latest_odom
-        stamp = Time.from_msg(msg.header.stamp)
+        now_ns = self.get_clock().now().nanoseconds
+        receive_age = None
+        if self.latest_odom_received_ns > 0:
+            receive_age = max(0.0, (now_ns - self.latest_odom_received_ns) * 1e-9)
         if msg.header.stamp.sec == 0 and msg.header.stamp.nanosec == 0:
-            age = 0.0
+            stamp_age = None
         else:
-            age = (self.get_clock().now() - stamp).nanoseconds * 1e-9
-        if age < 0.0:
-            age = 0.0
-        if age > self.max_odom_age:
+            stamp = Time.from_msg(msg.header.stamp)
+            stamp_age = max(0.0, (self.get_clock().now() - stamp).nanoseconds * 1e-9)
+        if receive_age is not None and receive_age > self.max_odom_age:
             return {
                 "available": False,
                 "reason": "odom_too_old",
-                "age_sec": age,
+                "odom_receive_age_sec": receive_age,
+                "odom_stamp_age_sec": stamp_age,
             }
 
         source_frame = msg.header.frame_id or self.map_frame
@@ -381,7 +386,8 @@ class GoalReachDebugCollector(Node):
             "x": x,
             "y": y,
             "yaw": yaw,
-            "odom_age_sec": age,
+            "odom_receive_age_sec": receive_age,
+            "odom_stamp_age_sec": stamp_age,
             "odom_linear_x": msg.twist.twist.linear.x,
             "odom_angular_z": msg.twist.twist.angular.z,
         }
@@ -448,6 +454,7 @@ class GoalReachDebugCollector(Node):
                     "goal_xy_tolerance",
                     "goal_yaw_tolerance",
                     "require_goal_yaw",
+                    "snap_final_xy_to_goal",
                     "resolution",
                     "yaw_bins",
                     "primitive_length",
@@ -467,11 +474,21 @@ class GoalReachDebugCollector(Node):
                     "publish_cmd_vel",
                     "cmd_vel_topic",
                     "lookahead_distance",
+                    "min_lookahead_distance",
+                    "lookahead_curvature_gain",
+                    "lookahead_error_gain",
+                    "heading_correction_gain",
+                    "cross_track_correction_gain",
+                    "cross_track_speed_gain",
+                    "min_tracking_speed_ratio",
+                    "path_reacquire_distance",
                     "linear_speed",
                     "min_linear_speed",
                     "reverse_speed",
                     "slowdown_distance",
                     "curvature_slowdown_gain",
+                    "min_curvature_speed_ratio",
+                    "min_goal_slowdown_ratio",
                     "min_turning_radius",
                     "max_angular_speed",
                 ],
@@ -628,8 +645,9 @@ class GoalReachDebugCollector(Node):
 
         planner_xy = planner.get("goal_xy_tolerance")
         tracker_xy = tracker.get("goal_tolerance")
+        snap_xy = bool(planner.get("snap_final_xy_to_goal"))
         if isinstance(planner_xy, (int, float)) and isinstance(tracker_xy, (int, float)):
-            if planner_xy > tracker_xy:
+            if planner_xy > tracker_xy and not snap_xy:
                 hints.append(
                     "planner 的 goal_xy_tolerance 大于 tracker 的 goal_tolerance；"
                     "planner 可能发布一个离原始目标较远的路径终点。"
diff --git a/src/planner/scripts/preflight_check.py b/src/planner/scripts/preflight_check.py
index 345d2d9..8859fa8 100755
--- a/src/planner/scripts/preflight_check.py
+++ b/src/planner/scripts/preflight_check.py
@@ -32,7 +32,7 @@ class PreflightCheck(Node):
         self.declare_parameter("map_topic", "/map")
         self.declare_parameter("odom_topic", "/odom_combined")
         self.declare_parameter("scan_topic", "/scan")
-        self.declare_parameter("cmd_vel_topic", "/cmd_vel")
+        self.declare_parameter("cmd_vel_topic", "/planner_cmd_vel")
         self.declare_parameter("expected_scan_frame", "laser_link")
         self.declare_parameter("tf_wait_sec", 3.0)
         self.declare_parameter("drive_command_topic", "/ackermann_cmd")
diff --git a/src/planner/scripts/send_goal.py b/src/planner/scripts/send_goal.py
index 3859919..1e9d586 100755
--- a/src/planner/scripts/send_goal.py
+++ b/src/planner/scripts/send_goal.py
@@ -39,8 +39,8 @@ def main():
     parser.add_argument("yaw", type=float, nargs="?", default=0.0)
     parser.add_argument("--topic", default="/goal_pose")
     parser.add_argument("--frame", default="map")
-    parser.add_argument("--timeout", type=float, default=5.0)
-    parser.add_argument("--repeat", type=int, default=1)
+    parser.add_argument("--timeout", type=float, default=8.0)
+    parser.add_argument("--repeat", type=int, default=3)
     args = parser.parse_args()
 
     rclpy.init()
diff --git a/src/planner/scripts/topology_pure_pursuit_node.py b/src/planner/scripts/topology_pure_pursuit_node.py
index 54086d6..045e173 100755
--- a/src/planner/scripts/topology_pure_pursuit_node.py
+++ b/src/planner/scripts/topology_pure_pursuit_node.py
@@ -62,12 +62,22 @@ class TopologyPurePursuit(Node):
         self.declare_parameter("path_resolution", 0.10)
         self.declare_parameter("nearest_node_max_distance", 2.0)
         self.declare_parameter("lookahead_distance", 0.55)
+        self.declare_parameter("min_lookahead_distance", 0.16)
+        self.declare_parameter("lookahead_curvature_gain", 0.50)
+        self.declare_parameter("lookahead_error_gain", 1.20)
+        self.declare_parameter("heading_correction_gain", 0.55)
+        self.declare_parameter("cross_track_correction_gain", 0.80)
+        self.declare_parameter("cross_track_speed_gain", 1.20)
+        self.declare_parameter("min_tracking_speed_ratio", 0.40)
+        self.declare_parameter("path_reacquire_distance", 0.50)
         self.declare_parameter("goal_tolerance", 0.25)
         self.declare_parameter("linear_speed", 0.30)
         self.declare_parameter("min_linear_speed", 0.08)
         self.declare_parameter("reverse_speed", 0.12)
         self.declare_parameter("slowdown_distance", 0.80)
         self.declare_parameter("curvature_slowdown_gain", 0.18)
+        self.declare_parameter("min_curvature_speed_ratio", 0.35)
+        self.declare_parameter("min_goal_slowdown_ratio", 0.25)
         self.declare_parameter("max_angular_speed", 1.5)
         self.declare_parameter("min_turning_radius", 0.35)
         self.declare_parameter("goal_yaw_tolerance", 0.10)
@@ -99,6 +109,35 @@ class TopologyPurePursuit(Node):
         self.lookahead_distance = float(
             self.get_parameter("lookahead_distance").value
         )
+        self.min_lookahead_distance = max(
+            0.05,
+            min(
+                self.lookahead_distance,
+                float(self.get_parameter("min_lookahead_distance").value),
+            ),
+        )
+        self.lookahead_curvature_gain = max(
+            0.0, float(self.get_parameter("lookahead_curvature_gain").value)
+        )
+        self.lookahead_error_gain = max(
+            0.0, float(self.get_parameter("lookahead_error_gain").value)
+        )
+        self.heading_correction_gain = max(
+            0.0, float(self.get_parameter("heading_correction_gain").value)
+        )
+        self.cross_track_correction_gain = max(
+            0.0, float(self.get_parameter("cross_track_correction_gain").value)
+        )
+        self.cross_track_speed_gain = max(
+            0.0, float(self.get_parameter("cross_track_speed_gain").value)
+        )
+        self.min_tracking_speed_ratio = max(
+            0.0,
+            min(1.0, float(self.get_parameter("min_tracking_speed_ratio").value)),
+        )
+        self.path_reacquire_distance = max(
+            0.0, float(self.get_parameter("path_reacquire_distance").value)
+        )
         self.goal_tolerance = float(self.get_parameter("goal_tolerance").value)
         self.linear_speed = float(self.get_parameter("linear_speed").value)
         self.min_linear_speed = float(self.get_parameter("min_linear_speed").value)
@@ -107,6 +146,14 @@ class TopologyPurePursuit(Node):
         self.curvature_slowdown_gain = float(
             self.get_parameter("curvature_slowdown_gain").value
         )
+        self.min_curvature_speed_ratio = max(
+            0.0,
+            min(1.0, float(self.get_parameter("min_curvature_speed_ratio").value)),
+        )
+        self.min_goal_slowdown_ratio = max(
+            0.0,
+            min(1.0, float(self.get_parameter("min_goal_slowdown_ratio").value)),
+        )
         self.max_angular_speed = float(
             self.get_parameter("max_angular_speed").value
         )
@@ -151,6 +198,7 @@ class TopologyPurePursuit(Node):
         self.tf_listener = TransformListener(self.tf_buffer, self)
 
         self.latest_odom = None
+        self.latest_odom_received_ns = 0
         self.active_path = []
         self.active_node_path = []
         self.active_goal_node = ""
@@ -162,9 +210,11 @@ class TopologyPurePursuit(Node):
         self.last_cmd_linear = 0.0
         self.last_cmd_angular = 0.0
 
-        self.plan_pub = self.create_publisher(
-            NavPath, self.get_parameter("plan_topic").value, 10
-        )
+        self.plan_pub = None
+        if self.enable_topology_planning:
+            self.plan_pub = self.create_publisher(
+                NavPath, self.get_parameter("plan_topic").value, 10
+            )
         self.cmd_pub = self.create_publisher(
             Twist, self.get_parameter("cmd_vel_topic").value, 10
         )
@@ -272,6 +322,7 @@ class TopologyPurePursuit(Node):
 
     def _odom_callback(self, msg):
         self.latest_odom = msg
+        self.latest_odom_received_ns = self.get_clock().now().nanoseconds
         if (
             self.enable_topology_planning
             and self.autoplan_default_goal
@@ -462,6 +513,8 @@ class TopologyPurePursuit(Node):
         return updated
 
     def _publish_path(self):
+        if self.plan_pub is None:
+            return
         path_msg = NavPath()
         path_msg.header.frame_id = self.map_frame
         path_msg.header.stamp = self.get_clock().now().to_msg()
@@ -474,6 +527,12 @@ class TopologyPurePursuit(Node):
     def _current_pose(self, compensate=True):
         if self.latest_odom is None:
             return None
+        now_ns = self.get_clock().now().nanoseconds
+        if self.latest_odom_received_ns <= 0:
+            return None
+        age = max(0.0, (now_ns - self.latest_odom_received_ns) * 1e-9)
+        if age > self.max_odom_age:
+            return None
 
         msg = self.latest_odom
         source_frame = msg.header.frame_id or self.map_frame
@@ -483,16 +542,6 @@ class TopologyPurePursuit(Node):
         vx = msg.twist.twist.linear.x
         wz = msg.twist.twist.angular.z
 
-        stamp = Time.from_msg(msg.header.stamp)
-        if msg.header.stamp.sec == 0 and msg.header.stamp.nanosec == 0:
-            age = 0.0
-        else:
-            age = (self.get_clock().now() - stamp).nanoseconds * 1e-9
-        if age < 0.0:
-            age = 0.0
-        if age > self.max_odom_age:
-            return None
-
         if compensate and self.latency_compensation:
             horizon = min(age, self.max_latency_compensation)
             if abs(wz) < 1e-4:
@@ -587,28 +636,53 @@ class TopologyPurePursuit(Node):
             )
             return
 
-        nearest_index = self._nearest_path_index(x, y)
+        projection = self._nearest_path_projection(x, y)
+        nearest_index = projection["index"]
         self.current_path_index = nearest_index
-        target_index = self._lookahead_index(x, y, nearest_index)
-        target_x, target_y, target_path_yaw = self.active_path[target_index]
-        nearest_x, nearest_y, _ = self.active_path[nearest_index]
-        cross_track_error = math.hypot(nearest_x - x, nearest_y - y)
+        cross_track_signed = projection["signed_error"]
+        cross_track_error = abs(cross_track_signed)
+        path_curvature = self._path_curvature(nearest_index)
+        lookahead_threshold = self._adaptive_lookahead(
+            path_curvature, cross_track_error, goal_distance
+        )
+        target = self._path_target_from_projection(projection, lookahead_threshold)
+        target_index = target["index"]
+        target_x = target["x"]
+        target_y = target["y"]
+        target_path_yaw = target["yaw"]
 
         dx = target_x - x
         dy = target_y - y
-        path_heading = self._segment_heading(target_index, x, y)
+        path_heading = target["path_heading"]
         reverse = math.cos(normalize_angle(target_path_yaw - path_heading)) < 0.0
         control_yaw = normalize_angle(yaw + math.pi) if reverse else yaw
+        # Track the XY curve.  The path pose yaw is still used to detect reverse
+        # segments, but heading correction should follow the segment tangent so
+        # a noisy or intentionally ignored goal yaw does not pull the vehicle
+        # away from the planned line.
+        heading_error = normalize_angle(path_heading - control_yaw)
         local_x = math.cos(control_yaw) * dx + math.sin(control_yaw) * dy
         local_y = -math.sin(control_yaw) * dx + math.cos(control_yaw) * dy
         lookahead = max(0.05, math.hypot(local_x, local_y))
 
         curvature = 2.0 * local_y / (lookahead * lookahead)
+        curvature += (
+            self.heading_correction_gain
+            * math.sin(heading_error)
+            / max(self.min_lookahead_distance, lookahead)
+        )
+        curvature += (
+            -self.cross_track_correction_gain
+            * cross_track_signed
+            / max(self.min_lookahead_distance * self.min_lookahead_distance, lookahead * lookahead)
+        )
         if self.min_turning_radius > 1e-3:
             max_curvature = 1.0 / self.min_turning_radius
             curvature = max(-max_curvature, min(max_curvature, curvature))
 
-        speed = self._target_speed(abs(curvature), goal_distance, reverse)
+        speed = self._target_speed(
+            abs(curvature), goal_distance, reverse, cross_track_error
+        )
         angular = speed * curvature
         angular = max(-self.max_angular_speed, min(self.max_angular_speed, angular))
         self._publish_cmd(speed, angular)
@@ -620,9 +694,13 @@ class TopologyPurePursuit(Node):
             nearest_index=nearest_index,
             target_index=target_index,
             cross_track_error=cross_track_error,
+            cross_track_signed=cross_track_signed,
             reverse=reverse,
             curvature=curvature,
             lookahead=lookahead,
+            lookahead_threshold=lookahead_threshold,
+            path_curvature=path_curvature,
+            heading_error=heading_error,
         )
 
     def _align_final_yaw(self, x, y, yaw, goal_x, goal_y, goal_yaw, goal_yaw_error):
@@ -670,15 +748,177 @@ class TopologyPurePursuit(Node):
                 best_dist = dist
         return best_index
 
-    def _lookahead_index(self, x, y, start_index):
+    def _nearest_path_projection(self, x, y):
+        if len(self.active_path) < 2:
+            px, py, pyaw = self.active_path[0]
+            return {
+                "index": 0,
+                "t": 0.0,
+                "x": px,
+                "y": py,
+                "yaw": pyaw,
+                "path_heading": pyaw,
+                "signed_error": 0.0,
+                "distance": math.hypot(x - px, y - py),
+            }
+
+        if self.path_reacquire_distance > 0.0:
+            nearest_vertex = self._nearest_path_index(x, y)
+            vx, vy, _ = self.active_path[nearest_vertex]
+            vertex_error = math.hypot(vx - x, vy - y)
+            if vertex_error > self.path_reacquire_distance:
+                start = 0
+            else:
+                start = max(0, self.current_path_index - 8)
+        else:
+            start = max(0, self.current_path_index - 8)
+
+        best = None
+        best_dist_sq = float("inf")
+        for index in range(start, len(self.active_path) - 1):
+            x0, y0, yaw0 = self.active_path[index]
+            x1, y1, yaw1 = self.active_path[index + 1]
+            dx = x1 - x0
+            dy = y1 - y0
+            seg_len_sq = dx * dx + dy * dy
+            if seg_len_sq <= 1e-10:
+                continue
+            t = ((x - x0) * dx + (y - y0) * dy) / seg_len_sq
+            t = max(0.0, min(1.0, t))
+            px = x0 + t * dx
+            py = y0 + t * dy
+            err_x = x - px
+            err_y = y - py
+            dist_sq = err_x * err_x + err_y * err_y
+            if dist_sq >= best_dist_sq:
+                continue
+            seg_len = math.sqrt(seg_len_sq)
+            ux = dx / seg_len
+            uy = dy / seg_len
+            signed_error = ux * err_y - uy * err_x
+            heading = math.atan2(dy, dx)
+            yaw = yaw0 if t < 0.5 else yaw1
+            best = {
+                "index": index,
+                "t": t,
+                "x": px,
+                "y": py,
+                "yaw": yaw,
+                "path_heading": heading,
+                "signed_error": signed_error,
+                "distance": math.sqrt(dist_sq),
+            }
+            best_dist_sq = dist_sq
+
+        if best is not None:
+            return best
+
+        index = min(max(0, self.current_path_index), len(self.active_path) - 1)
+        px, py, pyaw = self.active_path[index]
+        return {
+            "index": index,
+            "t": 0.0,
+            "x": px,
+            "y": py,
+            "yaw": pyaw,
+            "path_heading": pyaw,
+            "signed_error": 0.0,
+            "distance": math.hypot(x - px, y - py),
+        }
+
+    def _path_target_from_projection(self, projection, lookahead_distance):
+        remaining = max(0.05, float(lookahead_distance))
+        index = min(projection["index"], len(self.active_path) - 2)
+        t = max(0.0, min(1.0, projection.get("t", 0.0)))
+
+        while index < len(self.active_path) - 1:
+            x0, y0, yaw0 = self.active_path[index]
+            x1, y1, yaw1 = self.active_path[index + 1]
+            dx = x1 - x0
+            dy = y1 - y0
+            seg_len = math.hypot(dx, dy)
+            if seg_len <= 1e-9:
+                index += 1
+                t = 0.0
+                continue
+            available = (1.0 - t) * seg_len
+            if remaining <= available:
+                ratio = t + remaining / seg_len
+                x = x0 + ratio * dx
+                y = y0 + ratio * dy
+                heading = math.atan2(dy, dx)
+                yaw = yaw0 if ratio < 0.5 else yaw1
+                return {
+                    "index": index + 1,
+                    "x": x,
+                    "y": y,
+                    "yaw": yaw,
+                    "path_heading": heading,
+                }
+            remaining -= available
+            index += 1
+            t = 0.0
+
+        x, y, yaw = self.active_path[-1]
+        heading = self._segment_heading(len(self.active_path) - 1)
+        return {
+            "index": len(self.active_path) - 1,
+            "x": x,
+            "y": y,
+            "yaw": yaw,
+            "path_heading": heading,
+        }
+
+    def _lookahead_index(self, x, y, start_index, lookahead_distance=None):
+        threshold = (
+            self.lookahead_distance
+            if lookahead_distance is None
+            else max(0.05, float(lookahead_distance))
+        )
         target_index = len(self.active_path) - 1
         for index in range(start_index, len(self.active_path)):
             px, py, _ = self.active_path[index]
-            if math.hypot(px - x, py - y) >= self.lookahead_distance:
+            if math.hypot(px - x, py - y) >= threshold:
                 target_index = index
                 break
         return target_index
 
+    def _adaptive_lookahead(self, path_curvature, cross_track_error, goal_distance):
+        scale = (
+            1.0
+            + self.lookahead_curvature_gain * abs(path_curvature)
+            + self.lookahead_error_gain * max(0.0, cross_track_error)
+        )
+        lookahead = self.lookahead_distance / max(1.0, scale)
+        if goal_distance < self.lookahead_distance:
+            lookahead = min(lookahead, max(self.min_lookahead_distance, goal_distance))
+        return max(self.min_lookahead_distance, min(self.lookahead_distance, lookahead))
+
+    def _path_curvature(self, index):
+        if len(self.active_path) < 3:
+            return 0.0
+        i0 = max(0, index - 1)
+        i1 = max(0, min(index, len(self.active_path) - 1))
+        i2 = min(len(self.active_path) - 1, index + 1)
+        if i0 == i1:
+            i2 = min(len(self.active_path) - 1, i1 + 2)
+        if i1 == i2:
+            i0 = max(0, i1 - 2)
+        if i0 == i1 or i1 == i2:
+            return 0.0
+
+        x0, y0, _ = self.active_path[i0]
+        x1, y1, _ = self.active_path[i1]
+        x2, y2, _ = self.active_path[i2]
+        a = math.hypot(x1 - x0, y1 - y0)
+        b = math.hypot(x2 - x1, y2 - y1)
+        c = math.hypot(x2 - x0, y2 - y0)
+        denominator = a * b * c
+        if denominator <= 1e-6:
+            return 0.0
+        cross = (x1 - x0) * (y2 - y0) - (y1 - y0) * (x2 - x0)
+        return 2.0 * cross / denominator
+
     def _segment_heading(self, index, current_x=None, current_y=None):
         if len(self.active_path) < 2:
             return 0.0
@@ -716,9 +956,13 @@ class TopologyPurePursuit(Node):
         nearest_index=None,
         target_index=None,
         cross_track_error=None,
+        cross_track_signed=None,
         reverse=False,
         curvature=None,
         lookahead=None,
+        lookahead_threshold=None,
+        path_curvature=None,
+        heading_error=None,
     ):
         if self.state_log_period <= 0.0:
             return
@@ -731,10 +975,34 @@ class TopologyPurePursuit(Node):
         parts = [f"tracker_state mode={mode}"]
         if pose is None:
             parts.append("pose=unavailable")
+            if self.latest_odom is None:
+                parts.append("odom=none")
+            elif self.latest_odom_received_ns > 0:
+                receive_age = max(
+                    0.0,
+                    (now_ns - self.latest_odom_received_ns) * 1e-9,
+                )
+                parts.append(f"odom_receive_age={receive_age:.3f}")
         else:
             x, y, yaw, vx, wz = pose
             parts.append(f"pose=({x:.3f},{y:.3f},{yaw:.3f})")
             parts.append(f"odom_twist=({vx:.3f},{wz:.3f})")
+            if self.latest_odom_received_ns > 0:
+                receive_age = max(
+                    0.0,
+                    (now_ns - self.latest_odom_received_ns) * 1e-9,
+                )
+                parts.append(f"odom_receive_age={receive_age:.3f}")
+        if self.latest_odom is not None:
+            stamp = Time.from_msg(self.latest_odom.header.stamp)
+            if (
+                self.latest_odom.header.stamp.sec != 0
+                or self.latest_odom.header.stamp.nanosec != 0
+            ):
+                stamp_age = max(
+                    0.0, (self.get_clock().now() - stamp).nanoseconds * 1e-9
+                )
+                parts.append(f"odom_stamp_age={stamp_age:.3f}")
 
         parts.append(f"path_points={len(self.active_path)}")
         parts.append(f"completed={self.completed}")
@@ -748,21 +1016,41 @@ class TopologyPurePursuit(Node):
             parts.append(f"target_index={target_index}")
         if cross_track_error is not None:
             parts.append(f"cross_track={cross_track_error:.3f}")
+        if cross_track_signed is not None:
+            parts.append(f"cross_track_signed={cross_track_signed:.3f}")
         if curvature is not None:
             parts.append(f"curvature={curvature:.3f}")
         if lookahead is not None:
             parts.append(f"lookahead={lookahead:.3f}")
+        if lookahead_threshold is not None:
+            parts.append(f"lookahead_threshold={lookahead_threshold:.3f}")
+        if path_curvature is not None:
+            parts.append(f"path_curvature={path_curvature:.3f}")
+        if heading_error is not None:
+            parts.append(f"heading_error={heading_error:.3f}")
         parts.append(f"reverse={reverse}")
         parts.append(f"cmd=({self.last_cmd_linear:.3f},{self.last_cmd_angular:.3f})")
         self.get_logger().info(" ".join(parts))
 
-    def _target_speed(self, abs_curvature, goal_distance, reverse):
+    def _target_speed(self, abs_curvature, goal_distance, reverse, cross_track_error=0.0):
         base_speed = self.reverse_speed if reverse else self.linear_speed
         speed = base_speed
-        speed *= max(0.35, 1.0 - self.curvature_slowdown_gain * abs_curvature)
+        speed *= max(
+            self.min_curvature_speed_ratio,
+            1.0 - self.curvature_slowdown_gain * abs_curvature,
+        )
         if goal_distance < self.slowdown_distance:
-            ratio = max(0.25, goal_distance / max(0.05, self.slowdown_distance))
+            ratio = max(
+                self.min_goal_slowdown_ratio,
+                goal_distance / max(0.05, self.slowdown_distance),
+            )
             speed *= ratio
+        if cross_track_error > 0.0 and self.cross_track_speed_gain > 0.0:
+            tracking_ratio = max(
+                self.min_tracking_speed_ratio,
+                1.0 - self.cross_track_speed_gain * cross_track_error,
+            )
+            speed *= tracking_ratio
         speed = max(self.min_linear_speed, min(base_speed, speed))
         return -speed if reverse else speed
 
diff --git a/src/vlm_detect/config/vlm_detect.yaml b/src/vlm_detect/config/vlm_detect.yaml
index 8c6674b..ebce0bf 100644
--- a/src/vlm_detect/config/vlm_detect.yaml
+++ b/src/vlm_detect/config/vlm_detect.yaml
@@ -1,36 +1,18 @@
 # vlm_detect 参数配置
-# 使用: ros2 launch vlm_detect vlm_detect.launch.py
 
 vlm_node:
   ros__parameters:
-    # VLM 推理服务地址
     vlm_host: "http://192.168.10.189:8000"
-    # 模型名称 (OpenAI 格式)
     vlm_model: "./OpenGVLab/InternVL3-1B/"
-    # 订阅的压缩图像话题
     image_topic: "/image_mjpeg"
-    # 订阅的触发信号话题
     trigger_topic: "/sign4return"
-    # 触发信号值
     trigger_sign: 9
-    # 发布结果的话题
     result_topic: "/vlm_result"
-    # 发送给 VLM 的提示词
     prompt_text: "描述图片中有一个病人的特征，字数控制在20字以内。"
-    # 最大输出 token 数
     max_tokens: 100
 
 tts_node:
   ros__parameters:
-    # VLM 推理服务地址 (需与 vlm_node 一致)
-    vlm_host: "http://192.168.10.189:8000"
-    # 音频输出设备 (PulseAudio sink)
     audio_sink: "alsa_output.usb-C-Media_Electronics_Inc._USB_Audio_Device-00.analog-stereo"
-    # 订阅 VLM 结果的话题 (需与 vlm_node 一致)
     result_topic: "/vlm_result"
-    # TTS 语音 (edge-tts 语音名)
-    tts_voice: "zh-CN-XiaoxiaoNeural"
-    # 临时 MP3 存储路径
-    tmp_mp3_path: "/tmp/tts_out.mp3"
-    # 播放速度 (ffplay atempo, 范围 0.5~2.0)
     tts_speed: 1.5
diff --git a/src/vlm_detect/launch/vlm_detect.launch.py b/src/vlm_detect/launch/vlm_detect.launch.py
index b45783e..dd416d5 100644
--- a/src/vlm_detect/launch/vlm_detect.launch.py
+++ b/src/vlm_detect/launch/vlm_detect.launch.py
@@ -1,13 +1,15 @@
 #!/usr/bin/env python3
 # -*- coding: utf-8 -*-
 """
-vlm_detect 联合启动文件
-同时启动 vlm_node (图生文) 和 tts_node (语音播报)
+vlm_detect 启动文件
+同时启动 vlm_node (图生文) + tts_server (语音播报服务) + qr_tts_bridge (二维码播报)
 
 用法:
-  ros2 launch vlm_detect vlm_detect.launch.py                          # 默认配置
-  ros2 launch vlm_detect vlm_detect.launch.py vlm_host:=http://...     # 覆盖 VLM 服务地址
-  ros2 launch vlm_detect vlm_detect.launch.py use_tts:=false           # 只启动 vlm_node
+  ros2 launch vlm_detect vlm_detect.launch.py                          # 默认全部
+  ros2 launch vlm_detect vlm_detect.launch.py vlm_host:=http://...     # 指定 VLM 服务地址
+  ros2 launch vlm_detect vlm_detect.launch.py use_tts:=false           # 关闭语音播报
+  ros2 launch vlm_detect vlm_detect.launch.py use_qr_tts:=false        # 关闭二维码播报桥接
+  ros2 launch vlm_detect vlm_detect.launch.py use_vlm:=false           # 只启动语音服务
 """
 
 import os
@@ -23,7 +25,9 @@ from launch_ros.actions import Node
 def generate_launch_description():
 
     # ==================== Launch 参数 ====================
+    use_vlm = LaunchConfiguration('use_vlm')
     use_tts = LaunchConfiguration('use_tts')
+    use_qr_tts = LaunchConfiguration('use_qr_tts')
 
     config_file = LaunchConfiguration('config_file')
 
@@ -37,15 +41,22 @@ def generate_launch_description():
     prompt_text = LaunchConfiguration('prompt_text')
     max_tokens = LaunchConfiguration('max_tokens')
 
-    # tts_node 可覆盖参数
+    # tts_server 可覆盖参数
     audio_sink = LaunchConfiguration('audio_sink')
-    tts_voice = LaunchConfiguration('tts_voice')
     tts_speed = LaunchConfiguration('tts_speed')
 
-    # ==================== 声明参数 ====================
+    # ==================== 参数声明 ====================
+    declare_use_vlm = DeclareLaunchArgument(
+        'use_vlm', default_value='true',
+        description='启动 VLM 图生文节点')
+
     declare_use_tts = DeclareLaunchArgument(
         'use_tts', default_value='true',
-        description='是否同时启动 TTS 语音播报节点')
+        description='启动 TTS 语音播报服务')
+
+    declare_use_qr_tts = DeclareLaunchArgument(
+        'use_qr_tts', default_value='true',
+        description='启动二维码 → TTS 桥接节点')
 
     declare_config_file = DeclareLaunchArgument(
         'config_file',
@@ -57,47 +68,46 @@ def generate_launch_description():
     # vlm_node 参数
     declare_vlm_host = DeclareLaunchArgument(
         'vlm_host', default_value='http://192.168.10.189:8000',
-        description='VLM 推理服务地址')
+        description='VLM 服务器地址')
     declare_vlm_model = DeclareLaunchArgument(
         'vlm_model', default_value='./OpenGVLab/InternVL3-1B/',
         description='VLM 模型名称')
     declare_image_topic = DeclareLaunchArgument(
         'image_topic', default_value='/image_mjpeg',
-        description='订阅的压缩图像话题')
+        description='输入的压缩图像话题')
     declare_trigger_topic = DeclareLaunchArgument(
         'trigger_topic', default_value='/sign4return',
-        description='订阅的触发信号话题')
+        description='输入的触发信号话题')
     declare_trigger_sign = DeclareLaunchArgument(
         'trigger_sign', default_value='9',
         description='触发信号值 (Int32)')
     declare_result_topic = DeclareLaunchArgument(
         'result_topic', default_value='/vlm_result',
-        description='发布 VLM 结果的话题')
+        description='输出 VLM 结果的话题')
     declare_prompt_text = DeclareLaunchArgument(
-        'prompt_text', default_value='描述图片中有一个病人的特征，字数控制在20字以内。',
+        'prompt_text', default_value='请描述这张图片的内容，用一句简短的话概括，不超过20个字。',
         description='发送给 VLM 的提示词')
     declare_max_tokens = DeclareLaunchArgument(
         'max_tokens', default_value='100',
-        description='最大输出 token 数')
+        description='最大生成 token 数')
 
-    # tts_node 参数
+    # tts_server 参数
     declare_audio_sink = DeclareLaunchArgument(
         'audio_sink',
         default_value='alsa_output.usb-C-Media_Electronics_Inc._USB_Audio_Device-00.analog-stereo',
         description='音频输出设备 (PulseAudio sink)')
-    declare_tts_voice = DeclareLaunchArgument(
-        'tts_voice', default_value='zh-CN-XiaoxiaoNeural',
-        description='TTS 语音名称 (edge-tts)')
     declare_tts_speed = DeclareLaunchArgument(
         'tts_speed', default_value='1.5',
-        description='播放速度倍率 (0.5~2.0)')
+        description='语速倍率 (0.5~2.0)')
 
     # ==================== 节点 ====================
+    # VLM 图生文节点 (内部自带 TTS 服务客户端)
     vlm_node = Node(
         package='vlm_detect',
         executable='vlm_node',
         name='vlm_detect',
         output='screen',
+        condition=IfCondition(use_vlm),
         parameters=[config_file,
             {
                 'vlm_host': vlm_host,
@@ -111,26 +121,35 @@ def generate_launch_description():
             }],
     )
 
-    tts_node = Node(
+    # TTS 语音播报服务端
+    tts_server = Node(
         package='vlm_detect',
-        executable='tts_node',
-        name='tts_node',
+        executable='tts_server',
+        name='tts_server',
         output='screen',
         condition=IfCondition(use_tts),
         parameters=[config_file,
             {
-                'vlm_host': vlm_host,
                 'audio_sink': audio_sink,
-                'result_topic': result_topic,
-                'tts_voice': tts_voice,
                 'tts_speed': tts_speed,
             }],
     )
 
-    # ==================== 启动描述 ====================
+    # 二维码 → TTS 桥接 (订阅 qr_results，调用 /tts/speak)
+    qr_tts_bridge = Node(
+        package='vlm_detect',
+        executable='qr_tts_bridge',
+        name='qr_tts_bridge',
+        output='screen',
+        condition=IfCondition(use_qr_tts),
+    )
+
+    # ==================== 组装 ====================
     return LaunchDescription([
         # 参数声明
+        declare_use_vlm,
         declare_use_tts,
+        declare_use_qr_tts,
         declare_config_file,
         declare_vlm_host,
         declare_vlm_model,
@@ -141,12 +160,13 @@ def generate_launch_description():
         declare_prompt_text,
         declare_max_tokens,
         declare_audio_sink,
-        declare_tts_voice,
         declare_tts_speed,
         # 节点
         LogInfo(msg=['配置文件: ', config_file]),
         LogInfo(msg=['VLM 服务: ', vlm_host]),
-        LogInfo(msg=['TTS 播报: ', use_tts]),
+        LogInfo(msg=['TTS 服务: ', use_tts]),
+        LogInfo(msg=['QR-TTS 桥接: ', use_qr_tts]),
         vlm_node,
-        tts_node,
+        tts_server,
+        qr_tts_bridge,
     ])
diff --git a/src/vlm_detect/setup.py b/src/vlm_detect/setup.py
index 8c070ef..b4b075d 100644
--- a/src/vlm_detect/setup.py
+++ b/src/vlm_detect/setup.py
@@ -27,6 +27,8 @@ setup(
             'vlm_node = vlm_detect.vlm_node:main',
             'test_publisher = vlm_detect.test_publisher:main',
             'tts_node = vlm_detect.tts_node:main',
+            'tts_server = vlm_detect.tts_server:main',
+            'qr_tts_bridge = vlm_detect.qr_tts_bridge:main',
         ],
     },
 )
diff --git a/src/vlm_detect/vlm_detect/__pycache__/test_publisher.cpython-310.pyc b/src/vlm_detect/vlm_detect/__pycache__/test_publisher.cpython-310.pyc
index 49408f7..8953928 100644
Binary files a/src/vlm_detect/vlm_detect/__pycache__/test_publisher.cpython-310.pyc and b/src/vlm_detect/vlm_detect/__pycache__/test_publisher.cpython-310.pyc differ
diff --git a/src/vlm_detect/vlm_detect/__pycache__/tts_node.cpython-310.pyc b/src/vlm_detect/vlm_detect/__pycache__/tts_node.cpython-310.pyc
index 29784a4..75c715c 100644
Binary files a/src/vlm_detect/vlm_detect/__pycache__/tts_node.cpython-310.pyc and b/src/vlm_detect/vlm_detect/__pycache__/tts_node.cpython-310.pyc differ
diff --git a/src/vlm_detect/vlm_detect/__pycache__/vlm_node.cpython-310.pyc b/src/vlm_detect/vlm_detect/__pycache__/vlm_node.cpython-310.pyc
index 7c0e001..4e61913 100644
Binary files a/src/vlm_detect/vlm_detect/__pycache__/vlm_node.cpython-310.pyc and b/src/vlm_detect/vlm_detect/__pycache__/vlm_node.cpython-310.pyc differ
diff --git a/src/vlm_detect/vlm_detect/qr_tts_bridge.py b/src/vlm_detect/vlm_detect/qr_tts_bridge.py
new file mode 100644
index 0000000..fe95b02
--- /dev/null
+++ b/src/vlm_detect/vlm_detect/qr_tts_bridge.py
@@ -0,0 +1,66 @@
+#!/usr/bin/env python3
+"""
+QR 识别 → TTS 语音播报桥接节点
+订阅 qr_results，调用 /tts/speak 服务
+"""
+import rclpy
+from rclpy.node import Node
+from std_msgs.msg import String
+from origincar_msg.srv import Speak
+
+
+class QrTtsBridge(Node):
+    def __init__(self):
+        super().__init__('qr_tts_bridge')
+
+        # TTS 服务客户端
+        self.tts_client = self.create_client(Speak, '/tts/speak')
+        while not self.tts_client.wait_for_service(timeout_sec=5.0):
+            self.get_logger().info('Waiting for TTS service...')
+        self.get_logger().info('TTS service connected')
+
+        # 订阅 QR 识别结果
+        self.sub = self.create_subscription(
+            String, 'qr_results', self.callback, 10)
+
+        self.get_logger().info('QR-TTS Bridge ready, listening on qr_results')
+
+    def callback(self, msg):
+        text = msg.data.strip()
+        if not text:
+            return
+
+        self.get_logger().info(f'QR result: {text}')
+
+        if not self.tts_client.service_is_ready():
+            self.get_logger().warning('TTS service not available')
+            return
+
+        req = Speak.Request()
+        req.text = text
+        future = self.tts_client.call_async(req)
+        future.add_done_callback(self._tts_done_callback)
+
+    def _tts_done_callback(self, future):
+        try:
+            resp = future.result()
+            if not resp.success:
+                self.get_logger().warning(f'TTS failed: {resp.message}')
+        except Exception as e:
+            self.get_logger().error(f'TTS call error: {e}')
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = QrTtsBridge()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()
diff --git a/src/vlm_detect/vlm_detect/test_publisher.py b/src/vlm_detect/vlm_detect/test_publisher.py
index d0daf89..a29a8c1 100644
--- a/src/vlm_detect/vlm_detect/test_publisher.py
+++ b/src/vlm_detect/vlm_detect/test_publisher.py
@@ -2,7 +2,7 @@
 # -*- coding: utf-8 -*-
 """
 测试发布者：发送图片和触发信号给 VLM 节点
-用法: ros2 run vlm_detect test_publisher --ros-args -p image_path:="/path/to/image.jpg"
+用法: ros2 run vlm_detect test_publisher --ros-args -p image_path:="/home/sunrise/yiliao_ws/my_model/image.png"
 """
 
 import rclpy
diff --git a/src/vlm_detect/vlm_detect/tts_node.py b/src/vlm_detect/vlm_detect/tts_node.py
index c847a1c..2510269 100644
--- a/src/vlm_detect/vlm_detect/tts_node.py
+++ b/src/vlm_detect/vlm_detect/tts_node.py
@@ -1,60 +1,50 @@
 #!/usr/bin/env python3
-# -*- coding: utf-8 -*-
-import rclpy, subprocess, requests, os
+import rclpy, subprocess, os, wave
 from rclpy.node import Node
 from std_msgs.msg import String
+from piper import PiperVoice
+from piper.config import SynthesisConfig
+
+MODEL_PATH = '/home/sunrise/tts_model/zh_CN-huayan-medium.onnx'
 
 class TTSNode(Node):
     def __init__(self):
-        super().__init__("tts_node")
-
-        self.declare_parameter('vlm_host', 'http://192.168.10.189:8000')
-        self.declare_parameter('audio_sink',
-            'alsa_output.usb-C-Media_Electronics_Inc._USB_Audio_Device-00.analog-stereo')
+        super().__init__('tts_node')
+        self.declare_parameter('audio_sink', 'alsa_output.usb-C-Media_Electronics_Inc._USB_Audio_Device-00.analog-stereo')
         self.declare_parameter('result_topic', '/vlm_result')
-        self.declare_parameter('tts_voice', 'zh-CN-XiaoxiaoNeural')
-        self.declare_parameter('tmp_mp3_path', '/tmp/tts_out.mp3')
         self.declare_parameter('tts_speed', 1.5)
 
-        self.vlm_host = self.get_parameter('vlm_host').value
-        audio_sink = self.get_parameter('audio_sink').value
+        self.audio_sink = self.get_parameter('audio_sink').value
         result_topic = self.get_parameter('result_topic').value
-        self.tts_voice = self.get_parameter('tts_voice').value
-        self.tmp_mp3 = self.get_parameter('tmp_mp3_path').value
-        self.tts_speed = self.get_parameter('tts_speed').value
-
-        self.audio_env = {**os.environ, "PULSE_SINK": audio_sink}
+        tts_speed = self.get_parameter('tts_speed').value
+        self.length_scale = 1.0 / tts_speed
+        self.espeak_speed = int(175 * tts_speed)  # espeak default=175wpm, scale with tts_speed
 
         self.sub = self.create_subscription(String, result_topic, self.callback, 10)
-        self.get_logger().info(
-            f"TTS 节点启动 | host={self.vlm_host} | sink={audio_sink} | "
-            f"voice={self.tts_voice} | speed={self.tts_speed}x"
-        )
+        self.get_logger().info(f'Piper TTS started | voice=zh_CN-huayan | speed={tts_speed}x | sink={self.audio_sink}')
+        self.get_logger().info(f'Loading model: {MODEL_PATH}')
+        self.voice = PiperVoice.load(MODEL_PATH)
+        self.get_logger().info('Model loaded OK')
 
     def callback(self, msg):
-        text = msg.data
-        self.get_logger().info(f"语音播报: {text}")
+        text = msg.data.strip()
+        if not text:
+            return
+        self.get_logger().info(f'TTS: {text}')
         try:
-            resp = requests.post(
-                f"{self.vlm_host}/v1/tts",
-                json={"text": text, "voice": self.tts_voice},
-                timeout=60
-            )
-            resp.raise_for_status()
-            with open(self.tmp_mp3, "wb") as f:
-                f.write(resp.content)
-            speed_str = f"atempo={self.tts_speed}"
+            syn_config = SynthesisConfig(length_scale=self.length_scale)
+            wav_path = '/tmp/tts_out.wav'
+            with wave.open(wav_path, 'wb') as wf:
+                self.voice.synthesize_wav(text, wf, syn_config=syn_config)
             subprocess.Popen(
-                ["ffplay", "-nodisp", "-autoexit", "-af", speed_str, self.tmp_mp3],
-                stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL,
-                env=self.audio_env
-            )
+                ['paplay', f'--device={self.audio_sink}', wav_path],
+                stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL)
         except Exception as e:
-            self.get_logger().error(f"TTS 失败, 降级 espeak: {e}")
+            self.get_logger().error(f'Piper TTS failed, fallback espeak: {e}')
             subprocess.Popen(
-                ["espeak-ng", "-v", "zh", "-s", "150", text],
-                env=self.audio_env
-            )
+                ['espeak-ng', '-v', 'cmn', '-s', str(self.espeak_speed), text],
+                env={**os.environ, 'PULSE_SINK': self.audio_sink},
+                stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL)
 
 def main(args=None):
     rclpy.init(args=args)
@@ -67,5 +57,5 @@ def main(args=None):
         node.destroy_node()
         rclpy.shutdown()
 
-if __name__ == "__main__":
+if __name__ == '__main__':
     main()
diff --git a/src/vlm_detect/vlm_detect/tts_server.py b/src/vlm_detect/vlm_detect/tts_server.py
new file mode 100644
index 0000000..459e95f
--- /dev/null
+++ b/src/vlm_detect/vlm_detect/tts_server.py
@@ -0,0 +1,90 @@
+#!/usr/bin/env python3
+"""
+TTS 语音播报服务端 (ROS2 Service Server)
+服务类型: origincar_msg/srv/Speak
+主 TTS: Piper 离线模型  降级: espeak-ng
+"""
+import rclpy
+import subprocess
+import os
+import wave
+
+from rclpy.node import Node
+from origincar_msg.srv import Speak
+
+from piper import PiperVoice
+from piper.config import SynthesisConfig
+
+MODEL_PATH = '/home/sunrise/tts_model/zh_CN-huayan-medium.onnx'
+
+
+class TTSServer(Node):
+    def __init__(self):
+        super().__init__('tts_server')
+
+        self.declare_parameter('audio_sink',
+                               'alsa_output.usb-C-Media_Electronics_Inc._USB_Audio_Device-00.analog-stereo')
+        self.declare_parameter('tts_speed', 1.5)
+
+        self.audio_sink = self.get_parameter('audio_sink').value
+        tts_speed = self.get_parameter('tts_speed').value
+        self.length_scale = 1.0 / tts_speed
+        self.espeak_speed = int(175 * tts_speed)
+
+        self.srv = self.create_service(Speak, '/tts/speak', self.handle_speak)
+
+        self.get_logger().info(f'TTS Server ready | voice=zh_CN-huayan | speed={tts_speed}x | sink={self.audio_sink}')
+        self.get_logger().info(f'Loading model: {MODEL_PATH}')
+        self.voice = PiperVoice.load(MODEL_PATH)
+        self.get_logger().info('Model loaded OK')
+
+    def handle_speak(self, request, response):
+        text = request.text.strip()
+        if not text:
+            response.success = False
+            response.message = 'empty text'
+            return response
+
+        self.get_logger().info(f'TTS: {text}')
+
+        try:
+            syn_config = SynthesisConfig(length_scale=self.length_scale)
+            wav_path = '/tmp/tts_out.wav'
+            with wave.open(wav_path, 'wb') as wf:
+                self.voice.synthesize_wav(text, wf, syn_config=syn_config)
+            subprocess.Popen(
+                ['paplay', f'--device={self.audio_sink}', wav_path],
+                stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL)
+            response.success = True
+            response.message = 'ok'
+        except Exception as e:
+            self.get_logger().error(f'Piper TTS failed, fallback espeak: {e}')
+            try:
+                subprocess.Popen(
+                    ['espeak-ng', '-v', 'cmn', '-s', str(self.espeak_speed), text],
+                    env={**os.environ, 'PULSE_SINK': self.audio_sink},
+                    stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL)
+                response.success = True
+                response.message = 'ok (espeak fallback)'
+            except Exception as e2:
+                self.get_logger().error(f'espeak also failed: {e2}')
+                response.success = False
+                response.message = str(e2)
+
+        return response
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = TTSServer()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()
diff --git a/src/vlm_detect/vlm_detect/vlm_node.py b/src/vlm_detect/vlm_detect/vlm_node.py
index 02ca5fb..b9c3eb5 100644
--- a/src/vlm_detect/vlm_detect/vlm_node.py
+++ b/src/vlm_detect/vlm_detect/vlm_node.py
@@ -1,5 +1,8 @@
 #!/usr/bin/env python3
 # -*- coding: utf-8 -*-
+"""
+VLM 图生文节点 —— 收到触发信号后拍图发给 VLM 服务，结果调用 TTS 服务播报
+"""
 import rclpy
 from rclpy.node import Node
 from std_msgs.msg import Int32, String
@@ -13,6 +16,9 @@ import os
 import time
 import numpy as np
 
+from origincar_msg.srv import Speak
+
+
 class VLMProcessor(Node):
     def __init__(self):
         super().__init__('vlm_detect')
@@ -24,7 +30,7 @@ class VLMProcessor(Node):
         self.declare_parameter('trigger_topic', '/sign4return')
         self.declare_parameter('trigger_sign', 9)
         self.declare_parameter('result_topic', '/vlm_result')
-        self.declare_parameter('prompt_text', '描述图片中有一个病人的特征，字数控制在20字以内。')
+        self.declare_parameter('prompt_text', '请描述这张图片的内容，用一句简短的话概括，不超过20个字。')
         self.declare_parameter('max_tokens', 100)
 
         vlm_host = self.get_parameter('vlm_host').value
@@ -43,7 +49,7 @@ class VLMProcessor(Node):
         )
         self.vlm_model = vlm_model
 
-        # ROS2 组件
+        # ROS2 通信
         self.bridge = CvBridge()
         self.latest_image = None
         self.image_lock = threading.Lock()
@@ -56,8 +62,13 @@ class VLMProcessor(Node):
         )
         self.result_pub = self.create_publisher(String, result_topic, 10)
 
+        # TTS 服务客户端
+        self.tts_client = self.create_client(Speak, '/tts/speak')
+        while not self.tts_client.wait_for_service(timeout_sec=5.0):
+            self.get_logger().info('Waiting for TTS service...')
+
         self.get_logger().info(
-            f"VLM Processor 启动 | host={vlm_host} | model={vlm_model} | "
+            f"VLM Processor 就绪 | host={vlm_host} | model={vlm_model} | "
             f"image={image_topic} | trigger={trigger_topic}(sign={self.trigger_sign})"
         )
 
@@ -73,7 +84,7 @@ class VLMProcessor(Node):
 
     def sign_callback(self, msg):
         if msg.data == self.trigger_sign:
-            self.get_logger().info(f"收到触发信号 ({msg.data}), 开始处理...")
+            self.get_logger().info(f"收到触发信号 ({msg.data}), 开始推理...")
             with self.image_lock:
                 if self.latest_image is None:
                     self.get_logger().warning("无可用图片")
@@ -85,12 +96,34 @@ class VLMProcessor(Node):
             try:
                 description = self.process_image(temp_path)
                 self.get_logger().info(f"图像描述: {description}")
+
+                # 发布结果到话题
                 result_msg = String()
                 result_msg.data = description
                 self.result_pub.publish(result_msg)
+
+                # 调用 TTS 服务播报
+                if self.tts_client.service_is_ready():
+                    req = Speak.Request()
+                    req.text = description
+                    future = self.tts_client.call_async(req)
+                    future.add_done_callback(self._tts_done_callback)
+                else:
+                    self.get_logger().warning('TTS service not available')
+
                 os.remove(temp_path)
             except Exception as e:
-                self.get_logger().error(f"图像处理出错: {e}")
+                self.get_logger().error(f"图像推理失败: {e}")
+
+    def _tts_done_callback(self, future):
+        try:
+            resp = future.result()
+            if resp.success:
+                self.get_logger().debug(f'TTS OK: {resp.message}')
+            else:
+                self.get_logger().warning(f'TTS failed: {resp.message}')
+        except Exception as e:
+            self.get_logger().error(f'TTS call error: {e}')
 
     def process_image(self, image_path):
         with open(image_path, "rb") as image_file:
@@ -109,10 +142,12 @@ class VLMProcessor(Node):
                 ]
             }],
             max_tokens=self.max_tokens,
+            timeout=30,
         )
         self.get_logger().info(f"VLM 推理耗时 {time.time() - start_time:.1f}s")
         return response.choices[0].message.content
 
+
 def main(args=None):
     rclpy.init(args=args)
     node = VLMProcessor()
@@ -124,5 +159,6 @@ def main(args=None):
         node.destroy_node()
         rclpy.shutdown()
 
+
 if __name__ == '__main__':
     main()
diff --git a/test_results/goal_reach_debug_20260722_202420.json b/test_results/goal_reach_debug_20260722_202420.json
new file mode 100644
index 0000000..8dda81c
--- /dev/null
+++ b/test_results/goal_reach_debug_20260722_202420.json
@@ -0,0 +1,11585 @@
+{
+  "metadata": {
+    "started_wall_time": 1784723060.4802725,
+    "duration_sec": 30.058978473000025,
+    "output_path": "/home/sunrise/yiliao_ws/test_results/goal_reach_debug_20260722_202420.json"
+  },
+  "topics": {
+    "odom": "/odom",
+    "goal_pose": "/goal_pose",
+    "plan": "/plan",
+    "cmd_vel": "/planner_cmd_vel",
+    "ackermann_cmd": "/ackermann_cmd",
+    "planner_status": "/hybrid_astar_status",
+    "tracker_status": "/pure_pursuit_status"
+  },
+  "topic_endpoints": {
+    "/odom": {
+      "publisher_count": 1,
+      "subscriber_count": 4,
+      "publishers": [
+        {
+          "node_name": "origincar_base",
+          "node_namespace": "/",
+          "topic_type": "nav_msgs/msg/Odometry"
+        }
+      ],
+      "subscribers": [
+        {
+          "node_name": "planner_odom_map_tf",
+          "node_namespace": "/",
+          "topic_type": "nav_msgs/msg/Odometry"
+        },
+        {
+          "node_name": "grid_astar_theta_planner",
+          "node_namespace": "/",
+          "topic_type": "nav_msgs/msg/Odometry"
+        },
+        {
+          "node_name": "topology_pure_pursuit",
+          "node_namespace": "/",
+          "topic_type": "nav_msgs/msg/Odometry"
+        },
+        {
+          "node_name": "goal_reach_debug_collector",
+          "node_namespace": "/",
+          "topic_type": "nav_msgs/msg/Odometry"
+        }
+      ]
+    },
+    "/goal_pose": {
+      "publisher_count": 0,
+      "subscriber_count": 2,
+      "publishers": [],
+      "subscribers": [
+        {
+          "node_name": "grid_astar_theta_planner",
+          "node_namespace": "/",
+          "topic_type": "geometry_msgs/msg/PoseStamped"
+        },
+        {
+          "node_name": "goal_reach_debug_collector",
+          "node_namespace": "/",
+          "topic_type": "geometry_msgs/msg/PoseStamped"
+        }
+      ]
+    },
+    "/plan": {
+      "publisher_count": 1,
+      "subscriber_count": 2,
+      "publishers": [
+        {
+          "node_name": "grid_astar_theta_planner",
+          "node_namespace": "/",
+          "topic_type": "nav_msgs/msg/Path"
+        }
+      ],
+      "subscribers": [
+        {
+          "node_name": "topology_pure_pursuit",
+          "node_namespace": "/",
+          "topic_type": "nav_msgs/msg/Path"
+        },
+        {
+          "node_name": "goal_reach_debug_collector",
+          "node_namespace": "/",
+          "topic_type": "nav_msgs/msg/Path"
+        }
+      ]
+    },
+    "/planner_cmd_vel": {
+      "publisher_count": 1,
+      "subscriber_count": 2,
+      "publishers": [
+        {
+          "node_name": "topology_pure_pursuit",
+          "node_namespace": "/",
+          "topic_type": "geometry_msgs/msg/Twist"
+        }
+      ],
+      "subscribers": [
+        {
+          "node_name": "goal_reach_debug_collector",
+          "node_namespace": "/",
+          "topic_type": "geometry_msgs/msg/Twist"
+        },
+        {
+          "node_name": "planner_ackermann_cmd_bridge",
+          "node_namespace": "/",
+          "topic_type": "geometry_msgs/msg/Twist"
+        }
+      ]
+    },
+    "/ackermann_cmd": {
+      "publisher_count": 1,
+      "subscriber_count": 2,
+      "publishers": [
+        {
+          "node_name": "planner_ackermann_cmd_bridge",
+          "node_namespace": "/",
+          "topic_type": "ackermann_msgs/msg/AckermannDriveStamped"
+        }
+      ],
+      "subscribers": [
+        {
+          "node_name": "goal_reach_debug_collector",
+          "node_namespace": "/",
+          "topic_type": "ackermann_msgs/msg/AckermannDriveStamped"
+        },
+        {
+          "node_name": "origincar_base",
+          "node_namespace": "/",
+          "topic_type": "ackermann_msgs/msg/AckermannDriveStamped"
+        }
+      ]
+    },
+    "/hybrid_astar_status": {
+      "publisher_count": 1,
+      "subscriber_count": 1,
+      "publishers": [
+        {
+          "node_name": "grid_astar_theta_planner",
+          "node_namespace": "/",
+          "topic_type": "std_msgs/msg/String"
+        }
+      ],
+      "subscribers": [
+        {
+          "node_name": "goal_reach_debug_collector",
+          "node_namespace": "/",
+          "topic_type": "std_msgs/msg/String"
+        }
+      ]
+    },
+    "/pure_pursuit_status": {
+      "publisher_count": 1,
+      "subscriber_count": 1,
+      "publishers": [
+        {
+          "node_name": "topology_pure_pursuit",
+          "node_namespace": "/",
+          "topic_type": "std_msgs/msg/String"
+        }
+      ],
+      "subscribers": [
+        {
+          "node_name": "goal_reach_debug_collector",
+          "node_namespace": "/",
+          "topic_type": "std_msgs/msg/String"
+        }
+      ]
+    }
+  },
+  "remote_parameters": {
+    "planner": {
+      "node": "/grid_astar_theta_planner",
+      "service": "/grid_astar_theta_planner/get_parameters",
+      "values": {}
+    },
+    "tracker": {
+      "node": "/topology_pure_pursuit",
+      "service": "/topology_pure_pursuit/get_parameters",
+      "values": {
+        "goal_tolerance": 0.06,
+        "goal_yaw_tolerance": 0.174533,
+        "align_goal_yaw": false,
+        "publish_cmd_vel": true,
+        "cmd_vel_topic": "/planner_cmd_vel",
+        "lookahead_distance": 0.4,
+        "linear_speed": 0.7,
+        "min_linear_speed": 0.08,
+        "reverse_speed": 0.28,
+        "slowdown_distance": 0.35,
+        "curvature_slowdown_gain": 0.03,
+        "min_curvature_speed_ratio": 0.8,
+        "min_goal_slowdown_ratio": 0.35,
+        "min_turning_radius": 0.25,
+        "max_angular_speed": 3.0
+      }
+    }
+  },
+  "latest": {
+    "goal": null,
+    "plan": {
+      "frame_id": "map",
+      "points": 121,
+      "length": 3.5995680777533337,
+      "start": {
+        "x": 0.0008668303489685059,
+        "y": 0.0,
+        "yaw": -2.471993639119318e-06
+      },
+      "end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "reverse_segment_fraction": 0.0
+    },
+    "pose_map": {
+      "available": true,
+      "frame_id": "map",
+      "source_frame": "odom",
+      "x": 2.3958346247673035,
+      "y": 2.506111904978752,
+      "yaw": 1.7182797193527222,
+      "odom_age_sec": 0.041254036,
+      "odom_linear_x": 0.057999998331069946,
+      "odom_angular_z": 0.03450464457273483
+    },
+    "cmd_vel": {
+      "linear_x": 0.242403481345084,
+      "angular_z": -0.969613925380336
+    },
+    "ackermann_cmd": {
+      "speed": 0.24240347743034363,
+      "steering_angle": -0.5195767879486084
+    },
+    "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+    "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)",
+    "last_plan_ok": {
+      "points": 121,
+      "iterations": 43,
+      "planning_ms": 56.888,
+      "goal_error": 0.066,
+      "yaw_error": 0.96
+    },
+    "plan_points": [
+      {
+        "x": 0.0008668303489685059,
+        "y": 0.0,
+        "yaw": -2.471993639119318e-06
+      },
+      {
+        "x": 0.03079488661535851,
+        "y": 0.001797767054574212,
+        "yaw": 0.11999752800636088
+      },
+      {
+        "x": 0.060292504668656544,
+        "y": 0.007165359387125592,
+        "yaw": 0.2399975280063609
+      },
+      {
+        "x": 0.08893542828313294,
+        "y": 0.016025576375551354,
+        "yaw": 0.3599975280063608
+      },
+      {
+        "x": 0.11631169407094452,
+        "y": 0.02825098392629649,
+        "yaw": 0.47999752800636086
+      },
+      {
+        "x": 0.14202755663960828,
+        "y": 0.04366574732429637,
+        "yaw": 0.5999975280063609
+      },
+      {
+        "x": 0.16571315172499784,
+        "y": 0.06204816021590798,
+        "yaw": 0.719997528006361
+      },
+      {
+        "x": 0.18702781584855938,
+        "y": 0.08313383335115498,
+        "yaw": 0.839997528006361
+      },
+      {
+        "x": 0.20566498598756275,
+        "y": 0.10661949722247355,
+        "yaw": 0.9599975280063608
+      },
+      {
+        "x": 0.22287064632077427,
+        "y": 0.1311952017392957,
+        "yaw": 0.9599975280063608
+      },
+      {
+        "x": 0.24007630665398577,
+        "y": 0.1557709062561178,
+        "yaw": 0.9599975280063608
+      },
+      {
+        "x": 0.2572819669871973,
+        "y": 0.18034661077293995,
+        "yaw": 0.9599975280063608
+      },
+      {
+        "x": 0.2744876273204088,
+        "y": 0.20492231528976207,
+        "yaw": 0.9599975280063608
+      },
+      {
+        "x": 0.29169328765362035,
+        "y": 0.2294980198065842,
+        "yaw": 0.9599975280063608
+      },
+      {
+        "x": 0.3088989479868319,
+        "y": 0.25407372432340636,
+        "yaw": 0.9599975280063608
+      },
+      {
+        "x": 0.32610460832004334,
+        "y": 0.2786494288402285,
+        "yaw": 0.9599975280063608
+      },
+      {
+        "x": 0.34331026865325487,
+        "y": 0.3032251333570506,
+        "yaw": 0.9599975280063608
+      },
+      {
+        "x": 0.3605159289864664,
+        "y": 0.3278008378738727,
+        "yaw": 0.9599975280063608
+      },
+      {
+        "x": 0.3777215893196779,
+        "y": 0.3523765423906949,
+        "yaw": 0.9599975280063608
+      },
+      {
+        "x": 0.3949272496528894,
+        "y": 0.376952246907517,
+        "yaw": 0.9599975280063608
+      },
+      {
+        "x": 0.4121329099861009,
+        "y": 0.4015279514243391,
+        "yaw": 0.9599975280063608
+      },
+      {
+        "x": 0.4307700801251043,
+        "y": 0.4250136152956577,
+        "yaw": 0.839997528006361
+      },
+      {
+        "x": 0.45208474424866585,
+        "y": 0.4460992884309047,
+        "yaw": 0.719997528006361
+      },
+      {
+        "x": 0.4757703393340554,
+        "y": 0.4644817013225163,
+        "yaw": 0.5999975280063609
+      },
+      {
+        "x": 0.5014862019027191,
+        "y": 0.47989646472051617,
+        "yaw": 0.47999752800636086
+      },
+      {
+        "x": 0.5280960838314719,
+        "y": 0.4937497742073441,
+        "yaw": 0.47999752800636086
+      },
+      {
+        "x": 0.5547059657602246,
+        "y": 0.5076030836941721,
+        "yaw": 0.47999752800636086
+      },
+      {
+        "x": 0.5813158476889774,
+        "y": 0.521456393181,
+        "yaw": 0.47999752800636086
+      },
+      {
+        "x": 0.6079257296177302,
+        "y": 0.535309702667828,
+        "yaw": 0.47999752800636086
+      },
+      {
+        "x": 0.6336415921863939,
+        "y": 0.5507244660658278,
+        "yaw": 0.5999975280063609
+      },
+      {
+        "x": 0.6573271872717835,
+        "y": 0.5691068789574394,
+        "yaw": 0.719997528006361
+      },
+      {
+        "x": 0.678641851395345,
+        "y": 0.5901925520926865,
+        "yaw": 0.839997528006361
+      },
+      {
+        "x": 0.6972790215343484,
+        "y": 0.6136782159640051,
+        "yaw": 0.9599975280063608
+      },
+      {
+        "x": 0.7144846818675599,
+        "y": 0.6382539204808272,
+        "yaw": 0.9599975280063608
+      },
+      {
+        "x": 0.7316903422007714,
+        "y": 0.6628296249976493,
+        "yaw": 0.9599975280063608
+      },
+      {
+        "x": 0.7488960025339829,
+        "y": 0.6874053295144715,
+        "yaw": 0.9599975280063608
+      },
+      {
+        "x": 0.7661016628671945,
+        "y": 0.7119810340312936,
+        "yaw": 0.9599975280063608
+      },
+      {
+        "x": 0.783307323200406,
+        "y": 0.7365567385481158,
+        "yaw": 0.9599975280063608
+      },
+      {
+        "x": 0.8005129835336176,
+        "y": 0.7611324430649379,
+        "yaw": 0.9599975280063608
+      },
+      {
+        "x": 0.817718643866829,
+        "y": 0.78570814758176,
+        "yaw": 0.9599975280063608
+      },
+      {
+        "x": 0.8349243042000406,
+        "y": 0.8102838520985822,
+        "yaw": 0.9599975280063608
+      },
+      {
+        "x": 0.8521299645332521,
+        "y": 0.8348595566154043,
+        "yaw": 0.9599975280063608
+      },
+      {
+        "x": 0.8693356248664637,
+        "y": 0.8594352611322265,
+        "yaw": 0.9599975280063608
+      },
+      {
+        "x": 0.8865412851996751,
+        "y": 0.8840109656490487,
+        "yaw": 0.9599975280063608
+      },
+      {
+        "x": 0.9037469455328867,
+        "y": 0.9085866701658708,
+        "yaw": 0.9599975280063608
+      },
+      {
+        "x": 0.9209526058660982,
+        "y": 0.9331623746826929,
+        "yaw": 0.9599975280063608
+      },
+      {
+        "x": 0.9381582661993098,
+        "y": 0.957738079199515,
+        "yaw": 0.9599975280063608
+      },
+      {
+        "x": 0.9553639265325212,
+        "y": 0.9823137837163372,
+        "yaw": 0.9599975280063608
+      },
+      {
+        "x": 0.9725695868657328,
+        "y": 1.0068894882331594,
+        "yaw": 0.9599975280063608
+      },
+      {
+        "x": 0.9897752471989443,
+        "y": 1.0314651927499816,
+        "yaw": 0.9599975280063608
+      },
+      {
+        "x": 1.0069809075321559,
+        "y": 1.0560408972668036,
+        "yaw": 0.9599975280063608
+      },
+      {
+        "x": 1.0241865678653674,
+        "y": 1.0806166017836258,
+        "yaw": 0.9599975280063608
+      },
+      {
+        "x": 1.0413922281985788,
+        "y": 1.1051923063004478,
+        "yaw": 0.9599975280063608
+      },
+      {
+        "x": 1.0585978885317904,
+        "y": 1.12976801081727,
+        "yaw": 0.9599975280063608
+      },
+      {
+        "x": 1.0758035488650017,
+        "y": 1.154343715334092,
+        "yaw": 0.9599975280063608
+      },
+      {
+        "x": 1.0930092091982133,
+        "y": 1.1789194198509143,
+        "yaw": 0.9599975280063608
+      },
+      {
+        "x": 1.110214869531425,
+        "y": 1.2034951243677363,
+        "yaw": 0.9599975280063608
+      },
+      {
+        "x": 1.1274205298646365,
+        "y": 1.2280708288845585,
+        "yaw": 0.9599975280063608
+      },
+      {
+        "x": 1.1446261901978478,
+        "y": 1.2526465334013805,
+        "yaw": 0.9599975280063608
+      },
+      {
+        "x": 1.1618318505310594,
+        "y": 1.2772222379182028,
+        "yaw": 0.9599975280063608
+      },
+      {
+        "x": 1.179037510864271,
+        "y": 1.3017979424350248,
+        "yaw": 0.9599975280063608
+      },
+      {
+        "x": 1.1962431711974826,
+        "y": 1.326373646951847,
+        "yaw": 0.9599975280063608
+      },
+      {
+        "x": 1.213448831530694,
+        "y": 1.350949351468669,
+        "yaw": 0.9599975280063608
+      },
+      {
+        "x": 1.2306544918639055,
+        "y": 1.3755250559854912,
+        "yaw": 0.9599975280063608
+      },
+      {
+        "x": 1.247860152197117,
+        "y": 1.4001007605023132,
+        "yaw": 0.9599975280063608
+      },
+      {
+        "x": 1.2650658125303287,
+        "y": 1.4246764650191355,
+        "yaw": 0.9599975280063608
+      },
+      {
+        "x": 1.28227147286354,
+        "y": 1.4492521695359575,
+        "yaw": 0.9599975280063608
+      },
+      {
+        "x": 1.2994771331967516,
+        "y": 1.4738278740527797,
+        "yaw": 0.9599975280063608
+      },
+      {
+        "x": 1.3166827935299632,
+        "y": 1.4984035785696017,
+        "yaw": 0.9599975280063608
+      },
+      {
+        "x": 1.3353199636689665,
+        "y": 1.5218892424409203,
+        "yaw": 0.839997528006361
+      },
+      {
+        "x": 1.356634627792528,
+        "y": 1.5429749155761674,
+        "yaw": 0.719997528006361
+      },
+      {
+        "x": 1.3803202228779177,
+        "y": 1.5613573284677789,
+        "yaw": 0.5999975280063609
+      },
+      {
+        "x": 1.4060360854465814,
+        "y": 1.5767720918657788,
+        "yaw": 0.47999752800636086
+      },
+      {
+        "x": 1.4326459673753342,
+        "y": 1.5906254013526069,
+        "yaw": 0.47999752800636086
+      },
+      {
+        "x": 1.459255849304087,
+        "y": 1.6044787108394347,
+        "yaw": 0.47999752800636086
+      },
+      {
+        "x": 1.4858657312328396,
+        "y": 1.6183320203262626,
+        "yaw": 0.47999752800636086
+      },
+      {
+        "x": 1.5124756131615924,
+        "y": 1.6321853298130906,
+        "yaw": 0.47999752800636086
+      },
+      {
+        "x": 1.5390854950903452,
+        "y": 1.6460386392999187,
+        "yaw": 0.47999752800636086
+      },
+      {
+        "x": 1.565695377019098,
+        "y": 1.6598919487867465,
+        "yaw": 0.47999752800636086
+      },
+      {
+        "x": 1.5923052589478508,
+        "y": 1.6737452582735743,
+        "yaw": 0.47999752800636086
+      },
+      {
+        "x": 1.6189151408766034,
+        "y": 1.6875985677604024,
+        "yaw": 0.47999752800636086
+      },
+      {
+        "x": 1.6455250228053562,
+        "y": 1.7014518772472305,
+        "yaw": 0.47999752800636086
+      },
+      {
+        "x": 1.672134904734109,
+        "y": 1.7153051867340583,
+        "yaw": 0.47999752800636086
+      },
+      {
+        "x": 1.6987447866628616,
+        "y": 1.7291584962208861,
+        "yaw": 0.47999752800636086
+      },
+      {
+        "x": 1.7253546685916143,
+        "y": 1.7430118057077142,
+        "yaw": 0.47999752800636086
+      },
+      {
+        "x": 1.7519645505203671,
+        "y": 1.7568651151945422,
+        "yaw": 0.47999752800636086
+      },
+      {
+        "x": 1.77857443244912,
+        "y": 1.77071842468137,
+        "yaw": 0.47999752800636086
+      },
+      {
+        "x": 1.8051843143778727,
+        "y": 1.784571734168198,
+        "yaw": 0.47999752800636086
+      },
+      {
+        "x": 1.8317941963066253,
+        "y": 1.798425043655026,
+        "yaw": 0.47999752800636086
+      },
+      {
+        "x": 1.8584040782353781,
+        "y": 1.812278353141854,
+        "yaw": 0.47999752800636086
+      },
+      {
+        "x": 1.885013960164131,
+        "y": 1.8261316626286819,
+        "yaw": 0.47999752800636086
+      },
+      {
+        "x": 1.9116238420928835,
+        "y": 1.8399849721155097,
+        "yaw": 0.47999752800636086
+      },
+      {
+        "x": 1.9382337240216363,
+        "y": 1.8538382816023378,
+        "yaw": 0.47999752800636086
+      },
+      {
+        "x": 1.964843605950389,
+        "y": 1.8676915910891658,
+        "yaw": 0.47999752800636086
+      },
+      {
+        "x": 1.991453487879142,
+        "y": 1.8815449005759937,
+        "yaw": 0.47999752800636086
+      },
+      {
+        "x": 2.0180633698078947,
+        "y": 1.8953982100628215,
+        "yaw": 0.47999752800636086
+      },
+      {
+        "x": 2.0446732517366475,
+        "y": 1.9092515195496496,
+        "yaw": 0.47999752800636086
+      },
+      {
+        "x": 2.0712831336654003,
+        "y": 1.9231048290364776,
+        "yaw": 0.47999752800636086
+      },
+      {
+        "x": 2.097893015594153,
+        "y": 1.9369581385233055,
+        "yaw": 0.47999752800636086
+      },
+      {
+        "x": 2.124502897522906,
+        "y": 1.9508114480101333,
+        "yaw": 0.47999752800636086
+      },
+      {
+        "x": 2.1511127794516587,
+        "y": 1.9646647574969613,
+        "yaw": 0.47999752800636086
+      },
+      {
+        "x": 2.1768286420203227,
+        "y": 1.9800795208949613,
+        "yaw": 0.5999975280063609
+      },
+      {
+        "x": 2.200514237105712,
+        "y": 1.9984619337865728,
+        "yaw": 0.719997528006361
+      },
+      {
+        "x": 2.2218289012292733,
+        "y": 2.0195476069218197,
+        "yaw": 0.839997528006361
+      },
+      {
+        "x": 2.240466071368277,
+        "y": 2.0430332707931385,
+        "yaw": 0.9599975280063608
+      },
+      {
+        "x": 2.2576717317014885,
+        "y": 2.0676089753099607,
+        "yaw": 0.9599975280063608
+      },
+      {
+        "x": 2.2748773920347003,
+        "y": 2.092184679826783,
+        "yaw": 0.9599975280063608
+      },
+      {
+        "x": 2.2920830523679117,
+        "y": 2.1167603843436047,
+        "yaw": 0.9599975280063608
+      },
+      {
+        "x": 2.309288712701123,
+        "y": 2.141336088860427,
+        "yaw": 0.9599975280063608
+      },
+      {
+        "x": 2.3264943730343344,
+        "y": 2.165911793377249,
+        "yaw": 0.9599975280063608
+      },
+      {
+        "x": 2.343700033367546,
+        "y": 2.1904874978940714,
+        "yaw": 0.9599975280063608
+      },
+      {
+        "x": 2.3609056937007575,
+        "y": 2.215063202410893,
+        "yaw": 0.9599975280063608
+      },
+      {
+        "x": 2.378111354033969,
+        "y": 2.2396389069277154,
+        "yaw": 0.9599975280063608
+      },
+      {
+        "x": 2.3953170143671803,
+        "y": 2.2642146114445376,
+        "yaw": 0.9599975280063608
+      },
+      {
+        "x": 2.412522674700392,
+        "y": 2.28879031596136,
+        "yaw": 0.9599975280063608
+      },
+      {
+        "x": 2.4297283350336034,
+        "y": 2.3133660204781816,
+        "yaw": 0.9599975280063608
+      },
+      {
+        "x": 2.4469339953668148,
+        "y": 2.337941724995004,
+        "yaw": 0.9599975280063608
+      },
+      {
+        "x": 2.464139655700026,
+        "y": 2.362517429511826,
+        "yaw": 0.9599975280063608
+      },
+      {
+        "x": 2.481345316033238,
+        "y": 2.3870931340286483,
+        "yaw": 0.9599975280063608
+      },
+      {
+        "x": 2.4985509763664493,
+        "y": 2.41166883854547,
+        "yaw": 0.9599975280063608
+      },
+      {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      }
+    ]
+  },
+  "metrics": {
+    "sample_count": 300,
+    "goal_count": 0,
+    "plan_count": 3,
+    "planner_status_count": 9,
+    "tracker_status_count": 3,
+    "min_distance_to_goal": null,
+    "min_distance_to_plan_end": 0.12758898442616665,
+    "final_distance_to_goal": null,
+    "final_distance_to_plan_end": 0.13879026337272365,
+    "final_plan_end_to_goal": null,
+    "final_plan_end_yaw_to_goal": null,
+    "max_abs_cmd_linear": 0.6994810660961994,
+    "max_abs_cmd_angular": 2.464,
+    "max_abs_ackermann_speed": 0.6994810700416565,
+    "max_abs_steering_angle": 0.5195767879486084,
+    "tf_failures": 0,
+    "saw_plan_ok": true,
+    "saw_plan_failed": false,
+    "saw_goal_reached": false,
+    "saw_align_goal_yaw": false,
+    "saw_no_recent_odom": false
+  },
+  "diagnostic_hints": [
+    "没有采集到 /goal_pose；请先运行采集脚本，再发送目标。",
+    "tracker 的 min_linear_speed 数值大于 goal_tolerance；接近终点时可能仍以较高速度越过目标。"
+  ],
+  "events": {
+    "goals": [],
+    "plans": [
+      {
+        "frame_id": "map",
+        "points": 121,
+        "length": 3.5995680777533363,
+        "start": {
+          "x": 0.00019216537475585938,
+          "y": 0.0,
+          "yaw": 0.0
+        },
+        "end": {
+          "x": 2.5150759493367514,
+          "y": 2.4362507598464536,
+          "yaw": 0.96
+        },
+        "reverse_segment_fraction": 0.0,
+        "elapsed_sec": 5.436558864001199
+      },
+      {
+        "frame_id": "map",
+        "points": 121,
+        "length": 3.5995680777533363,
+        "start": {
+          "x": 0.00019216537475585938,
+          "y": 0.0,
+          "yaw": 0.0
+        },
+        "end": {
+          "x": 2.5150759493367514,
+          "y": 2.4362507598464536,
+          "yaw": 0.96
+        },
+        "reverse_segment_fraction": 0.0,
+        "elapsed_sec": 5.579407388000618
+      },
+      {
+        "frame_id": "map",
+        "points": 121,
+        "length": 3.5995680777533337,
+        "start": {
+          "x": 0.0008668303489685059,
+          "y": 0.0,
+          "yaw": -2.471993639119318e-06
+        },
+        "end": {
+          "x": 2.5157566366996607,
+          "y": 2.4362445430622923,
+          "yaw": 0.9599975280063608
+        },
+        "reverse_segment_fraction": 0.0,
+        "elapsed_sec": 5.727858651000133
+      }
+    ],
+    "planner_status": [
+      {
+        "elapsed_sec": 5.3221765700000105,
+        "data": "goal_received map=(2.500,2.500,0.000)"
+      },
+      {
+        "elapsed_sec": 5.324326440000732,
+        "data": "plan_start start=(0.000,0.000,0.000) goal=(2.500,2.500,0.000) obstacles=0 resolution=0.100 yaw_bins=36 timeout_ms=1500.000 grid_heuristic=true heuristic_weight=1.350 yaw_heuristic_weight=0.000 goal_yaw_tolerance=0.175"
+      },
+      {
+        "elapsed_sec": 5.437538529000449,
+        "data": "plan_ok hybrid_astar points=121 iterations=45 obstacles=0 planning_ms=71.628 goal_error=0.066 yaw_error=0.960",
+        "parsed_plan_ok": {
+          "points": 121,
+          "iterations": 45,
+          "planning_ms": 71.628,
+          "goal_error": 0.066,
+          "yaw_error": 0.96
+        }
+      },
+      {
+        "elapsed_sec": 5.473495158999867,
+        "data": "goal_received map=(2.500,2.500,0.000)"
+      },
+      {
+        "elapsed_sec": 5.475903196000218,
+        "data": "plan_start start=(0.000,0.000,0.000) goal=(2.500,2.500,0.000) obstacles=0 resolution=0.100 yaw_bins=36 timeout_ms=1500.000 grid_heuristic=true heuristic_weight=1.350 yaw_heuristic_weight=0.000 goal_yaw_tolerance=0.175"
+      },
+      {
+        "elapsed_sec": 5.580435928000952,
+        "data": "plan_ok hybrid_astar points=121 iterations=45 obstacles=0 planning_ms=62.015 goal_error=0.066 yaw_error=0.960",
+        "parsed_plan_ok": {
+          "points": 121,
+          "iterations": 45,
+          "planning_ms": 62.015,
+          "goal_error": 0.066,
+          "yaw_error": 0.96
+        }
+      },
+      {
+        "elapsed_sec": 5.624965623001117,
+        "data": "goal_received map=(2.500,2.500,0.000)"
+      },
+      {
+        "elapsed_sec": 5.627352660001634,
+        "data": "plan_start start=(0.001,0.000,-0.000) goal=(2.500,2.500,0.000) obstacles=0 resolution=0.100 yaw_bins=36 timeout_ms=1500.000 grid_heuristic=true heuristic_weight=1.350 yaw_heuristic_weight=0.000 goal_yaw_tolerance=0.175"
+      },
+      {
+        "elapsed_sec": 5.728937814999881,
+        "data": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+        "parsed_plan_ok": {
+          "points": 121,
+          "iterations": 43,
+          "planning_ms": 56.888,
+          "goal_error": 0.066,
+          "yaw_error": 0.96
+        }
+      }
+    ],
+    "tracker_status": [
+      {
+        "elapsed_sec": 5.442361102001087,
+        "data": "external_plan_ok points=121 length=3.600 goal=(2.515,2.436,0.960)"
+      },
+      {
+        "elapsed_sec": 5.587261788001342,
+        "data": "external_plan_ok points=121 length=3.600 goal=(2.515,2.436,0.960)"
+      },
+      {
+        "elapsed_sec": 5.7343625530011195,
+        "data": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+      }
+    ]
+  },
+  "samples": [
+    {
+      "elapsed_sec": 0.2045806830010406,
+      "pose_map": null,
+      "goal": null,
+      "plan_end": null,
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": null,
+      "yaw_error_to_plan_end": null,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": null,
+      "ackermann_cmd": null,
+      "planner_status": "",
+      "tracker_status": ""
+    },
+    {
+      "elapsed_sec": 0.2582582330014702,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 0.00019216537475585938,
+        "y": 0.0,
+        "yaw": 0.0,
+        "odom_age_sec": 0.029117854000000002,
+        "odom_linear_x": 0.0,
+        "odom_angular_z": 0.0
+      },
+      "goal": null,
+      "plan_end": null,
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": null,
+      "yaw_error_to_plan_end": null,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.0,
+        "angular_z": 0.0
+      },
+      "ackermann_cmd": {
+        "speed": 0.0,
+        "steering_angle": 0.0
+      },
+      "planner_status": "",
+      "tracker_status": ""
+    },
+    {
+      "elapsed_sec": 0.35756993500035605,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 0.00019216537475585938,
+        "y": 0.0,
+        "yaw": 0.0,
+        "odom_age_sec": 0.028299731,
+        "odom_linear_x": 0.0,
+        "odom_angular_z": 0.0
+      },
+      "goal": null,
+      "plan_end": null,
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": null,
+      "yaw_error_to_plan_end": null,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.0,
+        "angular_z": 0.0
+      },
+      "ackermann_cmd": {
+        "speed": 0.0,
+        "steering_angle": 0.0
+      },
+      "planner_status": "",
+      "tracker_status": ""
+    },
+    {
+      "elapsed_sec": 0.4578452180012391,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 0.00019216537475585938,
+        "y": 0.0,
+        "yaw": 0.0,
+        "odom_age_sec": 0.028546313,
+        "odom_linear_x": 0.0,
+        "odom_angular_z": 0.0
+      },
+      "goal": null,
+      "plan_end": null,
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": null,
+      "yaw_error_to_plan_end": null,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.0,
+        "angular_z": 0.0
+      },
+      "ackermann_cmd": {
+        "speed": 0.0,
+        "steering_angle": 0.0
+      },
+      "planner_status": "",
+      "tracker_status": ""
+    },
+    {
+      "elapsed_sec": 0.5578280429999722,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 0.00019216537475585938,
+        "y": 0.0,
+        "yaw": 0.0,
+        "odom_age_sec": 0.028538772,
+        "odom_linear_x": 0.0,
+        "odom_angular_z": 0.0
+      },
+      "goal": null,
+      "plan_end": null,
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": null,
+      "yaw_error_to_plan_end": null,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.0,
+        "angular_z": 0.0
+      },
+      "ackermann_cmd": {
+        "speed": 0.0,
+        "steering_angle": 0.0
+      },
+      "planner_status": "",
+      "tracker_status": ""
+    },
+    {
+      "elapsed_sec": 0.6578360769999563,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 0.00019216537475585938,
+        "y": 0.0,
+        "yaw": 0.0,
+        "odom_age_sec": 0.028498647000000002,
+        "odom_linear_x": 0.0,
+        "odom_angular_z": 0.0
+      },
+      "goal": null,
+      "plan_end": null,
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": null,
+      "yaw_error_to_plan_end": null,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.0,
+        "angular_z": 0.0
+      },
+      "ackermann_cmd": {
+        "speed": 0.0,
+        "steering_angle": 0.0
+      },
+      "planner_status": "",
+      "tracker_status": ""
+    },
+    {
+      "elapsed_sec": 0.7576833610000904,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 0.00019216537475585938,
+        "y": 0.0,
+        "yaw": 0.0,
+        "odom_age_sec": 0.028382606,
+        "odom_linear_x": 0.0,
+        "odom_angular_z": 0.0
+      },
+      "goal": null,
+      "plan_end": null,
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": null,
+      "yaw_error_to_plan_end": null,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.0,
+        "angular_z": 0.0
+      },
+      "ackermann_cmd": {
+        "speed": 0.0,
+        "steering_angle": 0.0
+      },
+      "planner_status": "",
+      "tracker_status": ""
+    },
+    {
+      "elapsed_sec": 0.8580006020001747,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 0.00019216537475585938,
+        "y": 0.0,
+        "yaw": 0.0,
+        "odom_age_sec": 0.028638855,
+        "odom_linear_x": 0.0,
+        "odom_angular_z": 0.0
+      },
+      "goal": null,
+      "plan_end": null,
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": null,
+      "yaw_error_to_plan_end": null,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.0,
+        "angular_z": 0.0
+      },
+      "ackermann_cmd": {
+        "speed": 0.0,
+        "steering_angle": 0.0
+      },
+      "planner_status": "",
+      "tracker_status": ""
+    },
+    {
+      "elapsed_sec": 0.958935967000798,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 0.00019216537475585938,
+        "y": 0.0,
+        "yaw": 0.0,
+        "odom_age_sec": 0.028959437,
+        "odom_linear_x": 0.0,
+        "odom_angular_z": 0.0
+      },
+      "goal": null,
+      "plan_end": null,
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": null,
+      "yaw_error_to_plan_end": null,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.0,
+        "angular_z": 0.0
+      },
+      "ackermann_cmd": {
+        "speed": 0.0,
+        "steering_angle": 0.0
+      },
+      "planner_status": "",
+      "tracker_status": ""
+    },
+    {
+      "elapsed_sec": 1.05906991700067,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 0.00019216537475585938,
+        "y": 0.0,
+        "yaw": 0.0,
+        "odom_age_sec": 0.029078812000000002,
+        "odom_linear_x": 0.0,
+        "odom_angular_z": 0.0
+      },
+      "goal": null,
+      "plan_end": null,
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": null,
+      "yaw_error_to_plan_end": null,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.0,
+        "angular_z": 0.0
+      },
+      "ackermann_cmd": {
+        "speed": 0.0,
+        "steering_angle": 0.0
+      },
+      "planner_status": "",
+      "tracker_status": ""
+    },
+    {
+      "elapsed_sec": 1.1577966620006919,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 0.00019216537475585938,
+        "y": 0.0,
+        "yaw": 0.0,
+        "odom_age_sec": 0.028552980000000002,
+        "odom_linear_x": 0.0,
+        "odom_angular_z": 0.0
+      },
+      "goal": null,
+      "plan_end": null,
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": null,
+      "yaw_error_to_plan_end": null,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.0,
+        "angular_z": 0.0
+      },
+      "ackermann_cmd": {
+        "speed": 0.0,
+        "steering_angle": 0.0
+      },
+      "planner_status": "",
+      "tracker_status": ""
+    },
+    {
+      "elapsed_sec": 1.257983237001099,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 0.00019216537475585938,
+        "y": 0.0,
+        "yaw": 0.0,
+        "odom_age_sec": 0.028617605,
+        "odom_linear_x": 0.0,
+        "odom_angular_z": 0.0
+      },
+      "goal": null,
+      "plan_end": null,
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": null,
+      "yaw_error_to_plan_end": null,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.0,
+        "angular_z": 0.0
+      },
+      "ackermann_cmd": {
+        "speed": 0.0,
+        "steering_angle": 0.0
+      },
+      "planner_status": "",
+      "tracker_status": ""
+    },
+    {
+      "elapsed_sec": 1.3575609380004607,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 0.00019216537475585938,
+        "y": 0.0,
+        "yaw": 0.0,
+        "odom_age_sec": 0.028370106000000003,
+        "odom_linear_x": 0.0,
+        "odom_angular_z": 0.0
+      },
+      "goal": null,
+      "plan_end": null,
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": null,
+      "yaw_error_to_plan_end": null,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.0,
+        "angular_z": 0.0
+      },
+      "ackermann_cmd": {
+        "speed": 0.0,
+        "steering_angle": 0.0
+      },
+      "planner_status": "",
+      "tracker_status": ""
+    },
+    {
+      "elapsed_sec": 1.4591423850015417,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 0.00019216537475585938,
+        "y": 0.0,
+        "yaw": 0.0,
+        "odom_age_sec": 0.12922672100000002,
+        "odom_linear_x": 0.0,
+        "odom_angular_z": 0.0
+      },
+      "goal": null,
+      "plan_end": null,
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": null,
+      "yaw_error_to_plan_end": null,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.0,
+        "angular_z": 0.0
+      },
+      "ackermann_cmd": {
+        "speed": 0.0,
+        "steering_angle": 0.0
+      },
+      "planner_status": "",
+      "tracker_status": ""
+    },
+    {
+      "elapsed_sec": 1.5577502140004071,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 0.00019216537475585938,
+        "y": 0.0,
+        "yaw": 0.0,
+        "odom_age_sec": 0.028399855,
+        "odom_linear_x": 0.0,
+        "odom_angular_z": 0.0
+      },
+      "goal": null,
+      "plan_end": null,
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": null,
+      "yaw_error_to_plan_end": null,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.0,
+        "angular_z": 0.0
+      },
+      "ackermann_cmd": {
+        "speed": 0.0,
+        "steering_angle": 0.0
+      },
+      "planner_status": "",
+      "tracker_status": ""
+    },
+    {
+      "elapsed_sec": 1.6577362470015942,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 0.00019216537475585938,
+        "y": 0.0,
+        "yaw": 0.0,
+        "odom_age_sec": 0.028429438,
+        "odom_linear_x": 0.0,
+        "odom_angular_z": 0.0
+      },
+      "goal": null,
+      "plan_end": null,
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": null,
+      "yaw_error_to_plan_end": null,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.0,
+        "angular_z": 0.0
+      },
+      "ackermann_cmd": {
+        "speed": 0.0,
+        "steering_angle": 0.0
+      },
+      "planner_status": "",
+      "tracker_status": ""
+    },
+    {
+      "elapsed_sec": 1.7577636140013055,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 0.00019216537475585938,
+        "y": 0.0,
+        "yaw": 0.0,
+        "odom_age_sec": 0.028487314000000003,
+        "odom_linear_x": 0.0,
+        "odom_angular_z": 0.0
+      },
+      "goal": null,
+      "plan_end": null,
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": null,
+      "yaw_error_to_plan_end": null,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.0,
+        "angular_z": 0.0
+      },
+      "ackermann_cmd": {
+        "speed": 0.0,
+        "steering_angle": 0.0
+      },
+      "planner_status": "",
+      "tracker_status": ""
+    },
+    {
+      "elapsed_sec": 1.8578048140007013,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 0.00019216537475585938,
+        "y": 0.0,
+        "yaw": 0.0,
+        "odom_age_sec": 0.028564230000000003,
+        "odom_linear_x": 0.0,
+        "odom_angular_z": 0.0
+      },
+      "goal": null,
+      "plan_end": null,
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": null,
+      "yaw_error_to_plan_end": null,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.0,
+        "angular_z": 0.0
+      },
+      "ackermann_cmd": {
+        "speed": 0.0,
+        "steering_angle": 0.0
+      },
+      "planner_status": "",
+      "tracker_status": ""
+    },
+    {
+      "elapsed_sec": 1.9576935980003327,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 0.00019216537475585938,
+        "y": 0.0,
+        "yaw": 0.0,
+        "odom_age_sec": 0.028366980000000003,
+        "odom_linear_x": 0.0,
+        "odom_angular_z": 0.0
+      },
+      "goal": null,
+      "plan_end": null,
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": null,
+      "yaw_error_to_plan_end": null,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.0,
+        "angular_z": 0.0
+      },
+      "ackermann_cmd": {
+        "speed": 0.0,
+        "steering_angle": 0.0
+      },
+      "planner_status": "",
+      "tracker_status": ""
+    },
+    {
+      "elapsed_sec": 2.0579264650004916,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 0.00019216537475585938,
+        "y": 0.0,
+        "yaw": 0.0,
+        "odom_age_sec": 0.028567980000000003,
+        "odom_linear_x": 0.0,
+        "odom_angular_z": 0.0
+      },
+      "goal": null,
+      "plan_end": null,
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": null,
+      "yaw_error_to_plan_end": null,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.0,
+        "angular_z": 0.0
+      },
+      "ackermann_cmd": {
+        "speed": 0.0,
+        "steering_angle": 0.0
+      },
+      "planner_status": "",
+      "tracker_status": ""
+    },
+    {
+      "elapsed_sec": 2.157790623999972,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 0.00019216537475585938,
+        "y": 0.0,
+        "yaw": 0.0,
+        "odom_age_sec": 0.028469147,
+        "odom_linear_x": 0.0,
+        "odom_angular_z": 0.0
+      },
+      "goal": null,
+      "plan_end": null,
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": null,
+      "yaw_error_to_plan_end": null,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.0,
+        "angular_z": 0.0
+      },
+      "ackermann_cmd": {
+        "speed": 0.0,
+        "steering_angle": 0.0
+      },
+      "planner_status": "",
+      "tracker_status": ""
+    },
+    {
+      "elapsed_sec": 2.2578199910003605,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 0.00019216537475585938,
+        "y": 0.0,
+        "yaw": 0.0,
+        "odom_age_sec": 0.028602646000000002,
+        "odom_linear_x": 0.0,
+        "odom_angular_z": 0.0
+      },
+      "goal": null,
+      "plan_end": null,
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": null,
+      "yaw_error_to_plan_end": null,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.0,
+        "angular_z": 0.0
+      },
+      "ackermann_cmd": {
+        "speed": 0.0,
+        "steering_angle": 0.0
+      },
+      "planner_status": "",
+      "tracker_status": ""
+    },
+    {
+      "elapsed_sec": 2.357943024000633,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 0.00019216537475585938,
+        "y": 0.0,
+        "yaw": 0.0,
+        "odom_age_sec": 0.028632022000000003,
+        "odom_linear_x": 0.0,
+        "odom_angular_z": 0.0
+      },
+      "goal": null,
+      "plan_end": null,
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": null,
+      "yaw_error_to_plan_end": null,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.0,
+        "angular_z": 0.0
+      },
+      "ackermann_cmd": {
+        "speed": 0.0,
+        "steering_angle": 0.0
+      },
+      "planner_status": "",
+      "tracker_status": ""
+    },
+    {
+      "elapsed_sec": 2.4575087670000357,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 0.00019216537475585938,
+        "y": 0.0,
+        "yaw": 0.0,
+        "odom_age_sec": 0.028360106000000003,
+        "odom_linear_x": 0.0,
+        "odom_angular_z": 0.0
+      },
+      "goal": null,
+      "plan_end": null,
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": null,
+      "yaw_error_to_plan_end": null,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.0,
+        "angular_z": 0.0
+      },
+      "ackermann_cmd": {
+        "speed": 0.0,
+        "steering_angle": 0.0
+      },
+      "planner_status": "",
+      "tracker_status": ""
+    },
+    {
+      "elapsed_sec": 2.557883383000444,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 0.00019216537475585938,
+        "y": 0.0,
+        "yaw": 0.0,
+        "odom_age_sec": 0.028646647,
+        "odom_linear_x": 0.0,
+        "odom_angular_z": 0.0
+      },
+      "goal": null,
+      "plan_end": null,
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": null,
+      "yaw_error_to_plan_end": null,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.0,
+        "angular_z": 0.0
+      },
+      "ackermann_cmd": {
+        "speed": 0.0,
+        "steering_angle": 0.0
+      },
+      "planner_status": "",
+      "tracker_status": ""
+    },
+    {
+      "elapsed_sec": 2.657807709001645,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 0.00019216537475585938,
+        "y": 0.0,
+        "yaw": 0.0,
+        "odom_age_sec": 0.028572563000000002,
+        "odom_linear_x": 0.0,
+        "odom_angular_z": 0.0
+      },
+      "goal": null,
+      "plan_end": null,
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": null,
+      "yaw_error_to_plan_end": null,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.0,
+        "angular_z": 0.0
+      },
+      "ackermann_cmd": {
+        "speed": 0.0,
+        "steering_angle": 0.0
+      },
+      "planner_status": "",
+      "tracker_status": ""
+    },
+    {
+      "elapsed_sec": 2.757809784001438,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 0.00019216537475585938,
+        "y": 0.0,
+        "yaw": 0.0,
+        "odom_age_sec": 0.028588438,
+        "odom_linear_x": 0.0,
+        "odom_angular_z": 0.0
+      },
+      "goal": null,
+      "plan_end": null,
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": null,
+      "yaw_error_to_plan_end": null,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.0,
+        "angular_z": 0.0
+      },
+      "ackermann_cmd": {
+        "speed": 0.0,
+        "steering_angle": 0.0
+      },
+      "planner_status": "",
+      "tracker_status": ""
+    },
+    {
+      "elapsed_sec": 2.857884525999907,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 0.00019216537475585938,
+        "y": 0.0,
+        "yaw": 0.0,
+        "odom_age_sec": 0.028630272000000002,
+        "odom_linear_x": 0.0,
+        "odom_angular_z": 0.0
+      },
+      "goal": null,
+      "plan_end": null,
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": null,
+      "yaw_error_to_plan_end": null,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.0,
+        "angular_z": 0.0
+      },
+      "ackermann_cmd": {
+        "speed": 0.0,
+        "steering_angle": 0.0
+      },
+      "planner_status": "",
+      "tracker_status": ""
+    },
+    {
+      "elapsed_sec": 2.9577482680015237,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 0.00019216537475585938,
+        "y": 0.0,
+        "yaw": 0.0,
+        "odom_age_sec": 0.028529772000000002,
+        "odom_linear_x": 0.0,
+        "odom_angular_z": 0.0
+      },
+      "goal": null,
+      "plan_end": null,
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": null,
+      "yaw_error_to_plan_end": null,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.0,
+        "angular_z": 0.0
+      },
+      "ackermann_cmd": {
+        "speed": 0.0,
+        "steering_angle": 0.0
+      },
+      "planner_status": "",
+      "tracker_status": ""
+    },
+    {
+      "elapsed_sec": 3.0578425520016026,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 0.00019216537475585938,
+        "y": 0.0,
+        "yaw": 0.0,
+        "odom_age_sec": 0.028580438000000003,
+        "odom_linear_x": 0.0,
+        "odom_angular_z": 0.0
+      },
+      "goal": null,
+      "plan_end": null,
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": null,
+      "yaw_error_to_plan_end": null,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.0,
+        "angular_z": 0.0
+      },
+      "ackermann_cmd": {
+        "speed": 0.0,
+        "steering_angle": 0.0
+      },
+      "planner_status": "",
+      "tracker_status": ""
+    },
+    {
+      "elapsed_sec": 3.157805501999974,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 0.00019216537475585938,
+        "y": 0.0,
+        "yaw": 0.0,
+        "odom_age_sec": 0.028554521000000003,
+        "odom_linear_x": 0.0,
+        "odom_angular_z": 0.0
+      },
+      "goal": null,
+      "plan_end": null,
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": null,
+      "yaw_error_to_plan_end": null,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.0,
+        "angular_z": 0.0
+      },
+      "ackermann_cmd": {
+        "speed": 0.0,
+        "steering_angle": 0.0
+      },
+      "planner_status": "",
+      "tracker_status": ""
+    },
+    {
+      "elapsed_sec": 3.2579017860007298,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 0.00019216537475585938,
+        "y": 0.0,
+        "yaw": 0.0,
+        "odom_age_sec": 0.028664771000000002,
+        "odom_linear_x": 0.0,
+        "odom_angular_z": 0.0
+      },
+      "goal": null,
+      "plan_end": null,
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": null,
+      "yaw_error_to_plan_end": null,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.0,
+        "angular_z": 0.0
+      },
+      "ackermann_cmd": {
+        "speed": 0.0,
+        "steering_angle": 0.0
+      },
+      "planner_status": "",
+      "tracker_status": ""
+    },
+    {
+      "elapsed_sec": 3.357715570000437,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 0.00019216537475585938,
+        "y": 0.0,
+        "yaw": 0.0,
+        "odom_age_sec": 0.028488647000000002,
+        "odom_linear_x": 0.0,
+        "odom_angular_z": 0.0
+      },
+      "goal": null,
+      "plan_end": null,
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": null,
+      "yaw_error_to_plan_end": null,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.0,
+        "angular_z": 0.0
+      },
+      "ackermann_cmd": {
+        "speed": 0.0,
+        "steering_angle": 0.0
+      },
+      "planner_status": "",
+      "tracker_status": ""
+    },
+    {
+      "elapsed_sec": 3.4579516030007653,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 0.00019216537475585938,
+        "y": 0.0,
+        "yaw": 0.0,
+        "odom_age_sec": 0.028682646000000003,
+        "odom_linear_x": 0.0,
+        "odom_angular_z": 0.0
+      },
+      "goal": null,
+      "plan_end": null,
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": null,
+      "yaw_error_to_plan_end": null,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.0,
+        "angular_z": 0.0
+      },
+      "ackermann_cmd": {
+        "speed": 0.0,
+        "steering_angle": 0.0
+      },
+      "planner_status": "",
+      "tracker_status": ""
+    },
+    {
+      "elapsed_sec": 3.5581298860015522,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 0.00019216537475585938,
+        "y": 0.0,
+        "yaw": 0.0,
+        "odom_age_sec": 0.028581813,
+        "odom_linear_x": 0.0,
+        "odom_angular_z": 0.0
+      },
+      "goal": null,
+      "plan_end": null,
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": null,
+      "yaw_error_to_plan_end": null,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.0,
+        "angular_z": 0.0
+      },
+      "ackermann_cmd": {
+        "speed": 0.0,
+        "steering_angle": 0.0
+      },
+      "planner_status": "",
+      "tracker_status": ""
+    },
+    {
+      "elapsed_sec": 3.65776587900109,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 0.00019216537475585938,
+        "y": 0.0,
+        "yaw": 0.0,
+        "odom_age_sec": 0.028585647000000002,
+        "odom_linear_x": 0.0,
+        "odom_angular_z": 0.0
+      },
+      "goal": null,
+      "plan_end": null,
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": null,
+      "yaw_error_to_plan_end": null,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.0,
+        "angular_z": 0.0
+      },
+      "ackermann_cmd": {
+        "speed": 0.0,
+        "steering_angle": 0.0
+      },
+      "planner_status": "",
+      "tracker_status": ""
+    },
+    {
+      "elapsed_sec": 3.7578443710008287,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 0.00019216537475585938,
+        "y": 0.0,
+        "yaw": 0.0,
+        "odom_age_sec": 0.028585480000000003,
+        "odom_linear_x": 0.0,
+        "odom_angular_z": 0.0
+      },
+      "goal": null,
+      "plan_end": null,
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": null,
+      "yaw_error_to_plan_end": null,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.0,
+        "angular_z": 0.0
+      },
+      "ackermann_cmd": {
+        "speed": 0.0,
+        "steering_angle": 0.0
+      },
+      "planner_status": "",
+      "tracker_status": ""
+    },
+    {
+      "elapsed_sec": 3.8577105710010073,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 0.00019216537475585938,
+        "y": 0.0,
+        "yaw": 0.0,
+        "odom_age_sec": 0.028527605,
+        "odom_linear_x": 0.0,
+        "odom_angular_z": 0.0
+      },
+      "goal": null,
+      "plan_end": null,
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": null,
+      "yaw_error_to_plan_end": null,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.0,
+        "angular_z": 0.0
+      },
+      "ackermann_cmd": {
+        "speed": 0.0,
+        "steering_angle": 0.0
+      },
+      "planner_status": "",
+      "tracker_status": ""
+    },
+    {
+      "elapsed_sec": 3.957784480000555,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 0.00019216537475585938,
+        "y": 0.0,
+        "yaw": 0.0,
+        "odom_age_sec": 0.028667729000000003,
+        "odom_linear_x": 0.0,
+        "odom_angular_z": 0.0
+      },
+      "goal": null,
+      "plan_end": null,
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": null,
+      "yaw_error_to_plan_end": null,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.0,
+        "angular_z": 0.0
+      },
+      "ackermann_cmd": {
+        "speed": 0.0,
+        "steering_angle": 0.0
+      },
+      "planner_status": "",
+      "tracker_status": ""
+    },
+    {
+      "elapsed_sec": 4.057831347001411,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 0.00019216537475585938,
+        "y": 0.0,
+        "yaw": 0.0,
+        "odom_age_sec": 0.02871198,
+        "odom_linear_x": 0.0,
+        "odom_angular_z": 0.0
+      },
+      "goal": null,
+      "plan_end": null,
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": null,
+      "yaw_error_to_plan_end": null,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.0,
+        "angular_z": 0.0
+      },
+      "ackermann_cmd": {
+        "speed": 0.0,
+        "steering_angle": 0.0
+      },
+      "planner_status": "",
+      "tracker_status": ""
+    },
+    {
+      "elapsed_sec": 4.157778214001155,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 0.00019216537475585938,
+        "y": 0.0,
+        "yaw": 0.0,
+        "odom_age_sec": 0.028616522000000002,
+        "odom_linear_x": 0.0,
+        "odom_angular_z": 0.0
+      },
+      "goal": null,
+      "plan_end": null,
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": null,
+      "yaw_error_to_plan_end": null,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.0,
+        "angular_z": 0.0
+      },
+      "ackermann_cmd": {
+        "speed": 0.0,
+        "steering_angle": 0.0
+      },
+      "planner_status": "",
+      "tracker_status": ""
+    },
+    {
+      "elapsed_sec": 4.257926206000775,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 0.00019216537475585938,
+        "y": 0.0,
+        "yaw": 0.0,
+        "odom_age_sec": 0.028727771000000003,
+        "odom_linear_x": 0.0,
+        "odom_angular_z": 0.0
+      },
+      "goal": null,
+      "plan_end": null,
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": null,
+      "yaw_error_to_plan_end": null,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.0,
+        "angular_z": 0.0
+      },
+      "ackermann_cmd": {
+        "speed": 0.0,
+        "steering_angle": 0.0
+      },
+      "planner_status": "",
+      "tracker_status": ""
+    },
+    {
+      "elapsed_sec": 4.35812736400112,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 0.00019216537475585938,
+        "y": 0.0,
+        "yaw": 0.0,
+        "odom_age_sec": 0.028772063,
+        "odom_linear_x": 0.0,
+        "odom_angular_z": 0.0
+      },
+      "goal": null,
+      "plan_end": null,
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": null,
+      "yaw_error_to_plan_end": null,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.0,
+        "angular_z": 0.0
+      },
+      "ackermann_cmd": {
+        "speed": 0.0,
+        "steering_angle": 0.0
+      },
+      "planner_status": "",
+      "tracker_status": ""
+    },
+    {
+      "elapsed_sec": 4.45831427200028,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 0.00019216537475585938,
+        "y": 0.0,
+        "yaw": 0.0,
+        "odom_age_sec": 0.028803563,
+        "odom_linear_x": 0.0,
+        "odom_angular_z": 0.0
+      },
+      "goal": null,
+      "plan_end": null,
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": null,
+      "yaw_error_to_plan_end": null,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.0,
+        "angular_z": 0.0
+      },
+      "ackermann_cmd": {
+        "speed": 0.0,
+        "steering_angle": 0.0
+      },
+      "planner_status": "",
+      "tracker_status": ""
+    },
+    {
+      "elapsed_sec": 4.558128389000558,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 0.00019216537475585938,
+        "y": 0.0,
+        "yaw": 0.0,
+        "odom_age_sec": 0.028765521000000002,
+        "odom_linear_x": 0.0,
+        "odom_angular_z": 0.0
+      },
+      "goal": null,
+      "plan_end": null,
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": null,
+      "yaw_error_to_plan_end": null,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.0,
+        "angular_z": 0.0
+      },
+      "ackermann_cmd": {
+        "speed": 0.0,
+        "steering_angle": 0.0
+      },
+      "planner_status": "",
+      "tracker_status": ""
+    },
+    {
+      "elapsed_sec": 4.658506256000692,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 0.00019216537475585938,
+        "y": 0.0,
+        "yaw": 0.0,
+        "odom_age_sec": 0.028919229,
+        "odom_linear_x": 0.0,
+        "odom_angular_z": 0.0
+      },
+      "goal": null,
+      "plan_end": null,
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": null,
+      "yaw_error_to_plan_end": null,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.0,
+        "angular_z": 0.0
+      },
+      "ackermann_cmd": {
+        "speed": 0.0,
+        "steering_angle": 0.0
+      },
+      "planner_status": "",
+      "tracker_status": ""
+    },
+    {
+      "elapsed_sec": 4.758095832001345,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 0.00019216537475585938,
+        "y": 0.0,
+        "yaw": 0.0,
+        "odom_age_sec": 0.02874198,
+        "odom_linear_x": 0.0,
+        "odom_angular_z": 0.0
+      },
+      "goal": null,
+      "plan_end": null,
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": null,
+      "yaw_error_to_plan_end": null,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.0,
+        "angular_z": 0.0
+      },
+      "ackermann_cmd": {
+        "speed": 0.0,
+        "steering_angle": 0.0
+      },
+      "planner_status": "",
+      "tracker_status": ""
+    },
+    {
+      "elapsed_sec": 4.859148322000692,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 0.00019216537475585938,
+        "y": 0.0,
+        "yaw": 0.0,
+        "odom_age_sec": 0.029147187,
+        "odom_linear_x": 0.0,
+        "odom_angular_z": 0.0
+      },
+      "goal": null,
+      "plan_end": null,
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": null,
+      "yaw_error_to_plan_end": null,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.0,
+        "angular_z": 0.0
+      },
+      "ackermann_cmd": {
+        "speed": 0.0,
+        "steering_angle": 0.0
+      },
+      "planner_status": "",
+      "tracker_status": ""
+    },
+    {
+      "elapsed_sec": 4.95788123300008,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 0.00019216537475585938,
+        "y": 0.0,
+        "yaw": 0.0,
+        "odom_age_sec": 0.028484980000000003,
+        "odom_linear_x": 0.0,
+        "odom_angular_z": 0.0
+      },
+      "goal": null,
+      "plan_end": null,
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": null,
+      "yaw_error_to_plan_end": null,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.0,
+        "angular_z": 0.0
+      },
+      "ackermann_cmd": {
+        "speed": 0.0,
+        "steering_angle": 0.0
+      },
+      "planner_status": "",
+      "tracker_status": ""
+    },
+    {
+      "elapsed_sec": 5.057914808001442,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 0.00019216537475585938,
+        "y": 0.0,
+        "yaw": 0.0,
+        "odom_age_sec": 0.028659313000000002,
+        "odom_linear_x": 0.0,
+        "odom_angular_z": 0.0
+      },
+      "goal": null,
+      "plan_end": null,
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": null,
+      "yaw_error_to_plan_end": null,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.0,
+        "angular_z": 0.0
+      },
+      "ackermann_cmd": {
+        "speed": 0.0,
+        "steering_angle": 0.0
+      },
+      "planner_status": "",
+      "tracker_status": ""
+    },
+    {
+      "elapsed_sec": 5.157883092000702,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 0.00019216537475585938,
+        "y": 0.0,
+        "yaw": 0.0,
+        "odom_age_sec": 0.028696021000000002,
+        "odom_linear_x": 0.0,
+        "odom_angular_z": 0.0
+      },
+      "goal": null,
+      "plan_end": null,
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": null,
+      "yaw_error_to_plan_end": null,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.0,
+        "angular_z": 0.0
+      },
+      "ackermann_cmd": {
+        "speed": 0.0,
+        "steering_angle": 0.0
+      },
+      "planner_status": "",
+      "tracker_status": ""
+    },
+    {
+      "elapsed_sec": 5.257824667000023,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 0.00019216537475585938,
+        "y": 0.0,
+        "yaw": 0.0,
+        "odom_age_sec": 0.02851773,
+        "odom_linear_x": 0.0,
+        "odom_angular_z": 0.0
+      },
+      "goal": null,
+      "plan_end": null,
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": null,
+      "yaw_error_to_plan_end": null,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.0,
+        "angular_z": 0.0
+      },
+      "ackermann_cmd": {
+        "speed": 0.0,
+        "steering_angle": 0.0
+      },
+      "planner_status": "",
+      "tracker_status": ""
+    },
+    {
+      "elapsed_sec": 5.3582457000011345,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 0.00019216537475585938,
+        "y": 0.0,
+        "yaw": 0.0,
+        "odom_age_sec": 0.027814773,
+        "odom_linear_x": 0.0,
+        "odom_angular_z": 0.0
+      },
+      "goal": null,
+      "plan_end": null,
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": null,
+      "yaw_error_to_plan_end": null,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.0,
+        "angular_z": 0.0
+      },
+      "ackermann_cmd": {
+        "speed": 0.0,
+        "steering_angle": 0.0
+      },
+      "planner_status": "plan_start start=(0.000,0.000,0.000) goal=(2.500,2.500,0.000) obstacles=0 resolution=0.100 yaw_bins=36 timeout_ms=1500.000 grid_heuristic=true heuristic_weight=1.350 yaw_heuristic_weight=0.000 goal_yaw_tolerance=0.175",
+      "tracker_status": ""
+    },
+    {
+      "elapsed_sec": 5.457553735001056,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 0.00019216537475585938,
+        "y": 0.0,
+        "yaw": 0.0,
+        "odom_age_sec": 0.028438439000000003,
+        "odom_linear_x": 0.0,
+        "odom_angular_z": 0.0
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5150759493367514,
+        "y": 2.4362507598464536,
+        "yaw": 0.96
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 3.5014223126734407,
+      "yaw_error_to_plan_end": 0.96,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.6361957075042455,
+        "angular_z": 1.9329531907687783
+      },
+      "ackermann_cmd": {
+        "speed": 0.636195719242096,
+        "steering_angle": 0.4098701477050781
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=45 obstacles=0 planning_ms=71.628 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.515,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 5.583944337000503,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 0.00019216537475585938,
+        "y": 0.0,
+        "yaw": 0.0,
+        "odom_age_sec": 0.054602965,
+        "odom_linear_x": 0.0,
+        "odom_angular_z": 0.0
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5150759493367514,
+        "y": 2.4362507598464536,
+        "yaw": 0.96
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 3.5014223126734407,
+      "yaw_error_to_plan_end": 0.96,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.6361957075042455,
+        "angular_z": 1.9329531907687783
+      },
+      "ackermann_cmd": {
+        "speed": 0.636195719242096,
+        "steering_angle": 0.4098701477050781
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=45 obstacles=0 planning_ms=62.015 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.515,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 5.657667511000909,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 0.00316619873046875,
+        "y": 0.0,
+        "yaw": 6.6946231527253985e-06,
+        "odom_age_sec": 0.028409522000000003,
+        "odom_linear_x": 0.04500000178813934,
+        "odom_angular_z": 0.0001847798703238368
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5150759493367514,
+        "y": 2.4362507598464536,
+        "yaw": 0.96
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 3.499286835920048,
+      "yaw_error_to_plan_end": 0.9599933053768472,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.6359036453937889,
+        "angular_z": 1.940909788121077
+      },
+      "ackermann_cmd": {
+        "speed": 0.6359036564826965,
+        "steering_angle": 0.4115419387817383
+      },
+      "planner_status": "plan_start start=(0.001,0.000,-0.000) goal=(2.500,2.500,0.000) obstacles=0 resolution=0.100 yaw_bins=36 timeout_ms=1500.000 grid_heuristic=true heuristic_weight=1.350 yaw_heuristic_weight=0.000 goal_yaw_tolerance=0.175",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.515,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 5.757855169000322,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 0.016282618045806885,
+        "y": 2.4586915969848633e-06,
+        "yaw": 0.0012106623034924269,
+        "odom_age_sec": 0.028551397000000003,
+        "odom_linear_x": 0.15700000524520874,
+        "odom_angular_z": 0.018186144530773163
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 3.490364717846047,
+      "yaw_error_to_plan_end": 0.9587868657028684,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.6332934288219569,
+        "angular_z": 2.0116587231570877
+      },
+      "ackermann_cmd": {
+        "speed": 0.6332934498786926,
+        "steering_angle": 0.42637428641319275
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 5.858270202001222,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 0.039534926414489746,
+        "y": 4.088878631591797e-05,
+        "yaw": 0.0034855883568525314,
+        "odom_age_sec": 0.028865854000000003,
+        "odom_linear_x": 0.24799999594688416,
+        "odom_angular_z": 0.027863916009664536
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 3.47372454342529,
+      "yaw_error_to_plan_end": 0.9565119396495083,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.6279986081702756,
+        "angular_z": 2.1531797073995023
+      },
+      "ackermann_cmd": {
+        "speed": 0.6279985904693604,
+        "steering_angle": 0.455853670835495
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 5.958238193999932,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 0.07500123977661133,
+        "y": 0.00020110607147216797,
+        "yaw": 0.00862658582627773,
+        "odom_age_sec": 0.02875573,
+        "odom_linear_x": 0.3659999966621399,
+        "odom_angular_z": 0.05999554321169853
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 3.448419135562706,
+      "yaw_error_to_plan_end": 0.9513709421800831,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.6239316841878692,
+        "angular_z": 2.2600682094284523
+      },
+      "ackermann_cmd": {
+        "speed": 0.6239317059516907,
+        "steering_angle": 0.4779350161552429
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 6.058046395000929,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 0.11713391542434692,
+        "y": 0.0006334930658340454,
+        "yaw": 0.01603860594332218,
+        "odom_age_sec": 0.028637063,
+        "odom_linear_x": 0.43799999356269836,
+        "odom_angular_z": 0.07964138686656952
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 3.4184195391851833,
+      "yaw_error_to_plan_end": 0.9439589220630387,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.6192502651494479,
+        "angular_z": 2.3811568912834273
+      },
+      "ackermann_cmd": {
+        "speed": 0.6192502379417419,
+        "steering_angle": 0.502741277217865
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 6.157833470000696,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 0.16598302125930786,
+        "y": 0.0015320628881454468,
+        "yaw": 0.027971815317869186,
+        "odom_age_sec": 0.028521397,
+        "odom_linear_x": 0.48399999737739563,
+        "odom_angular_z": 0.13205131888389587
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 3.383675650078679,
+      "yaw_error_to_plan_end": 0.9320257126884917,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.616,
+        "angular_z": 2.464
+      },
+      "ackermann_cmd": {
+        "speed": 0.6159999966621399,
+        "steering_angle": 0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 6.258114587000819,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 0.21835476160049438,
+        "y": 0.0031726807355880737,
+        "yaw": 0.04412075132131577,
+        "odom_age_sec": 0.028645396,
+        "odom_linear_x": 0.5230000019073486,
+        "odom_angular_z": 0.16878104209899902
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 3.3463254568190317,
+      "yaw_error_to_plan_end": 0.9158767766850451,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.616,
+        "angular_z": 2.464
+      },
+      "ackermann_cmd": {
+        "speed": 0.6159999966621399,
+        "steering_angle": 0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 6.357943579001585,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 0.27422863245010376,
+        "y": 0.0058364421129226685,
+        "yaw": 0.0619685985147953,
+        "odom_age_sec": 0.028598022,
+        "odom_linear_x": 0.5490000247955322,
+        "odom_angular_z": 0.18046903610229492
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 3.306256392204834,
+      "yaw_error_to_plan_end": 0.8980289294915655,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.616,
+        "angular_z": 2.464
+      },
+      "ackermann_cmd": {
+        "speed": 0.6159999966621399,
+        "steering_angle": 0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 6.457773613001336,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 0.33186817169189453,
+        "y": 0.009606912732124329,
+        "yaw": 0.08070702850818633,
+        "odom_age_sec": 0.028341564000000003,
+        "odom_linear_x": 0.5619999766349792,
+        "odom_angular_z": 0.18945133686065674
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 3.2646499071919473,
+      "yaw_error_to_plan_end": 0.8792904994981744,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.616,
+        "angular_z": 2.464
+      },
+      "ackermann_cmd": {
+        "speed": 0.6159999966621399,
+        "steering_angle": 0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 6.557904772000256,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 0.39209645986557007,
+        "y": 0.014677062630653381,
+        "yaw": 0.10098479688167572,
+        "odom_age_sec": 0.028567688,
+        "odom_linear_x": 0.5820000171661377,
+        "odom_angular_z": 0.2066677212715149
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 3.220857278575851,
+      "yaw_error_to_plan_end": 0.8590127311246851,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.616,
+        "angular_z": 2.464
+      },
+      "ackermann_cmd": {
+        "speed": 0.6159999966621399,
+        "steering_angle": 0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 6.65787780500068,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 0.45318639278411865,
+        "y": 0.021071046590805054,
+        "yaw": 0.1225937828421593,
+        "odom_age_sec": 0.028608897,
+        "odom_linear_x": 0.5950000286102295,
+        "odom_angular_z": 0.217640221118927
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 3.176044557172353,
+      "yaw_error_to_plan_end": 0.8374037451642016,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.616,
+        "angular_z": 2.464
+      },
+      "ackermann_cmd": {
+        "speed": 0.6159999966621399,
+        "steering_angle": 0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 6.757840464000765,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 0.514252245426178,
+        "y": 0.02878527343273163,
+        "yaw": 0.1452058404684067,
+        "odom_age_sec": 0.028471022000000002,
+        "odom_linear_x": 0.6019999980926514,
+        "odom_angular_z": 0.2327808141708374
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 3.1307954202107062,
+      "yaw_error_to_plan_end": 0.8147916875379542,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.616,
+        "angular_z": 2.464
+      },
+      "ackermann_cmd": {
+        "speed": 0.6159999966621399,
+        "steering_angle": 0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 6.857988581001337,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 0.5758389830589294,
+        "y": 0.03798562288284302,
+        "yaw": 0.169258251786232,
+        "odom_age_sec": 0.028617146000000003,
+        "odom_linear_x": 0.6019999980926514,
+        "odom_angular_z": 0.2407892644405365
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 3.0846274250105568,
+      "yaw_error_to_plan_end": 0.7907392762201289,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.616,
+        "angular_z": 2.464
+      },
+      "ackermann_cmd": {
+        "speed": 0.6159999966621399,
+        "steering_angle": 0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 6.957875323001645,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 0.6383925080299377,
+        "y": 0.0488499253988266,
+        "yaw": 0.193904846906662,
+        "odom_age_sec": 0.028452647,
+        "odom_linear_x": 0.6150000095367432,
+        "odom_angular_z": 0.2504505217075348
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 3.0371284352269847,
+      "yaw_error_to_plan_end": 0.7660926810996989,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.616,
+        "angular_z": 2.464
+      },
+      "ackermann_cmd": {
+        "speed": 0.6159999966621399,
+        "steering_angle": 0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 7.057828773000438,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 0.6984999775886536,
+        "y": 0.06081564724445343,
+        "yaw": 0.21993522346019745,
+        "odom_age_sec": 0.028518897,
+        "odom_linear_x": 0.5950000286102295,
+        "odom_angular_z": 0.2649911642074585
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 2.9908333628220842,
+      "yaw_error_to_plan_end": 0.7400623045461634,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.2513508582472093,
+        "angular_z": 0.8572602818583216
+      },
+      "ackermann_cmd": {
+        "speed": -0.25135084986686707,
+        "steering_angle": -0.4537734389305115
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 7.157965598000374,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 0.7534834742546082,
+        "y": 0.07326160371303558,
+        "yaw": 0.2502075433731079,
+        "odom_age_sec": 0.028609022,
+        "odom_linear_x": 0.5170000195503235,
+        "odom_angular_z": 0.31159859895706177
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 2.947761026733636,
+      "yaw_error_to_plan_end": 0.7097899846332529,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.6305484946319078,
+        "angular_z": 2.0853591504652558
+      },
+      "ackermann_cmd": {
+        "speed": 0.6305484771728516,
+        "steering_angle": 0.4417594075202942
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 7.257806507001078,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 0.7982586026191711,
+        "y": 0.08485554158687592,
+        "yaw": 0.28372570872306824,
+        "odom_age_sec": 0.028572647000000003,
+        "odom_linear_x": 0.43799999356269836,
+        "odom_angular_z": 0.34336188435554504
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 2.9118430131670734,
+      "yaw_error_to_plan_end": 0.6762718192832926,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.616,
+        "angular_z": 2.464
+      },
+      "ackermann_cmd": {
+        "speed": 0.6159999966621399,
+        "steering_angle": 0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 7.357995124000809,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 0.8388933539390564,
+        "y": 0.0968499630689621,
+        "yaw": 0.31658869981765747,
+        "odom_age_sec": 0.028656438000000003,
+        "odom_linear_x": 0.4050000011920929,
+        "odom_angular_z": 0.3039385974407196
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 2.8783046173004068,
+      "yaw_error_to_plan_end": 0.6434088281887034,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.24653113140863828,
+        "angular_z": 0.9822759572375539
+      },
+      "ackermann_cmd": {
+        "speed": -0.24653112888336182,
+        "steering_angle": -0.5178931355476379
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 7.457977241001572,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 0.872425377368927,
+        "y": 0.10784517228603363,
+        "yaw": 0.34225094318389887,
+        "odom_age_sec": 0.028604730000000002,
+        "odom_linear_x": 0.3140000104904175,
+        "odom_angular_z": 0.24583689868450165
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 2.84990899113021,
+      "yaw_error_to_plan_end": 0.6177465848224619,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.2473504881534342,
+        "angular_z": 0.9614134158594521
+      },
+      "ackermann_cmd": {
+        "speed": -0.24735048413276672,
+        "steering_angle": -0.5072998404502869
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 7.558000858000014,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 0.8961837887763977,
+        "y": 0.11629657447338104,
+        "yaw": 0.3678664863109589,
+        "odom_age_sec": 0.028608521,
+        "odom_linear_x": 0.22200000286102295,
+        "odom_angular_z": 0.2580014765262604
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 2.8293417585527,
+      "yaw_error_to_plan_end": 0.592131041695402,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.6186000122728937,
+        "angular_z": 2.3978111146191114
+      },
+      "ackermann_cmd": {
+        "speed": 0.6186000108718872,
+        "steering_angle": 0.5061348676681519
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 7.657907392000197,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 0.9164678454399109,
+        "y": 0.1241065114736557,
+        "yaw": 0.3905105888843536,
+        "odom_age_sec": 0.028504563,
+        "odom_linear_x": 0.22200000286102295,
+        "odom_angular_z": 0.21295076608657837
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 2.811353217752555,
+      "yaw_error_to_plan_end": 0.5694869391220072,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.6193543999306433,
+        "angular_z": 2.378486058952523
+      },
+      "ackermann_cmd": {
+        "speed": 0.6193544268608093,
+        "steering_angle": 0.5021966099739075
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 7.759259589000067,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 0.9404292702674866,
+        "y": 0.1339031308889389,
+        "yaw": 0.40701934695243835,
+        "odom_age_sec": 0.029174604000000003,
+        "odom_linear_x": 0.2549999952316284,
+        "odom_angular_z": 0.15169349312782288
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 2.7897011111655887,
+      "yaw_error_to_plan_end": 0.5529781810539225,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.24832322080239774,
+        "angular_z": 0.936438075594642
+      },
+      "ackermann_cmd": {
+        "speed": -0.2483232170343399,
+        "steering_angle": -0.49455973505973816
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 7.857701292999991,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 0.9626680016517639,
+        "y": 0.14338727295398712,
+        "yaw": 0.4207590520381927,
+        "odom_age_sec": 0.028342147,
+        "odom_linear_x": 0.22200000286102295,
+        "odom_angular_z": 0.13818825781345367
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 2.7693462711267163,
+      "yaw_error_to_plan_end": 0.5392384759681681,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.24899089121266185,
+        "angular_z": 0.9191649562702047
+      },
+      "ackermann_cmd": {
+        "speed": -0.24899089336395264,
+        "steering_angle": -0.48571234941482544
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 7.957928743000593,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 0.9782673716545105,
+        "y": 0.1502939909696579,
+        "yaw": 0.4360068142414093,
+        "odom_age_sec": 0.028557230000000003,
+        "odom_linear_x": 0.15000000596046448,
+        "odom_angular_z": 0.15598443150520325
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 2.7548944021035897,
+      "yaw_error_to_plan_end": 0.5239907137649515,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.6223422575669922,
+        "angular_z": 2.301414035395907
+      },
+      "ackermann_cmd": {
+        "speed": 0.6223422288894653,
+        "steering_angle": 0.4864308536052704
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 8.058013443000164,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 0.9927738308906555,
+        "y": 0.15699081122875214,
+        "yaw": 0.4502025842666626,
+        "odom_age_sec": 0.028632188000000003,
+        "odom_linear_x": 0.17000000178813934,
+        "odom_angular_z": 0.13745227456092834
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 2.7412541292019954,
+      "yaw_error_to_plan_end": 0.5097949437396982,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.6229552312113112,
+        "angular_z": 2.285497226399027
+      },
+      "ackermann_cmd": {
+        "speed": 0.6229552030563354,
+        "steering_angle": 0.48316332697868347
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 8.157808852000016,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 1.0136335492134094,
+        "y": 0.1669548898935318,
+        "yaw": 0.46106988191604614,
+        "odom_age_sec": 0.028550854,
+        "odom_linear_x": 0.24199999868869781,
+        "odom_angular_z": 0.10196255147457123
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 2.7214057580482227,
+      "yaw_error_to_plan_end": 0.4989276460903147,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.24973348692898614,
+        "angular_z": 0.8998287912388139
+      },
+      "ackermann_cmd": {
+        "speed": -0.24973349273204803,
+        "steering_angle": -0.4757741093635559
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 8.257827761000954,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 1.0380457043647766,
+        "y": 0.17891637980937958,
+        "yaw": 0.4709116518497467,
+        "odom_age_sec": 0.028468896,
+        "odom_linear_x": 0.26100000739097595,
+        "odom_angular_z": 0.10240167379379272
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 2.6979918525000777,
+      "yaw_error_to_plan_end": 0.48908587615661414,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.2505555833575616,
+        "angular_z": 0.8782694033891973
+      },
+      "ackermann_cmd": {
+        "speed": -0.2505555748939514,
+        "steering_angle": -0.46465200185775757
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 8.359289291000096,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 1.0580891966819763,
+        "y": 0.188994362950325,
+        "yaw": 0.48469871282577515,
+        "odom_age_sec": 0.029263062000000003,
+        "odom_linear_x": 0.20200000703334808,
+        "odom_angular_z": 0.15045501291751862
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 2.6786055584391253,
+      "yaw_error_to_plan_end": 0.4752988151805857,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.6268290834250059,
+        "angular_z": 2.1840789795271967
+      },
+      "ackermann_cmd": {
+        "speed": 0.6268290877342224,
+        "steering_angle": 0.4622539281845093
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 8.45779937000043,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 1.0746939778327942,
+        "y": 0.19765473902225494,
+        "yaw": 0.5010567903518677,
+        "odom_age_sec": 0.028484022,
+        "odom_linear_x": 0.18299999833106995,
+        "odom_angular_z": 0.16225489974021912
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 2.662319646010309,
+      "yaw_error_to_plan_end": 0.45894073765449317,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.6277432905099197,
+        "angular_z": 2.159936408415349
+      },
+      "ackermann_cmd": {
+        "speed": 0.6277433037757874,
+        "steering_angle": 0.4572543501853943
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 8.557797487001153,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 1.0935974717140198,
+        "y": 0.2079077512025833,
+        "yaw": 0.5156865119934082,
+        "odom_age_sec": 0.028408688,
+        "odom_linear_x": 0.22200000286102295,
+        "odom_angular_z": 0.13455137610435486
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 2.643486627261102,
+      "yaw_error_to_plan_end": 0.4443110160129527,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.6289324521449596,
+        "angular_z": 2.1284136733523713
+      },
+      "ackermann_cmd": {
+        "speed": 0.6289324760437012,
+        "steering_angle": 0.4507141411304474
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 8.657848312001079,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 1.118075668811798,
+        "y": 0.22160933911800385,
+        "yaw": 0.5268954634666444,
+        "odom_age_sec": 0.028530355,
+        "odom_linear_x": 0.28700000047683716,
+        "odom_angular_z": 0.10935904085636139
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 2.6188014767342165,
+      "yaw_error_to_plan_end": 0.43310206453971645,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.2523651955998183,
+        "angular_z": 0.8302455735493561
+      },
+      "ackermann_cmd": {
+        "speed": -0.25236520171165466,
+        "steering_angle": -0.43972912430763245
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 8.757785888001308,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 1.1410890221595764,
+        "y": 0.23481179773807526,
+        "yaw": 0.5373430252075195,
+        "odom_age_sec": 0.028490605000000002,
+        "odom_linear_x": 0.24199999868869781,
+        "odom_angular_z": 0.10378549993038177
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 2.5953838218365783,
+      "yaw_error_to_plan_end": 0.42265450279884137,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.2532456660727045,
+        "angular_z": 0.8065975137797035
+      },
+      "ackermann_cmd": {
+        "speed": -0.25324565172195435,
+        "steering_angle": -0.42738574743270874
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 8.859362043000147,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 1.150816023349762,
+        "y": 0.24049486219882965,
+        "yaw": 0.5430442094802856,
+        "odom_age_sec": 0.07925537,
+        "odom_linear_x": 0.22200000286102295,
+        "odom_angular_z": 0.11513406038284302
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 2.5854167050949437,
+      "yaw_error_to_plan_end": 0.41695331852607515,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.6332402454057658,
+        "angular_z": 2.013093494403882
+      },
+      "ackermann_cmd": {
+        "speed": 0.6332402229309082,
+        "steering_angle": 0.4266744554042816
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 8.959096452001177,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 1.1539453864097595,
+        "y": 0.24234701693058014,
+        "yaw": 0.5491393208503723,
+        "odom_age_sec": 0.028801354,
+        "odom_linear_x": 0.07100000232458115,
+        "odom_angular_z": 0.12194320559501648
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 2.5821921378129455,
+      "yaw_error_to_plan_end": 0.41085820715598853,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.6346991100702107,
+        "angular_z": 1.9736388916776206
+      },
+      "ackermann_cmd": {
+        "speed": 0.6346991062164307,
+        "steering_angle": 0.4184109568595886
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 9.057928905000153,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 1.1584965586662292,
+        "y": 0.2450963407754898,
+        "yaw": 0.5586323142051697,
+        "odom_age_sec": 0.028554063,
+        "odom_linear_x": 0.057999998331069946,
+        "odom_angular_z": 0.0823325663805008
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 2.5774571507220063,
+      "yaw_error_to_plan_end": 0.4013652138011911,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.632799642443482,
+        "angular_z": 2.024969630182797
+      },
+      "ackermann_cmd": {
+        "speed": 0.6327996253967285,
+        "steering_angle": 0.429158091545105
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 9.158021105000444,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 1.164052426815033,
+        "y": 0.24850161373615265,
+        "yaw": 0.5619882345199585,
+        "odom_age_sec": 0.028634771000000003,
+        "odom_linear_x": 0.12999999523162842,
+        "odom_angular_z": 0.06858428567647934
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 2.5716382707986645,
+      "yaw_error_to_plan_end": 0.39800929348640235,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.6331220529962298,
+        "angular_z": 2.0162811003428573
+      },
+      "ackermann_cmd": {
+        "speed": 0.6331220269203186,
+        "steering_angle": 0.4273412525653839
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 9.259110636001424,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 1.1815305352210999,
+        "y": 0.25933580100536346,
+        "yaw": 0.5682182908058168,
+        "odom_age_sec": 0.029118145,
+        "odom_linear_x": 0.22200000286102295,
+        "odom_angular_z": 0.061844438314437866
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 2.553251057203455,
+      "yaw_error_to_plan_end": 0.39177923720054403,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.6345191093654637,
+        "angular_z": 1.97851792408968
+      },
+      "ackermann_cmd": {
+        "speed": 0.634519100189209,
+        "steering_angle": 0.4194338619709015
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 9.35774900600154,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 1.2072697281837463,
+        "y": 0.27551598846912384,
+        "yaw": 0.5751944184303285,
+        "odom_age_sec": 0.028445147,
+        "odom_linear_x": 0.3199999928474426,
+        "odom_angular_z": 0.07080041617155075
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 2.5260415428871745,
+      "yaw_error_to_plan_end": 0.38480310957603236,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.2546971144359797,
+        "angular_z": 0.7672109452452125
+      },
+      "ackermann_cmd": {
+        "speed": -0.25469711422920227,
+        "steering_angle": -0.40673211216926575
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 9.457893123000758,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 1.2225331664085388,
+        "y": 0.2852194458246231,
+        "yaw": 0.5795596837997438,
+        "odom_age_sec": 0.07853887200000001,
+        "odom_linear_x": 0.35899999737739563,
+        "odom_angular_z": 0.08873529732227325
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 2.5098477868305356,
+      "yaw_error_to_plan_end": 0.3804378442066171,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.6370365229671947,
+        "angular_z": 1.9100016420430057
+      },
+      "ackermann_cmd": {
+        "speed": 0.6370365023612976,
+        "steering_angle": 0.40504372119903564
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 9.559063779001008,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 1.2596483826637268,
+        "y": 0.3092048019170761,
+        "yaw": 0.5932450294494629,
+        "odom_age_sec": 0.028894646000000003,
+        "odom_linear_x": 0.43799999356269836,
+        "odom_angular_z": 0.14937184751033783
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 2.470244118759988,
+      "yaw_error_to_plan_end": 0.36675249855689795,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.2554535551856273,
+        "angular_z": 0.746485308928491
+      },
+      "ackermann_cmd": {
+        "speed": -0.25545355677604675,
+        "steering_angle": -0.39581942558288574
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 9.658039065001503,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 1.2939310669898987,
+        "y": 0.33206866681575775,
+        "yaw": 0.6101848483085632,
+        "odom_age_sec": 0.028756688000000002,
+        "odom_linear_x": 0.3919999897480011,
+        "odom_angular_z": 0.1738227754831314
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 2.433190054429464,
+      "yaw_error_to_plan_end": 0.3498126796977976,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.6390212496342237,
+        "angular_z": 1.855557964755798
+      },
+      "ackermann_cmd": {
+        "speed": 0.6390212774276733,
+        "steering_angle": 0.393571674823761
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 9.75779489100023,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 1.3202943205833435,
+        "y": 0.3503132313489914,
+        "yaw": 0.6326298713684083,
+        "odom_age_sec": 0.028477980000000003,
+        "odom_linear_x": 0.27399998903274536,
+        "odom_angular_z": 0.24802014231681824
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 2.4042128829286646,
+      "yaw_error_to_plan_end": 0.32736765663795253,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.6398133819464976,
+        "angular_z": 1.8337239830825445
+      },
+      "ackermann_cmd": {
+        "speed": 0.6398133635520935,
+        "steering_angle": 0.38896214962005615
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 9.857803841001441,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 1.3389690518379211,
+        "y": 0.3639264553785324,
+        "yaw": 0.6600939631462097,
+        "odom_age_sec": 0.028610563000000002,
+        "odom_linear_x": 0.2150000035762787,
+        "odom_angular_z": 0.27059605717658997
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 2.3831347583437243,
+      "yaw_error_to_plan_end": 0.29990356486015113,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.2561634387912088,
+        "angular_z": 0.7269113676429871
+      },
+      "ackermann_cmd": {
+        "speed": -0.25616344809532166,
+        "steering_angle": -0.38548699021339417
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 9.957676333000563,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 1.35262793302536,
+        "y": 0.3744528144598007,
+        "yaw": 0.6831703782081604,
+        "odom_age_sec": 0.028358605000000002,
+        "odom_linear_x": 0.16300000250339508,
+        "odom_angular_z": 0.21320044994354248
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 2.367245976539977,
+      "yaw_error_to_plan_end": 0.27682714979820044,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.6400305083057068,
+        "angular_z": 1.8277287739015857
+      },
+      "ackermann_cmd": {
+        "speed": 0.6400305032730103,
+        "steering_angle": 0.3876955807209015
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 10.058055283001522,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 1.3663857579231262,
+        "y": 0.3855297416448593,
+        "yaw": 0.697831928730011,
+        "odom_age_sec": 0.028717188,
+        "odom_linear_x": 0.17599999904632568,
+        "odom_angular_z": 0.12435071170330048
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 2.3508476372857694,
+      "yaw_error_to_plan_end": 0.26216559927634986,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.6398915379767649,
+        "angular_z": 1.8315664861650403
+      },
+      "ackermann_cmd": {
+        "speed": 0.6398915648460388,
+        "steering_angle": 0.38850638270378113
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 10.157661192000887,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 1.38181871175766,
+        "y": 0.3982875794172287,
+        "yaw": 0.7068073153495789,
+        "odom_age_sec": 0.028355147,
+        "odom_linear_x": 0.20900000631809235,
+        "odom_angular_z": 0.08196298778057098
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 2.3321843416186416,
+      "yaw_error_to_plan_end": 0.25319021265678193,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.25624686834754556,
+        "angular_z": 0.7246030475462408
+      },
+      "ackermann_cmd": {
+        "speed": -0.2562468647956848,
+        "steering_angle": -0.3842668831348419
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 10.257781976000842,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 1.4004713892936707,
+        "y": 0.41397814452648163,
+        "yaw": 0.7151825428009032,
+        "odom_age_sec": 0.028609188,
+        "odom_linear_x": 0.2549999952316284,
+        "odom_angular_z": 0.08407530188560486
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 2.3094203969239637,
+      "yaw_error_to_plan_end": 0.2448149852054576,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.6408790442299168,
+        "angular_z": 1.804256267994767
+      },
+      "ackermann_cmd": {
+        "speed": 0.6408790349960327,
+        "steering_angle": 0.3827333152294159
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 10.357718885001304,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 1.4245005249977112,
+        "y": 0.43453215062618256,
+        "yaw": 0.7231596708297731,
+        "odom_age_sec": 0.028497605000000002,
+        "odom_linear_x": 0.3269999921321869,
+        "odom_angular_z": 0.07903799414634705
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 2.2798448200171766,
+      "yaw_error_to_plan_end": 0.23683785717658778,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.6414397179393317,
+        "angular_z": 1.7887090860687007
+      },
+      "ackermann_cmd": {
+        "speed": 0.6414397358894348,
+        "steering_angle": 0.37944355607032776
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 10.457592501999898,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 1.4518643021583557,
+        "y": 0.45831017196178436,
+        "yaw": 0.7308321595191954,
+        "odom_age_sec": 0.028394897000000002,
+        "odom_linear_x": 0.3529999852180481,
+        "odom_angular_z": 0.07466673851013184
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 2.2459054467800974,
+      "yaw_error_to_plan_end": 0.2291653684871654,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.2568860609634192,
+        "angular_z": 0.7068629467209352
+      },
+      "ackermann_cmd": {
+        "speed": -0.2568860650062561,
+        "steering_angle": -0.374879390001297
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 10.559042157001386,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 1.466864287853241,
+        "y": 0.47149525582790375,
+        "yaw": 0.7343968749046327,
+        "odom_age_sec": 0.029055188000000003,
+        "odom_linear_x": 0.39899998903274536,
+        "odom_angular_z": 0.07272402197122574
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 2.227198895733517,
+      "yaw_error_to_plan_end": 0.22560065310172817,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.2571451094602015,
+        "angular_z": 0.6996456344711207
+      },
+      "ackermann_cmd": {
+        "speed": -0.25714510679244995,
+        "steering_angle": -0.3710549473762512
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 10.657858860000488,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 1.4970754981040955,
+        "y": 0.4984007328748703,
+        "yaw": 0.7477238774299622,
+        "odom_age_sec": 0.028574938,
+        "odom_linear_x": 0.38600000739097595,
+        "odom_angular_z": 0.14588850736618042
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 2.1892806797695132,
+      "yaw_error_to_plan_end": 0.2122736505763987,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.2572523847488218,
+        "angular_z": 0.6966521750850327
+      },
+      "ackermann_cmd": {
+        "speed": -0.25725239515304565,
+        "steering_angle": -0.3694678246974945
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 10.758064727000601,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 1.5211734175682068,
+        "y": 0.5204863995313644,
+        "yaw": 0.7652971148490907,
+        "odom_age_sec": 0.028320564000000003,
+        "odom_linear_x": 0.3070000112056732,
+        "odom_angular_z": 0.18872570991516113
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 2.158546975231939,
+      "yaw_error_to_plan_end": 0.19470041315727016,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.2574165125788243,
+        "angular_z": 0.6920669730746192
+      },
+      "ackermann_cmd": {
+        "speed": -0.2574165165424347,
+        "steering_angle": -0.367035835981369
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 10.858463843000209,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 1.5429708361625671,
+        "y": 0.5412293523550034,
+        "yaw": 0.785988450050354,
+        "odom_age_sec": 0.029032687,
+        "odom_linear_x": 0.2809999883174896,
+        "odom_angular_z": 0.19913801550865173
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 2.1301161439550604,
+      "yaw_error_to_plan_end": 0.17400907795600684,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.25749048071221065,
+        "angular_z": 0.6899984454778183
+      },
+      "ackermann_cmd": {
+        "speed": -0.25749048590660095,
+        "steering_angle": -0.36593830585479736
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 10.957998669000517,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 1.5623049139976501,
+        "y": 0.5603633970022202,
+        "yaw": 0.8010216355323793,
+        "odom_age_sec": 0.028711563000000002,
+        "odom_linear_x": 0.27399998903274536,
+        "odom_angular_z": 0.1443336009979248
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 2.1042814121849487,
+      "yaw_error_to_plan_end": 0.15897589247398158,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.2575547389135015,
+        "angular_z": 0.6882003998783938
+      },
+      "ackermann_cmd": {
+        "speed": -0.2575547397136688,
+        "steering_angle": -0.3649841248989105
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 11.05798078600128,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 1.5809428095817566,
+        "y": 0.5793431252241135,
+        "yaw": 0.8141196966171265,
+        "odom_age_sec": 0.028642813000000003,
+        "odom_linear_x": 0.26100000739097595,
+        "odom_angular_z": 0.13125436007976532
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 2.078932362281241,
+      "yaw_error_to_plan_end": 0.14587783138923438,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.6440710038292272,
+        "angular_z": 1.7153449860414585
+      },
+      "ackermann_cmd": {
+        "speed": 0.6440709829330444,
+        "steering_angle": 0.3638893961906433
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 11.158037028000763,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 1.6017171740531921,
+        "y": 0.6010513156652451,
+        "yaw": 0.8255445361137391,
+        "odom_age_sec": 0.028636605000000002,
+        "odom_linear_x": 0.3009999990463257,
+        "odom_angular_z": 0.10925919562578201
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 2.0502200665194543,
+      "yaw_error_to_plan_end": 0.13445299189262172,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.6443061986289599,
+        "angular_z": 1.708755307074818
+      },
+      "ackermann_cmd": {
+        "speed": 0.6443061828613281,
+        "steering_angle": 0.3624899685382843
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 11.257752146000712,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 1.6213106513023376,
+        "y": 0.6219760626554489,
+        "yaw": 0.835984170436859,
+        "odom_age_sec": 0.028494230000000002,
+        "odom_linear_x": 0.2680000066757202,
+        "odom_angular_z": 0.10485794395208359
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 2.0227713019002285,
+      "yaw_error_to_plan_end": 0.12401335756950183,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.6445590764994323,
+        "angular_z": 1.7016643072286455
+      },
+      "ackermann_cmd": {
+        "speed": 0.6445590853691101,
+        "steering_angle": 0.360983669757843
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 11.35913155200069,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 1.6381234526634216,
+        "y": 0.6403329819440842,
+        "yaw": 0.849402129650116,
+        "odom_age_sec": 0.029166145,
+        "odom_linear_x": 0.23499999940395355,
+        "odom_angular_z": 0.14142769575119019
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 1.9988842740588126,
+      "yaw_error_to_plan_end": 0.11059539835624486,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.6446769673548666,
+        "angular_z": 1.6983564243113756
+      },
+      "ackermann_cmd": {
+        "speed": 0.6446769833564758,
+        "steering_angle": 0.360280841588974
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 11.45791025500148,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 1.6490105986595154,
+        "y": 0.6525446623563766,
+        "yaw": 0.8629304170608519,
+        "odom_age_sec": 0.02874248,
+        "odom_linear_x": 0.13699999451637268,
+        "odom_angular_z": 0.12990981340408325
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 1.9831374029271363,
+      "yaw_error_to_plan_end": 0.0970671109455089,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.25795356893571264,
+        "angular_z": 0.6770185208723917
+      },
+      "ackermann_cmd": {
+        "speed": -0.25795355439186096,
+        "steering_angle": -0.3590461313724518
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 11.557674580000821,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 1.6539830565452576,
+        "y": 0.6582665890455246,
+        "yaw": 0.8743244409561157,
+        "odom_age_sec": 0.02854423,
+        "odom_linear_x": 0.052000001072883606,
+        "odom_angular_z": 0.10532209277153015
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 1.9758186425939475,
+      "yaw_error_to_plan_end": 0.08567308705024512,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.6447489251901976,
+        "angular_z": 1.6963367190106329
+      },
+      "ackermann_cmd": {
+        "speed": 0.6447489261627197,
+        "steering_angle": 0.3598516583442688
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 11.658110738000687,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 1.6561259627342224,
+        "y": 0.6607929319143295,
+        "yaw": 0.8821118474006653,
+        "odom_age_sec": 0.028755521000000003,
+        "odom_linear_x": 0.039000000804662704,
+        "odom_angular_z": 0.06692764908075333
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 1.9726107875478553,
+      "yaw_error_to_plan_end": 0.07788568060569556,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.64472827265678,
+        "angular_z": 1.6969164427024137
+      },
+      "ackermann_cmd": {
+        "speed": 0.6447282433509827,
+        "steering_angle": 0.35997486114501953
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 11.757854022000174,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 1.6622790694236755,
+        "y": 0.6681495159864426,
+        "yaw": 0.887054741382599,
+        "odom_age_sec": 0.028561688000000002,
+        "odom_linear_x": 0.12399999797344208,
+        "odom_angular_z": 0.04498455673456192
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 1.963309446473908,
+      "yaw_error_to_plan_end": 0.07294278662376186,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.6447833959674362,
+        "angular_z": 1.6953690219955095
+      },
+      "ackermann_cmd": {
+        "speed": 0.6447833776473999,
+        "steering_angle": 0.3596460223197937
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 11.857752223000716,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 1.6748177409172058,
+        "y": 0.6832777708768845,
+        "yaw": 0.8916792869567872,
+        "odom_age_sec": 0.028444146000000003,
+        "odom_linear_x": 0.2150000035762787,
+        "odom_angular_z": 0.046378470957279205
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 1.944240399442914,
+      "yaw_error_to_plan_end": 0.06831824104957362,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.25799996394909114,
+        "angular_z": 0.6757152985730002
+      },
+      "ackermann_cmd": {
+        "speed": -0.2579999566078186,
+        "steering_angle": -0.3583536744117737
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 11.95796633900136,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 1.6921407580375671,
+        "y": 0.7043800204992294,
+        "yaw": 0.8972359895706177,
+        "odom_age_sec": 0.028610938000000002,
+        "odom_linear_x": 0.27399998903274536,
+        "odom_angular_z": 0.058283910155296326
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 1.9177324735470582,
+      "yaw_error_to_plan_end": 0.06276153843574317,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.25804779372143694,
+        "angular_z": 0.6743712377977479
+      },
+      "ackermann_cmd": {
+        "speed": -0.25804778933525085,
+        "steering_angle": -0.3576394021511078
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 12.057789207001406,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 1.7103108763694763,
+        "y": 0.7267980426549911,
+        "yaw": 0.9062824249267578,
+        "odom_age_sec": 0.028610063,
+        "odom_linear_x": 0.2809999883174896,
+        "odom_angular_z": 0.09961569309234619
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 1.8896957984259366,
+      "yaw_error_to_plan_end": 0.05371510307960303,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.2580932816718917,
+        "angular_z": 0.6730924790432451
+      },
+      "ackermann_cmd": {
+        "speed": -0.2580932676792145,
+        "steering_angle": -0.356959730386734
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 12.158065990000978,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 1.7295884490013123,
+        "y": 0.7510725110769272,
+        "yaw": 0.9179330468177794,
+        "odom_age_sec": 0.028760397000000004,
+        "odom_linear_x": 0.3070000112056732,
+        "odom_angular_z": 0.1188434362411499
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 1.8595335965597906,
+      "yaw_error_to_plan_end": 0.042064481188581415,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.2581280580574268,
+        "angular_z": 0.6721145118549069
+      },
+      "ackermann_cmd": {
+        "speed": -0.2581280469894409,
+        "steering_angle": -0.35643988847732544
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 12.257956815001307,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 1.7487494349479675,
+        "y": 0.7757838815450668,
+        "yaw": 0.9285212755203247,
+        "odom_age_sec": 0.028729854000000003,
+        "odom_linear_x": 0.3140000104904175,
+        "odom_angular_z": 0.10297946631908417
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 1.829051572806296,
+      "yaw_error_to_plan_end": 0.03147625248603614,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.2581593603846952,
+        "angular_z": 0.6712339944618718
+      },
+      "ackermann_cmd": {
+        "speed": -0.2581593692302704,
+        "steering_angle": -0.3559718132019043
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 12.357637308001358,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 1.757518470287323,
+        "y": 0.7872783988714218,
+        "yaw": 0.9344502091407775,
+        "odom_age_sec": 0.028441521,
+        "odom_linear_x": 0.2939999997615814,
+        "odom_angular_z": 0.12264364957809448
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 1.8149419995393932,
+      "yaw_error_to_plan_end": 0.02554731886558337,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.6454061530149766,
+        "angular_z": 1.6778668933758183
+      },
+      "ackermann_cmd": {
+        "speed": 0.6454061269760132,
+        "steering_angle": 0.35592544078826904
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 12.45765800800109,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 1.772601306438446,
+        "y": 0.807429239153862,
+        "yaw": 0.9488120079040527,
+        "odom_age_sec": 0.028502689,
+        "odom_linear_x": 0.2290000021457672,
+        "odom_angular_z": 0.14957840740680695
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 1.790340509272459,
+      "yaw_error_to_plan_end": 0.01118552010230811,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.6454310598208136,
+        "angular_z": 1.6771661377690847
+      },
+      "ackermann_cmd": {
+        "speed": 0.6454310417175293,
+        "steering_angle": 0.35577642917633057
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 12.559138831000382,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 1.7844762206077576,
+        "y": 0.8237969130277634,
+        "yaw": 0.9642009139060973,
+        "odom_age_sec": 0.029155687000000003,
+        "odom_linear_x": 0.18299999833106995,
+        "odom_angular_z": 0.14974702894687653
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 1.7705248957762543,
+      "yaw_error_to_plan_end": 0.004203385899736456,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.6454749774618104,
+        "angular_z": 1.6759303663782203
+      },
+      "ackermann_cmd": {
+        "speed": 0.6454749703407288,
+        "steering_angle": 0.3555136024951935
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 12.658151283001644,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 1.7919129729270935,
+        "y": 0.8343726843595505,
+        "yaw": 0.9768401980400085,
+        "odom_age_sec": 0.028776438,
+        "odom_linear_x": 0.11100000143051147,
+        "odom_angular_z": 0.11800544708967209
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 1.757823398776871,
+      "yaw_error_to_plan_end": 0.0168426700336477,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.6454668495090634,
+        "angular_z": 1.6761590876756411
+      },
+      "ackermann_cmd": {
+        "speed": 0.6454668641090393,
+        "steering_angle": 0.3555622696876526
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 12.758054900001298,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 1.7995726466178894,
+        "y": 0.8455596417188644,
+        "yaw": 0.9857874512672424,
+        "odom_age_sec": 0.02868423,
+        "odom_linear_x": 0.14300000667572021,
+        "odom_angular_z": 0.07878345251083374
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 1.7444764151490835,
+      "yaw_error_to_plan_end": 0.025789923260881586,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.25818422533244456,
+        "angular_z": 0.6705343907821395
+      },
+      "ackermann_cmd": {
+        "speed": -0.2581842243671417,
+        "steering_angle": -0.35559988021850586
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 12.85778372599998,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 1.8065921664237976,
+        "y": 0.8559869974851608,
+        "yaw": 0.9924561381340027,
+        "odom_age_sec": 0.02859998,
+        "odom_linear_x": 0.11699999868869781,
+        "odom_angular_z": 0.06726399064064026
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 1.7320878026979767,
+      "yaw_error_to_plan_end": 0.03245861012764184,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.6454755114647347,
+        "angular_z": 1.6759153392692097
+      },
+      "ackermann_cmd": {
+        "speed": 0.6454755067825317,
+        "steering_angle": 0.355510413646698
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 12.95880546600165,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 1.814865529537201,
+        "y": 0.8684645146131516,
+        "yaw": 0.9994024634361267,
+        "odom_age_sec": 0.028887396000000003,
+        "odom_linear_x": 0.15700000524520874,
+        "odom_angular_z": 0.07027889043092728
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 1.7173184217562587,
+      "yaw_error_to_plan_end": 0.03940493542976586,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.25817373879240574,
+        "angular_z": 0.6708294595028884
+      },
+      "ackermann_cmd": {
+        "speed": -0.25817373394966125,
+        "steering_angle": -0.3557567596435547
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 13.059405123000033,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 1.8229815363883972,
+        "y": 0.8808901757001877,
+        "yaw": 1.0062315464019775,
+        "odom_age_sec": 0.029209687,
+        "odom_linear_x": 0.14300000667572021,
+        "odom_angular_z": 0.0654812604188919
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 1.7026639561826795,
+      "yaw_error_to_plan_end": 0.046234018395616694,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.6454523065192931,
+        "angular_z": 1.6765683129728388
+      },
+      "ackermann_cmd": {
+        "speed": 0.6454523205757141,
+        "steering_angle": 0.35564929246902466
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 13.158058701999835,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 1.8322784304618835,
+        "y": 0.8953358381986618,
+        "yaw": 1.0123226642608643,
+        "odom_age_sec": 0.028732855,
+        "odom_linear_x": 0.17599999904632568,
+        "odom_angular_z": 0.0600452683866024
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 1.6856874250959222,
+      "yaw_error_to_plan_end": 0.052325136254503406,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.25814909709021333,
+        "angular_z": 0.6715227210437317
+      },
+      "ackermann_cmd": {
+        "speed": -0.25814908742904663,
+        "steering_angle": -0.3561252951622009
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 13.257850861000406,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 1.8415438532829285,
+        "y": 0.9099208563566208,
+        "yaw": 1.0181859731674194,
+        "odom_age_sec": 0.028729396,
+        "odom_linear_x": 0.15700000524520874,
+        "odom_angular_z": 0.05828014761209488
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 1.6686002738587005,
+      "yaw_error_to_plan_end": 0.05818844516105859,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.6453593583469343,
+        "angular_z": 1.679183306518442
+      },
+      "ackermann_cmd": {
+        "speed": 0.6453593373298645,
+        "steering_angle": 0.35620537400245667
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 13.358193227000811,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 1.8487655520439148,
+        "y": 0.9214462488889694,
+        "yaw": 1.0248149633407593,
+        "odom_age_sec": 0.028786271000000002,
+        "odom_linear_x": 0.13699999451637268,
+        "odom_angular_z": 0.06787599623203278
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 1.655140773179326,
+      "yaw_error_to_plan_end": 0.06481743533439843,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.2581185319572877,
+        "angular_z": 0.6723824295541919
+      },
+      "ackermann_cmd": {
+        "speed": -0.25811854004859924,
+        "steering_angle": -0.35658231377601624
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 13.457545844999913,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 1.8543035387992859,
+        "y": 0.9304098337888718,
+        "yaw": 1.0303845405578613,
+        "odom_age_sec": 0.028333063000000002,
+        "odom_linear_x": 0.09099999815225601,
+        "odom_angular_z": 0.05308469384908676
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 1.644706165968429,
+      "yaw_error_to_plan_end": 0.07038701255150048,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.6453433811099941,
+        "angular_z": 1.6796327254531807
+      },
+      "ackermann_cmd": {
+        "speed": 0.6453433632850647,
+        "steering_angle": 0.3563009202480316
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 13.558988501001295,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 1.8613888621330261,
+        "y": 0.9420261830091476,
+        "yaw": 1.0352510213851929,
+        "odom_age_sec": 0.029037229,
+        "odom_linear_x": 0.15000000596046448,
+        "odom_angular_z": 0.047418780624866486
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 1.6312221467081665,
+      "yaw_error_to_plan_end": 0.07525349337883203,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.25810350511572483,
+        "angular_z": 0.6728050094499954
+      },
+      "ackermann_cmd": {
+        "speed": -0.25810351967811584,
+        "steering_angle": -0.3568069338798523
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 13.657804745000249,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 1.871717393398285,
+        "y": 0.9591421335935593,
+        "yaw": 1.039624810218811,
+        "odom_age_sec": 0.028536230000000003,
+        "odom_linear_x": 0.20900000631809235,
+        "odom_angular_z": 0.04079628363251686
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 1.6114025179856661,
+      "yaw_error_to_plan_end": 0.07962728221245019,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.26126920790947766,
+        "angular_z": 0.582592763453319
+      },
+      "ackermann_cmd": {
+        "speed": -0.261269211769104,
+        "steering_angle": -0.3086770474910736
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 13.757758821000607,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 1.8818339705467224,
+        "y": 0.9760703295469284,
+        "yaw": 1.0449274778366089,
+        "odom_age_sec": 0.028523813000000002,
+        "odom_linear_x": 0.18299999833106995,
+        "odom_angular_z": 0.05930107831954956
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 1.5918437990197598,
+      "yaw_error_to_plan_end": 0.08492994983024804,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.6530078133493782,
+        "angular_z": 1.4612507166299242
+      },
+      "ackermann_cmd": {
+        "speed": 0.6530078053474426,
+        "steering_angle": 0.30969786643981934
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 13.857794438001292,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 1.8906187415122986,
+        "y": 0.9909755438566208,
+        "yaw": 1.0526832342147825,
+        "odom_age_sec": 0.028518813,
+        "odom_linear_x": 0.17000000178813934,
+        "odom_angular_z": 0.08154422044754028
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 1.574674527660954,
+      "yaw_error_to_plan_end": 0.09268570620842165,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.26086334508673503,
+        "angular_z": 0.5942918826720013
+      },
+      "ackermann_cmd": {
+        "speed": -0.260863333940506,
+        "steering_angle": -0.314936101436615
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 13.958112762000383,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 1.8977155089378357,
+        "y": 1.0032389611005783,
+        "yaw": 1.0610198974609375,
+        "odom_age_sec": 0.028834437,
+        "odom_linear_x": 0.12999999523162842,
+        "odom_angular_z": 0.08215538412332535
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 1.5606023944421394,
+      "yaw_error_to_plan_end": 0.10102236945457664,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.6526197726817141,
+        "angular_z": 1.4724415800984583
+      },
+      "ackermann_cmd": {
+        "speed": 0.6526197791099548,
+        "steering_angle": 0.31209295988082886
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 14.058051088000866,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 1.9059023261070251,
+        "y": 1.0176684111356735,
+        "yaw": 1.0688713788986206,
+        "odom_age_sec": 0.028664230000000002,
+        "odom_linear_x": 0.17000000178813934,
+        "odom_angular_z": 0.07766473293304443
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 1.54411156404591,
+      "yaw_error_to_plan_end": 0.10887385089225976,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.26071606405200654,
+        "angular_z": 0.598527604737127
+      },
+      "ackermann_cmd": {
+        "speed": -0.260716050863266,
+        "steering_angle": -0.3172011375427246
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 14.157972372000586,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 1.9168497920036316,
+        "y": 1.0373207777738571,
+        "yaw": 1.07629656791687,
+        "odom_age_sec": 0.028721396000000003,
+        "odom_linear_x": 0.2290000021457672,
+        "odom_angular_z": 0.0720195323228836
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 1.521734901259916,
+      "yaw_error_to_plan_end": 0.11629903991050905,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.6517150897853464,
+        "angular_z": 1.4984764093247769
+      },
+      "ackermann_cmd": {
+        "speed": 0.651715099811554,
+        "steering_angle": 0.3176625370979309
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 14.258271738000985,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 1.9292415976524353,
+        "y": 1.0599424093961716,
+        "yaw": 1.0825163125991821,
+        "odom_age_sec": 0.028913271,
+        "odom_linear_x": 0.26100000739097595,
+        "odom_angular_z": 0.06308113038539886
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 1.4960639873222283,
+      "yaw_error_to_plan_end": 0.1225187845928213,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.25997924423840657,
+        "angular_z": 0.6196405895215233
+      },
+      "ackermann_cmd": {
+        "speed": -0.259979248046875,
+        "steering_angle": -0.32848188281059265
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 14.357819106000534,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 1.9402422308921814,
+        "y": 1.0803297013044357,
+        "yaw": 1.0897290706634524,
+        "odom_age_sec": 0.028648188,
+        "odom_linear_x": 0.22200000286102295,
+        "odom_angular_z": 0.07798123359680176
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 1.4729975863494038,
+      "yaw_error_to_plan_end": 0.12973154265709153,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.25984578271689157,
+        "angular_z": 0.6234509958304305
+      },
+      "ackermann_cmd": {
+        "speed": -0.2598457932472229,
+        "steering_angle": -0.3305160403251648
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 14.457642598001257,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 1.948779284954071,
+        "y": 1.0964409559965134,
+        "yaw": 1.0978307723999023,
+        "odom_age_sec": 0.028535647,
+        "odom_linear_x": 0.17000000178813934,
+        "odom_angular_z": 0.08272712677717209
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 1.4548322821915831,
+      "yaw_error_to_plan_end": 0.13783324439354153,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.6499255935668921,
+        "angular_z": 1.5497446820736889
+      },
+      "ackermann_cmd": {
+        "speed": 0.6499255895614624,
+        "steering_angle": 0.3286192715167999
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 14.55789171500146,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 1.9570128321647644,
+        "y": 1.112301990389824,
+        "yaw": 1.106136441230774,
+        "odom_age_sec": 0.028731271000000003,
+        "odom_linear_x": 0.18299999833106995,
+        "odom_angular_z": 0.08443564921617508
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 1.4370172308928735,
+      "yaw_error_to_plan_end": 0.14613891322441308,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.25951406499295476,
+        "angular_z": 0.6329033653404522
+      },
+      "ackermann_cmd": {
+        "speed": -0.25951406359672546,
+        "steering_angle": -0.335559606552124
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 14.657680082000297,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 1.9673911929130554,
+        "y": 1.1327060908079147,
+        "yaw": 1.1138720512390137,
+        "odom_age_sec": 0.028501147,
+        "odom_linear_x": 0.2290000021457672,
+        "odom_angular_z": 0.07558676600456238
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 1.4141842724500293,
+      "yaw_error_to_plan_end": 0.15387452323265283,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.6486929821757853,
+        "angular_z": 1.5848810666159983
+      },
+      "ackermann_cmd": {
+        "speed": 0.6486929655075073,
+        "steering_angle": 0.3361191153526306
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 14.758978863001175,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 1.9769975543022156,
+        "y": 1.1519520729780197,
+        "yaw": 1.1212620735168455,
+        "odom_age_sec": 0.028989104,
+        "odom_linear_x": 0.20200000703334808,
+        "odom_angular_z": 0.07410638779401779
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 1.3927198202010695,
+      "yaw_error_to_plan_end": 0.16126454551048464,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.2587163575419578,
+        "angular_z": 0.6555269585678624
+      },
+      "ackermann_cmd": {
+        "speed": -0.258716344833374,
+        "steering_angle": -0.3476155400276184
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 14.85776185800023,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 1.98381108045578,
+        "y": 1.165866419672966,
+        "yaw": 1.1297491788864138,
+        "odom_age_sec": 0.028511814000000003,
+        "odom_linear_x": 0.14300000667572021,
+        "odom_angular_z": 0.0900714173913002
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 1.3772532995762972,
+      "yaw_error_to_plan_end": 0.16975165088005295,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.6474158215413454,
+        "angular_z": 1.621134718899362
+      },
+      "ackermann_cmd": {
+        "speed": 0.647415816783905,
+        "steering_angle": 0.3438488245010376
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 14.95918801300104,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 1.9906763434410095,
+        "y": 1.1802242249250412,
+        "yaw": 1.139436960220337,
+        "odom_age_sec": 0.029252645,
+        "odom_linear_x": 0.17000000178813934,
+        "odom_angular_z": 0.0969059020280838
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 1.3613582753787457,
+      "yaw_error_to_plan_end": 0.17943943221397607,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.2585712034380001,
+        "angular_z": 0.6596273470552853
+      },
+      "ackermann_cmd": {
+        "speed": -0.25857120752334595,
+        "steering_angle": -0.34979814291000366
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 15.057947717001298,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 1.9982017874717712,
+        "y": 1.1963594406843185,
+        "yaw": 1.1488821506500244,
+        "odom_age_sec": 0.028659563000000002,
+        "odom_linear_x": 0.17599999904632568,
+        "odom_angular_z": 0.09241755306720734
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 1.3435691605042674,
+      "yaw_error_to_plan_end": 0.18888462264366354,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.6468590269229106,
+        "angular_z": 1.636891339732506
+      },
+      "ackermann_cmd": {
+        "speed": 0.6468590497970581,
+        "steering_angle": 0.34720537066459656
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 15.157825542000865,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.005880296230316,
+        "y": 1.213225170969963,
+        "yaw": 1.1568760871887207,
+        "odom_age_sec": 0.028580688000000003,
+        "odom_linear_x": 0.1889999955892563,
+        "odom_angular_z": 0.08127385377883911
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 1.325047269754376,
+      "yaw_error_to_plan_end": 0.19687855918235986,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.2580805487218756,
+        "angular_z": 0.6734504778024734
+      },
+      "ackermann_cmd": {
+        "speed": -0.25808054208755493,
+        "steering_angle": -0.35715001821517944
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 15.258056909000516,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.0136772990226746,
+        "y": 1.2307336777448654,
+        "yaw": 1.1663491725921633,
+        "odom_age_sec": 0.028778230000000002,
+        "odom_linear_x": 0.18299999833106995,
+        "odom_angular_z": 0.09853469580411911
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 1.3058867132031524,
+      "yaw_error_to_plan_end": 0.20635164458580246,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.6455168246173597,
+        "angular_z": 1.6747526841939369
+      },
+      "ackermann_cmd": {
+        "speed": 0.6455168128013611,
+        "steering_angle": 0.3552631437778473
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 15.357710651000161,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.021010100841522,
+        "y": 1.2476560324430466,
+        "yaw": 1.1766910552978516,
+        "odom_age_sec": 0.028438272,
+        "odom_linear_x": 0.1889999955892563,
+        "odom_angular_z": 0.10453544557094574
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 1.2874458366547716,
+      "yaw_error_to_plan_end": 0.21669352729149072,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.25756357630947585,
+        "angular_z": 0.6879530387292919
+      },
+      "ackermann_cmd": {
+        "speed": -0.25756359100341797,
+        "steering_angle": -0.36485281586647034
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 15.457618477001233,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.030166804790497,
+        "y": 1.2694106549024582,
+        "yaw": 1.1865515708923338,
+        "odom_age_sec": 0.028501772,
+        "odom_linear_x": 0.24199999868869781,
+        "odom_angular_z": 0.09461000561714172
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 1.2638428729125177,
+      "yaw_error_to_plan_end": 0.22655404288597292,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.644013150920379,
+        "angular_z": 1.7169650993271601
+      },
+      "ackermann_cmd": {
+        "speed": 0.6440131664276123,
+        "steering_angle": 0.3642334043979645
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 15.557896677000826,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.039672553539276,
+        "y": 1.292616292834282,
+        "yaw": 1.1953428983688352,
+        "odom_age_sec": 0.02860073,
+        "odom_linear_x": 0.2549999952316284,
+        "odom_angular_z": 0.08675248920917511
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 1.2387661720269265,
+      "yaw_error_to_plan_end": 0.2353453703624744,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.2563903935592692,
+        "angular_z": 0.7206281294188619
+      },
+      "ackermann_cmd": {
+        "speed": -0.25639039278030396,
+        "steering_angle": -0.3821651339530945
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 15.657684294001228,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.047914445400238,
+        "y": 1.3132539242506027,
+        "yaw": 1.205663800239563,
+        "odom_age_sec": 0.028568896000000003,
+        "odom_linear_x": 0.20200000703334808,
+        "odom_angular_z": 0.11056777834892273
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 1.216546031146749,
+      "yaw_error_to_plan_end": 0.24566627223320214,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.640783015517959,
+        "angular_z": 1.8069160898229721
+      },
+      "ackermann_cmd": {
+        "speed": 0.6407830119132996,
+        "steering_angle": 0.3832958936691284
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 15.75781691100019,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.0543381571769714,
+        "y": 1.329870030283928,
+        "yaw": 1.2175143957138062,
+        "odom_age_sec": 0.028595146000000002,
+        "odom_linear_x": 0.17000000178813934,
+        "odom_angular_z": 0.12206146866083145
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 1.198737492435518,
+      "yaw_error_to_plan_end": 0.2575168677074453,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.25554219160047786,
+        "angular_z": 0.7440478523998171
+      },
+      "ackermann_cmd": {
+        "speed": -0.25554218888282776,
+        "steering_angle": -0.3945341110229492
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 15.857778111001608,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.059055745601654,
+        "y": 1.3425195664167404,
+        "yaw": 1.2292598485946653,
+        "odom_age_sec": 0.028683813000000002,
+        "odom_linear_x": 0.11699999868869781,
+        "odom_angular_z": 0.11277061700820923
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 1.1852468217498102,
+      "yaw_error_to_plan_end": 0.26926232058830446,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.6404411307381324,
+        "angular_z": 1.8163785512169082
+      },
+      "ackermann_cmd": {
+        "speed": 0.6404411196708679,
+        "steering_angle": 0.3852967321872711
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 15.957688603000861,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.0622958540916443,
+        "y": 1.3515321463346481,
+        "yaw": 1.2389265298843382,
+        "odom_age_sec": 0.028575063,
+        "odom_linear_x": 0.09799999743700027,
+        "odom_angular_z": 0.09277874231338501
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 1.1756817872953995,
+      "yaw_error_to_plan_end": 0.2789290018779773,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.6397271877054286,
+        "angular_z": 1.8361027002049206
+      },
+      "ackermann_cmd": {
+        "speed": 0.6397271752357483,
+        "steering_angle": 0.3894645571708679
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 16.059092759000123,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.066975772380829,
+        "y": 1.364941045641899,
+        "yaw": 1.2461233139038086,
+        "odom_age_sec": 0.029218645,
+        "odom_linear_x": 0.15000000596046448,
+        "odom_angular_z": 0.06437303870916367
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 1.1615056813308853,
+      "yaw_error_to_plan_end": 0.28612578589744775,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.2551607805006736,
+        "angular_z": 0.7545231707709156
+      },
+      "ackermann_cmd": {
+        "speed": -0.25516077876091003,
+        "steering_angle": -0.40005508065223694
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 16.158076087000154,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.071680247783661,
+        "y": 1.3787070959806442,
+        "yaw": 1.251991629600525,
+        "odom_age_sec": 0.028828021000000002,
+        "odom_linear_x": 0.13699999451637268,
+        "odom_angular_z": 0.06173519790172577
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 1.146991408499926,
+      "yaw_error_to_plan_end": 0.29199410159416406,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.6390601144462164,
+        "angular_z": 1.8544881064923928
+      },
+      "ackermann_cmd": {
+        "speed": 0.6390601396560669,
+        "steering_angle": 0.3933459222316742
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 16.25899974300046,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.075540244579315,
+        "y": 1.3902451246976852,
+        "yaw": 1.2593443393707275,
+        "odom_age_sec": 0.029027438000000003,
+        "odom_linear_x": 0.12399999797344208,
+        "odom_angular_z": 0.07645224779844284
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 1.134859134479055,
+      "yaw_error_to_plan_end": 0.2993468113643667,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.6374491716438972,
+        "angular_z": 1.898713034344632
+      },
+      "ackermann_cmd": {
+        "speed": 0.6374491453170776,
+        "steering_angle": 0.40266770124435425
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 16.35772352999993,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.081288516521454,
+        "y": 1.4078921526670456,
+        "yaw": 1.2668063640594482,
+        "odom_age_sec": 0.028530438,
+        "odom_linear_x": 0.20900000631809235,
+        "odom_angular_z": 0.06906784325838089
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 1.116365167085933,
+      "yaw_error_to_plan_end": 0.30680883605308734,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.2539613938209072,
+        "angular_z": 0.7872381807614405
+      },
+      "ackermann_cmd": {
+        "speed": -0.25396138429641724,
+        "steering_angle": -0.4172484278678894
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 16.4578418960009,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.08869332075119,
+        "y": 1.4311577528715134,
+        "yaw": 1.272446155548096,
+        "odom_age_sec": 0.028674354000000003,
+        "odom_linear_x": 0.24199999868869781,
+        "odom_angular_z": 0.05368427932262421
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 1.0920542713825656,
+      "yaw_error_to_plan_end": 0.3124486275417351,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.6340736709667899,
+        "angular_z": 1.9905785458786642
+      },
+      "ackermann_cmd": {
+        "speed": 0.6340736746788025,
+        "steering_angle": 0.4219611883163452
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 16.558993968999857,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.095792233943939,
+        "y": 1.4539577215909958,
+        "yaw": 1.2813570499420166,
+        "odom_age_sec": 0.029054062000000002,
+        "odom_linear_x": 0.24199999868869781,
+        "odom_angular_z": 0.09906820952892303
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 1.068296540862205,
+      "yaw_error_to_plan_end": 0.32135952193565576,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.251789895891382,
+        "angular_z": 0.8455975210230922
+      },
+      "ackermann_cmd": {
+        "speed": -0.2517898976802826,
+        "steering_angle": -0.4477178156375885
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 16.657967630000712,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.1028212904930115,
+        "y": 1.4773329943418503,
+        "yaw": 1.2920184135437012,
+        "odom_age_sec": 0.028555564000000002,
+        "odom_linear_x": 0.23499999940395355,
+        "odom_angular_z": 0.1021270602941513
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 1.0440435615510695,
+      "yaw_error_to_plan_end": 0.3320208855373403,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.25165112090099356,
+        "angular_z": 0.8492889525656807
+      },
+      "ackermann_cmd": {
+        "speed": -0.2516511082649231,
+        "steering_angle": -0.449635773897171
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 16.75882907900086,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.10877126455307,
+        "y": 1.4979012459516525,
+        "yaw": 1.3025578260421753,
+        "odom_age_sec": 0.028922479,
+        "odom_linear_x": 0.20900000631809235,
+        "odom_angular_z": 0.11469582468271255
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 1.022802638036178,
+      "yaw_error_to_plan_end": 0.34256029803581445,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.6439036101362932,
+        "angular_z": 1.7200318070882763
+      },
+      "ackermann_cmd": {
+        "speed": 0.6439036130905151,
+        "steering_angle": 0.36488449573516846
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 16.857949531000486,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.1133617758750916,
+        "y": 1.514473482966423,
+        "yaw": 1.314656376838684,
+        "odom_age_sec": 0.028693313,
+        "odom_linear_x": 0.16300000250339508,
+        "odom_angular_z": 0.11570356786251068
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 1.0057750798505036,
+      "yaw_error_to_plan_end": 0.3546588488323232,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.6564690855867755,
+        "angular_z": 1.3607952180764589
+      },
+      "ackermann_cmd": {
+        "speed": 0.6564691066741943,
+        "steering_angle": 0.2881735861301422
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 16.958110647001376,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.117979943752289,
+        "y": 1.5319713801145554,
+        "yaw": 1.325433611869812,
+        "odom_age_sec": 0.028743855000000002,
+        "odom_linear_x": 0.1889999955892563,
+        "odom_angular_z": 0.10628268122673035
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.9878948581097343,
+      "yaw_error_to_plan_end": 0.36543608386345117,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.6700969954374844,
+        "angular_z": 0.9541863577092908
+      },
+      "ackermann_cmd": {
+        "speed": 0.6700969934463501,
+        "steering_angle": 0.20087890326976776
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 17.05795843099986,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.124458968639374,
+        "y": 1.5576061457395554,
+        "yaw": 1.3332264423370361,
+        "odom_age_sec": 0.028689521000000003,
+        "odom_linear_x": 0.2809999883174896,
+        "odom_angular_z": 0.06567481905221939
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.9618312213061532,
+      "yaw_error_to_plan_end": 0.3732289143306753,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.6827066811174253,
+        "angular_z": 0.5622030638013281
+      },
+      "ackermann_cmd": {
+        "speed": 0.6827066540718079,
+        "steering_angle": 0.11721941828727722
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 17.157830007001394,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.132821261882782,
+        "y": 1.5916715115308762,
+        "yaw": 1.3378974199295044,
+        "odom_age_sec": 0.028592438,
+        "odom_linear_x": 0.3659999966621399,
+        "odom_angular_z": 0.042231496423482895
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.9273311743257151,
+      "yaw_error_to_plan_end": 0.37789989192314355,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.6932351920361434,
+        "angular_z": 0.22331442609103633
+      },
+      "ackermann_cmd": {
+        "speed": 0.6932352185249329,
+        "steering_angle": 0.04603257775306702
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 17.258271081000203,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.1426331400871277,
+        "y": 1.632423684000969,
+        "yaw": 1.341956853866577,
+        "odom_age_sec": 0.026989524,
+        "odom_linear_x": 0.42500001192092896,
+        "odom_angular_z": 0.04027172178030014
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.8861992536593827,
+      "yaw_error_to_plan_end": 0.3819593258602161,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.6994810660961994,
+        "angular_z": -0.017284973345897257
+      },
+      "ackermann_cmd": {
+        "speed": 0.6994810700416565,
+        "steering_angle": -0.0035336781293153763
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 17.35787290799999,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.1539966464042664,
+        "y": 1.6804247349500656,
+        "yaw": 1.3435423374176025,
+        "odom_age_sec": 0.028535063000000003,
+        "odom_linear_x": 0.503000020980835,
+        "odom_angular_z": 0.0015185698866844177
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.8379342891380726,
+      "yaw_error_to_plan_end": 0.3835448094112417,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.6962652925575709,
+        "angular_z": -0.12382605571522783
+      },
+      "ackermann_cmd": {
+        "speed": 0.6962652802467346,
+        "steering_angle": -0.025426099076867104
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 17.457868066001538,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.166545331478119,
+        "y": 1.7335117310285568,
+        "yaw": 1.3408652544021604,
+        "odom_age_sec": 0.028807354,
+        "odom_linear_x": 0.5559999942779541,
+        "odom_angular_z": -0.03374776616692543
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.7847177459465118,
+      "yaw_error_to_plan_end": 0.3808677263957996,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.6910106144861201,
+        "angular_z": -0.29579813370470137
+      },
+      "ackermann_cmd": {
+        "speed": 0.691010594367981,
+        "steering_angle": -0.06113714724779129
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 17.55778339200151,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.180292308330536,
+        "y": 1.7908241003751755,
+        "yaw": 1.336171507835388,
+        "odom_age_sec": 0.028725729000000002,
+        "odom_linear_x": 0.5950000286102295,
+        "odom_angular_z": -0.051879025995731354
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.727395259433674,
+      "yaw_error_to_plan_end": 0.3761739798290271,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.6858761468258225,
+        "angular_z": -0.46129590444945007
+      },
+      "ackermann_cmd": {
+        "speed": 0.6858761310577393,
+        "steering_angle": -0.09588180482387543
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 17.65800900800059,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.195048987865448,
+        "y": 1.85097436606884,
+        "yaw": 1.330150485038757,
+        "odom_age_sec": 0.028736354000000002,
+        "odom_linear_x": 0.6150000095367432,
+        "odom_angular_z": -0.0623282752931118
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.6673788849661905,
+      "yaw_error_to_plan_end": 0.37015295703239626,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.6806822444267959,
+        "angular_z": -0.6261549152788936
+      },
+      "ackermann_cmd": {
+        "speed": 0.6806822419166565,
+        "steering_angle": -0.1307937502861023
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 17.758023709000554,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.2107309699058533,
+        "y": 1.9131603688001633,
+        "yaw": 1.3225548267364502,
+        "odom_age_sec": 0.028790438,
+        "odom_linear_x": 0.640999972820282,
+        "odom_angular_z": -0.08182062208652496
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.6055226756831656,
+      "yaw_error_to_plan_end": 0.36255729873008935,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.6763491176144371,
+        "angular_z": -0.7617263539180136
+      },
+      "ackermann_cmd": {
+        "speed": 0.6763491034507751,
+        "steering_angle": -0.1596800982952118
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 17.85791407600118,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.2274333834648132,
+        "y": 1.9771697968244553,
+        "yaw": 1.3132605552673342,
+        "odom_age_sec": 0.028705105,
+        "odom_linear_x": 0.6610000133514404,
+        "odom_angular_z": -0.0966605395078659
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.5421069276344478,
+      "yaw_error_to_plan_end": 0.35326302726097336,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.6719158257116116,
+        "angular_z": -0.8985810074481595
+      },
+      "ackermann_cmd": {
+        "speed": 0.6719158291816711,
+        "steering_angle": -0.1889583319425583
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 17.958063484000377,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.244849383831024,
+        "y": 2.041347309947014,
+        "yaw": 1.302060127258301,
+        "odom_age_sec": 0.028765271000000002,
+        "odom_linear_x": 0.6610000133514404,
+        "odom_angular_z": -0.12293443828821182
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.4788888852113128,
+      "yaw_error_to_plan_end": 0.34206259925194016,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.6675974793252061,
+        "angular_z": -1.030087672679774
+      },
+      "ackermann_cmd": {
+        "speed": 0.6675974726676941,
+        "steering_angle": -0.21716614067554474
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 18.057994767999844,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.2632893919944763,
+        "y": 2.106070324778557,
+        "yaw": 1.2862597703933714,
+        "odom_age_sec": 0.028770604,
+        "odom_linear_x": 0.6539999842643738,
+        "odom_angular_z": -0.16566607356071472
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.4156377317668636,
+      "yaw_error_to_plan_end": 0.3262622423870105,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.6637172621568315,
+        "angular_z": -1.1467371154677095
+      },
+      "ackermann_cmd": {
+        "speed": 0.6637172698974609,
+        "steering_angle": -0.24221739172935486
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 18.15786880199994,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.2828728556632996,
+        "y": 2.17063407599926,
+        "yaw": 1.2680876255035398,
+        "odom_age_sec": 0.028767521,
+        "odom_linear_x": 0.6669999957084656,
+        "odom_angular_z": -0.18104946613311768
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.3532474708801664,
+      "yaw_error_to_plan_end": 0.30809009749717897,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.6522089581418643,
+        "angular_z": -1.4842736008956603
+      },
+      "ackermann_cmd": {
+        "speed": 0.6522089838981628,
+        "steering_angle": -0.3146246075630188
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 18.25784146100159,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.303839147090912,
+        "y": 2.2355795353651047,
+        "yaw": 1.25339937210083,
+        "odom_age_sec": 0.028728979,
+        "odom_linear_x": 0.6740000247955322,
+        "odom_angular_z": -0.1388414353132248
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.29184836425134636,
+      "yaw_error_to_plan_end": 0.29340184409446923,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.5307554652817249,
+        "angular_z": -1.7063521003923803
+      },
+      "ackermann_cmd": {
+        "speed": 0.5307554602622986,
+        "steering_angle": -0.4309222996234894
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 18.35803453600056,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.325551211833954,
+        "y": 2.2996807545423508,
+        "yaw": 1.238867163658142,
+        "odom_age_sec": 0.028833271,
+        "odom_linear_x": 0.6610000133514404,
+        "odom_angular_z": -0.15794271230697632
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.23415330871730886,
+      "yaw_error_to_plan_end": 0.2788696356517813,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.41503526955825243,
+        "angular_z": -1.6601410782330097
+      },
+      "ackermann_cmd": {
+        "speed": 0.41503527760505676,
+        "steering_angle": -0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 18.457996944000115,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.3470420241355896,
+        "y": 2.35996775329113,
+        "yaw": 1.2167389392852783,
+        "odom_age_sec": 0.028771562,
+        "odom_linear_x": 0.640999972820282,
+        "odom_angular_z": -0.2719067335128784
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.18515606700953302,
+      "yaw_error_to_plan_end": 0.2567414112789175,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.3285656539363013,
+        "angular_z": -1.3142626157452053
+      },
+      "ackermann_cmd": {
+        "speed": 0.32856565713882446,
+        "steering_angle": -0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 18.557787562000158,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.3690897822380066,
+        "y": 2.41701178252697,
+        "yaw": 1.1802479028701782,
+        "odom_age_sec": 0.028634563000000002,
+        "odom_linear_x": 0.5889999866485596,
+        "odom_angular_z": -0.39284443855285645
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.14792249752990608,
+      "yaw_error_to_plan_end": 0.22025037486381738,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.26088490080219007,
+        "angular_z": -1.0435396032087603
+      },
+      "ackermann_cmd": {
+        "speed": 0.2608849108219147,
+        "steering_angle": -0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 18.657640679000906,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.391171157360077,
+        "y": 2.467957779765129,
+        "yaw": 1.1298272609710693,
+        "odom_age_sec": 0.028550979,
+        "odom_linear_x": 0.5360000133514404,
+        "odom_angular_z": -0.5445140600204468
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.1285584343574703,
+      "yaw_error_to_plan_end": 0.1698297329647085,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.22620600888660342,
+        "angular_z": -0.9048240355464137
+      },
+      "ackermann_cmd": {
+        "speed": 0.2262060046195984,
+        "steering_angle": -0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 18.759090126000956,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.4133452773094177,
+        "y": 2.512342259287834,
+        "yaw": 1.0664306879043581,
+        "odom_age_sec": 0.029135437,
+        "odom_linear_x": 0.47099998593330383,
+        "odom_angular_z": -0.6633378267288208
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.12758898442616665,
+      "yaw_error_to_plan_end": 0.10643315989799729,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.08976592836229721,
+        "angular_z": -0.35906371344918886
+      },
+      "ackermann_cmd": {
+        "speed": -0.08976592868566513,
+        "steering_angle": 0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 18.857915288001095,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.434355914592743,
+        "y": 2.548242375254631,
+        "yaw": 0.9948564767837524,
+        "odom_age_sec": 0.028703146000000002,
+        "odom_linear_x": 0.38600000739097595,
+        "odom_angular_z": -0.7251275181770325
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.1384542956188464,
+      "yaw_error_to_plan_end": 0.034858948777391596,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.09763841347469078,
+        "angular_z": -0.3905536538987631
+      },
+      "ackermann_cmd": {
+        "speed": -0.09763841331005096,
+        "steering_angle": 0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 18.957718655001372,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.4520227313041687,
+        "y": 2.5739574879407883,
+        "yaw": 0.9203699827194213,
+        "odom_age_sec": 0.028700772000000003,
+        "odom_linear_x": 0.2809999883174896,
+        "odom_angular_z": -0.7511019706726074
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.1517460572274258,
+      "yaw_error_to_plan_end": 0.03962754528693957,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.10727853458600708,
+        "angular_z": -0.42911413834402834
+      },
+      "ackermann_cmd": {
+        "speed": -0.10727853327989578,
+        "steering_angle": 0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 19.0577247300007,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.4642862677574158,
+        "y": 2.589225336909294,
+        "yaw": 0.8445306420326233,
+        "odom_age_sec": 0.028698396,
+        "odom_linear_x": 0.17000000178813934,
+        "odom_angular_z": -0.7591738700866699
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.1614073175698973,
+      "yaw_error_to_plan_end": 0.11546688597373755,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.11415275329710839,
+        "angular_z": -0.45661101318843356
+      },
+      "ackermann_cmd": {
+        "speed": -0.11415275186300278,
+        "steering_angle": 0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 19.15786655500051,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.471555173397064,
+        "y": 2.5970132797956467,
+        "yaw": 0.7758437395095825,
+        "odom_age_sec": 0.028733063000000003,
+        "odom_linear_x": 0.08500000089406967,
+        "odom_angular_z": -0.6407749056816101
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.1667343877816732,
+      "yaw_error_to_plan_end": 0.18415378849677833,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.11790412731606939,
+        "angular_z": -0.47161650926427756
+      },
+      "ackermann_cmd": {
+        "speed": -0.11790412664413452,
+        "steering_angle": 0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 19.25831329600078,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.4741844534873962,
+        "y": 2.599506661295891,
+        "yaw": 0.7290327548980713,
+        "odom_age_sec": 0.028894521000000003,
+        "odom_linear_x": 0.01899999938905239,
+        "odom_angular_z": -0.40194493532180786
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.1684718542284009,
+      "yaw_error_to_plan_end": 0.23096477310828956,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.11909824420891707,
+        "angular_z": -0.4763929768356683
+      },
+      "ackermann_cmd": {
+        "speed": -0.1190982460975647,
+        "steering_angle": 0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 19.357534123000733,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.4741844534873962,
+        "y": 2.599506661295891,
+        "yaw": 0.7290327548980713,
+        "odom_age_sec": 0.028439355000000003,
+        "odom_linear_x": 0.0,
+        "odom_angular_z": 0.0
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.1684718542284009,
+      "yaw_error_to_plan_end": 0.23096477310828956,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.11860418537679426,
+        "angular_z": -0.47441674150717705
+      },
+      "ackermann_cmd": {
+        "speed": -0.11860418319702148,
+        "steering_angle": 0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 19.45775657300146,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.473956525325775,
+        "y": 2.599307104945183,
+        "yaw": 0.7290327548980713,
+        "odom_age_sec": 0.028574938,
+        "odom_linear_x": -0.006000000052154064,
+        "odom_angular_z": 0.0
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.16833492922944043,
+      "yaw_error_to_plan_end": 0.23096477310828956,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.11860418537679426,
+        "angular_z": -0.47441674150717705
+      },
+      "ackermann_cmd": {
+        "speed": -0.11860418319702148,
+        "steering_angle": 0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 19.55799423100143,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.473728120326996,
+        "y": 2.5991070717573166,
+        "yaw": 0.7290327548980713,
+        "odom_age_sec": 0.028823896,
+        "odom_linear_x": -0.006000000052154064,
+        "odom_angular_z": 0.0
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.16819809583174525,
+      "yaw_error_to_plan_end": 0.23096477310828956,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.11850779017752609,
+        "angular_z": -0.47403116071010437
+      },
+      "ackermann_cmd": {
+        "speed": -0.11850778758525848,
+        "steering_angle": 0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 19.657757599001343,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.473495662212372,
+        "y": 2.598903462290764,
+        "yaw": 0.7290327548980713,
+        "odom_age_sec": 0.028625813000000003,
+        "odom_linear_x": -0.006000000052154064,
+        "odom_angular_z": 0.0
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.16805925731476296,
+      "yaw_error_to_plan_end": 0.23096477310828956,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.11841145946554868,
+        "angular_z": -0.4736458378621947
+      },
+      "ackermann_cmd": {
+        "speed": -0.11841145902872086,
+        "steering_angle": 0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 19.757723466000243,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.4720043540000916,
+        "y": 2.5975952595472336,
+        "yaw": 0.7319111824035646,
+        "odom_age_sec": 0.028485022000000002,
+        "odom_linear_x": -0.026000000536441803,
+        "odom_angular_z": 0.03611808642745018
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.1671774983412149,
+      "yaw_error_to_plan_end": 0.22808634560279628,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.11790621497765634,
+        "angular_z": -0.47162485991062536
+      },
+      "ackermann_cmd": {
+        "speed": -0.11790621280670166,
+        "steering_angle": 0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 19.85783295800138,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.468793570995331,
+        "y": 2.594758316874504,
+        "yaw": 0.7382937073707582,
+        "odom_age_sec": 0.028532480000000002,
+        "odom_linear_x": -0.052000001072883606,
+        "odom_angular_z": 0.07795163244009018
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.16532436610656712,
+      "yaw_error_to_plan_end": 0.22170382063560268,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.1167100415407696,
+        "angular_z": -0.4668401661630784
+      },
+      "ackermann_cmd": {
+        "speed": -0.11671004444360733,
+        "steering_angle": 0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 19.957717533001414,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.463633954524994,
+        "y": 2.590130850672722,
+        "yaw": 0.7503373622894288,
+        "odom_age_sec": 0.028397689,
+        "odom_linear_x": -0.07100000232458115,
+        "odom_angular_z": 0.1441255509853363
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.1624739045725592,
+      "yaw_error_to_plan_end": 0.20966016571693202,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.11449346651610894,
+        "angular_z": -0.45797386606443574
+      },
+      "ackermann_cmd": {
+        "speed": -0.1144934669137001,
+        "steering_angle": 0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 20.057949233001636,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.457790791988373,
+        "y": 2.5847404450178146,
+        "yaw": 0.7707119584083557,
+        "odom_age_sec": 0.028716812,
+        "odom_linear_x": -0.07800000160932541,
+        "odom_angular_z": 0.22498764097690582
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.15940850683284513,
+      "yaw_error_to_plan_end": 0.18928556959800513,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.11225073535452862,
+        "angular_z": -0.4490029414181145
+      },
+      "ackermann_cmd": {
+        "speed": -0.11225073784589767,
+        "steering_angle": 0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 20.159009181001238,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.4510411620140076,
+        "y": 2.5782261341810226,
+        "yaw": 0.7977863550186156,
+        "odom_age_sec": 0.029173978000000003,
+        "odom_linear_x": -0.09799999743700027,
+        "odom_angular_z": 0.28833019733428955
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.15603481944872344,
+      "yaw_error_to_plan_end": 0.16221117298774523,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.11005319424550002,
+        "angular_z": -0.44021277698200006
+      },
+      "ackermann_cmd": {
+        "speed": -0.11005319654941559,
+        "steering_angle": 0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 20.25780117600152,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.444105565547943,
+        "y": 2.5711529701948166,
+        "yaw": 0.8297392725944519,
+        "odom_age_sec": 0.028695855000000003,
+        "odom_linear_x": -0.09099999815225601,
+        "odom_angular_z": 0.3294859230518341
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.15275522808912334,
+      "yaw_error_to_plan_end": 0.13025825541190894,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.10741482666520803,
+        "angular_z": -0.42965930666083213
+      },
+      "ackermann_cmd": {
+        "speed": -0.10741482675075531,
+        "steering_angle": 0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 20.359111790001407,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.437832772731781,
+        "y": 2.5643139332532883,
+        "yaw": 0.8645916581153871,
+        "odom_age_sec": 0.029368186,
+        "odom_linear_x": -0.09099999815225601,
+        "odom_angular_z": 0.35397911071777344
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.14991296568201892,
+      "yaw_error_to_plan_end": 0.09540586989097377,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.10555353283320348,
+        "angular_z": -0.42221413133281394
+      },
+      "ackermann_cmd": {
+        "speed": -0.1055535301566124,
+        "steering_angle": 0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 20.458054409000397,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.434684455394745,
+        "y": 2.5606945008039474,
+        "yaw": 0.8819347620010376,
+        "odom_age_sec": 0.028888188000000002,
+        "odom_linear_x": -0.09799999743700027,
+        "odom_angular_z": 0.36084890365600586
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.14852774341326566,
+      "yaw_error_to_plan_end": 0.07806276600532325,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.10364745506947659,
+        "angular_z": -0.41458982027790636
+      },
+      "ackermann_cmd": {
+        "speed": -0.10364745557308197,
+        "steering_angle": 0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 20.557884110001396,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.42825049161911,
+        "y": 2.552882954478264,
+        "yaw": 0.920168697834015,
+        "odom_age_sec": 0.028690396000000003,
+        "odom_linear_x": -0.09799999743700027,
+        "odom_angular_z": 0.3812561631202698
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.14581441782107762,
+      "yaw_error_to_plan_end": 0.03982883017234584,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.1026649567285151,
+        "angular_z": -0.4106598269140604
+      },
+      "ackermann_cmd": {
+        "speed": -0.10266495496034622,
+        "steering_angle": 0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 20.657722186000683,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.422667682170868,
+        "y": 2.5455456227064133,
+        "yaw": 0.9588474631309509,
+        "odom_age_sec": 0.028577813,
+        "odom_linear_x": -0.09099999815225601,
+        "odom_angular_z": 0.3898177146911621
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.14356977212015806,
+      "yaw_error_to_plan_end": 0.0011500648754099174,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.10106105540167208,
+        "angular_z": -0.4042442216066883
+      },
+      "ackermann_cmd": {
+        "speed": -0.10106105357408524,
+        "steering_angle": 0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 20.757747261000986,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.4171544909477234,
+        "y": 2.5376770943403244,
+        "yaw": 0.9971161484718323,
+        "odom_age_sec": 0.028696897000000002,
+        "odom_linear_x": -0.09799999743700027,
+        "odom_angular_z": 0.38077154755592346
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.14146004950393906,
+      "yaw_error_to_plan_end": 0.03711862046547143,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.09962339779759433,
+        "angular_z": -0.3984935911903773
+      },
+      "ackermann_cmd": {
+        "speed": -0.09962339699268341,
+        "steering_angle": 0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 20.85755037800118,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.411594569683075,
+        "y": 2.5290501564741135,
+        "yaw": 1.037197232246399,
+        "odom_age_sec": 0.028523646000000003,
+        "odom_linear_x": -0.10400000214576721,
+        "odom_angular_z": 0.4100419282913208
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.13950848750492598,
+      "yaw_error_to_plan_end": 0.07719970424003807,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.09823271538226756,
+        "angular_z": -0.39293086152907025
+      },
+      "ackermann_cmd": {
+        "speed": -0.09823271632194519,
+        "steering_angle": 0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 20.95805953600029,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.406427323818207,
+        "y": 2.5202539414167404,
+        "yaw": 1.0804349184036255,
+        "odom_age_sec": 0.028796146,
+        "odom_linear_x": -0.10400000214576721,
+        "odom_angular_z": 0.44006139039993286
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.13787848877546918,
+      "yaw_error_to_plan_end": 0.12043739039726464,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.097057860613895,
+        "angular_z": -0.38823144245558
+      },
+      "ackermann_cmd": {
+        "speed": -0.09705786406993866,
+        "steering_angle": 0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 21.057860612001605,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.401624619960785,
+        "y": 2.511182114481926,
+        "yaw": 1.1255629062652586,
+        "odom_age_sec": 0.028658355000000003,
+        "odom_linear_x": -0.10400000214576721,
+        "odom_angular_z": 0.45363932847976685
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.13653481920431765,
+      "yaw_error_to_plan_end": 0.16556537825889772,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.24025126447477482,
+        "angular_z": -0.9610050578990993
+      },
+      "ackermann_cmd": {
+        "speed": 0.24025125801563263,
+        "steering_angle": -0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 21.15794039499997,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.3973812460899353,
+        "y": 2.5021982640028,
+        "yaw": 1.1716398000717163,
+        "odom_age_sec": 0.028693313,
+        "odom_linear_x": -0.09799999743700027,
+        "odom_angular_z": 0.462236225605011
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.13550876874912343,
+      "yaw_error_to_plan_end": 0.21164227206535546,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.2383831447570566,
+        "angular_z": -0.9535325790282264
+      },
+      "ackermann_cmd": {
+        "speed": 0.23838314414024353,
+        "steering_angle": -0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 21.258176178000213,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.394568622112274,
+        "y": 2.495484873652458,
+        "yaw": 1.2174198627471924,
+        "odom_age_sec": 0.028930896,
+        "odom_linear_x": -0.057999998331069946,
+        "odom_angular_z": 0.4522222876548767
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.13489237060732806,
+      "yaw_error_to_plan_end": 0.25742233474083154,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.23716210402989202,
+        "angular_z": -0.9486484161195681
+      },
+      "ackermann_cmd": {
+        "speed": 0.23716209828853607,
+        "steering_angle": -0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 21.357760671000506,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.3940083384513855,
+        "y": 2.493977829813957,
+        "yaw": 1.2376539707183838,
+        "odom_age_sec": 0.028580813,
+        "odom_linear_x": -0.006000000052154064,
+        "odom_angular_z": 0.0
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.13474338768748886,
+      "yaw_error_to_plan_end": 0.27765644271202294,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.2369485035040085,
+        "angular_z": -0.947794014016034
+      },
+      "ackermann_cmd": {
+        "speed": 0.23694850504398346,
+        "steering_angle": -0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 21.457584580000912,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.3941096663475037,
+        "y": 2.4942648857831955,
+        "yaw": 1.2376539707183838,
+        "odom_age_sec": 0.028524855000000002,
+        "odom_linear_x": 0.006000000052154064,
+        "odom_angular_z": 0.0
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.1347751667233605,
+      "yaw_error_to_plan_end": 0.27765644271202294,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.23714836232998043,
+        "angular_z": -0.9485934493199217
+      },
+      "ackermann_cmd": {
+        "speed": 0.23714835941791534,
+        "steering_angle": -0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 21.558075530001588,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.3951799273490906,
+        "y": 2.4973240345716476,
+        "yaw": 1.2434000968933105,
+        "odom_age_sec": 0.028871688000000003,
+        "odom_linear_x": 0.04500000178813934,
+        "odom_angular_z": 0.03876194357872009
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.13516451872016289,
+      "yaw_error_to_plan_end": 0.28340256888694965,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.2375927532882667,
+        "angular_z": -0.9503710131530668
+      },
+      "ackermann_cmd": {
+        "speed": 0.23759275674819946,
+        "steering_angle": -0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 21.6577475640006,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.3974215388298035,
+        "y": 2.5038025826215744,
+        "yaw": 1.2426198720932007,
+        "odom_age_sec": 0.028643104000000003,
+        "odom_linear_x": 0.07100000232458115,
+        "odom_angular_z": -0.028120197355747223
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.13626182186130567,
+      "yaw_error_to_plan_end": 0.28262234408683984,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.239714039239606,
+        "angular_z": -0.958856156958424
+      },
+      "ackermann_cmd": {
+        "speed": 0.23971404135227203,
+        "steering_angle": -0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 21.75777105599991,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.4011234641075134,
+        "y": 2.514409586787224,
+        "yaw": 1.2349830865859985,
+        "odom_age_sec": 0.028601979000000003,
+        "odom_linear_x": 0.12399999797344208,
+        "odom_angular_z": -0.0966399610042572
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.13874630920879108,
+      "yaw_error_to_plan_end": 0.2749855585796377,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.2435217590160418,
+        "angular_z": -0.9740870360641672
+      },
+      "ackermann_cmd": {
+        "speed": 0.2435217648744583,
+        "steering_angle": -0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 21.85804000600001,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.4057909846305847,
+        "y": 2.527389094233513,
+        "yaw": 1.2210183143615723,
+        "odom_age_sec": 0.028683147000000003,
+        "odom_linear_x": 0.14300000667572021,
+        "odom_angular_z": -0.15794791281223297
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.14282777686143655,
+      "yaw_error_to_plan_end": 0.2610207863552114,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.10033770588611976,
+        "angular_z": -0.40135082354447904
+      },
+      "ackermann_cmd": {
+        "speed": -0.10033770650625229,
+        "steering_angle": 0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 21.95797812300043,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.410664737224579,
+        "y": 2.540314242243767,
+        "yaw": 1.200916051864624,
+        "odom_age_sec": 0.028896437,
+        "odom_linear_x": 0.12999999523162842,
+        "odom_angular_z": -0.21800246834754944
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.147901350984375,
+      "yaw_error_to_plan_end": 0.24091852385826318,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.10418866899559127,
+        "angular_z": -0.4167546759823651
+      },
+      "ackermann_cmd": {
+        "speed": -0.10418866574764252,
+        "steering_angle": 0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 22.057791406999968,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.4142648577690125,
+        "y": 2.5492859333753586,
+        "yaw": 1.175804376602173,
+        "odom_age_sec": 0.028825188,
+        "odom_linear_x": 0.07800000160932541,
+        "odom_angular_z": -0.2576974034309387
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.15191753392685967,
+      "yaw_error_to_plan_end": 0.21580684859581223,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.10732992498776045,
+        "angular_z": -0.4293196999510418
+      },
+      "ackermann_cmd": {
+        "speed": -0.10732992738485336,
+        "steering_angle": 0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 22.158013648000633,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.4163207411766052,
+        "y": 2.55405430495739,
+        "yaw": 1.149994134902954,
+        "odom_age_sec": 0.028841188,
+        "odom_linear_x": 0.039000000804662704,
+        "odom_angular_z": -0.2538056969642639
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.15416431920600682,
+      "yaw_error_to_plan_end": 0.18999660689659326,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.10887933807291733,
+        "angular_z": -0.4355173522916693
+      },
+      "ackermann_cmd": {
+        "speed": -0.10887933522462845,
+        "steering_angle": 0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 22.258075765001195,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.416596829891205,
+        "y": 2.5546594113111496,
+        "yaw": 1.1383303403854372,
+        "odom_age_sec": 0.028856105000000003,
+        "odom_linear_x": 0.0,
+        "odom_angular_z": 0.0
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.15444982456670034,
+      "yaw_error_to_plan_end": 0.1783328123790764,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.10891651217628504,
+        "angular_z": -0.43566604870514014
+      },
+      "ackermann_cmd": {
+        "speed": -0.1089165136218071,
+        "steering_angle": 0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 22.35781809100081,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.416596829891205,
+        "y": 2.5546594113111496,
+        "yaw": 1.1383303403854372,
+        "odom_age_sec": 0.028812854000000002,
+        "odom_linear_x": 0.0,
+        "odom_angular_z": 0.0
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.15444982456670034,
+      "yaw_error_to_plan_end": 0.1783328123790764,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.10873267649495706,
+        "angular_z": -0.43493070597982825
+      },
+      "ackermann_cmd": {
+        "speed": -0.1087326779961586,
+        "steering_angle": 0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 22.457722250001098,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.416596829891205,
+        "y": 2.5546594113111496,
+        "yaw": 1.1383303403854372,
+        "odom_age_sec": 0.028645813000000003,
+        "odom_linear_x": 0.0,
+        "odom_angular_z": 0.0
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.15444982456670034,
+      "yaw_error_to_plan_end": 0.1783328123790764,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.10873267649495706,
+        "angular_z": -0.43493070597982825
+      },
+      "ackermann_cmd": {
+        "speed": -0.1087326779961586,
+        "steering_angle": 0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 22.5578307830001,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.416596829891205,
+        "y": 2.5546594113111496,
+        "yaw": 1.1383303403854372,
+        "odom_age_sec": 0.028645271000000003,
+        "odom_linear_x": 0.0,
+        "odom_angular_z": 0.0
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.15444982456670034,
+      "yaw_error_to_plan_end": 0.1783328123790764,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.10873267649495706,
+        "angular_z": -0.43493070597982825
+      },
+      "ackermann_cmd": {
+        "speed": -0.1087326779961586,
+        "steering_angle": 0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 22.657646734000082,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.4164671301841736,
+        "y": 2.554383799433708,
+        "yaw": 1.1383303403854372,
+        "odom_age_sec": 0.028576188000000002,
+        "odom_linear_x": -0.006000000052154064,
+        "odom_angular_z": 0.0
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.15432203342387657,
+      "yaw_error_to_plan_end": 0.1783328123790764,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.10873267649495706,
+        "angular_z": -0.43493070597982825
+      },
+      "ackermann_cmd": {
+        "speed": -0.1087326779961586,
+        "steering_angle": 0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 22.757866892001402,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.4154998660087585,
+        "y": 2.5523276776075363,
+        "yaw": 1.1393070220947268,
+        "odom_age_sec": 0.028762522000000002,
+        "odom_linear_x": -0.026000000536441803,
+        "odom_angular_z": 0.011148534715175629
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.15338420451668858,
+      "yaw_error_to_plan_end": 0.17930949408836594,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.10809608424725291,
+        "angular_z": -0.43238433698901163
+      },
+      "ackermann_cmd": {
+        "speed": -0.10809608548879623,
+        "steering_angle": 0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 22.85920750600053,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.4136921763420105,
+        "y": 2.5484729260206223,
+        "yaw": 1.14303719997406,
+        "odom_age_sec": 0.029338895,
+        "odom_linear_x": -0.04500000178813934,
+        "odom_angular_z": 0.05719660222530365
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.15169826633663253,
+      "yaw_error_to_plan_end": 0.1830396719676992,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.10689974782673171,
+        "angular_z": -0.42759899130692686
+      },
+      "ackermann_cmd": {
+        "speed": -0.10689974576234818,
+        "steering_angle": 0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 22.957752502001313,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.410951554775238,
+        "y": 2.542532965540886,
+        "yaw": 1.1557606458663938,
+        "odom_age_sec": 0.028788063000000003,
+        "odom_linear_x": -0.07100000232458115,
+        "odom_angular_z": 0.1499544382095337
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.14926933358923036,
+      "yaw_error_to_plan_end": 0.19576311786003298,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.10527539872280971,
+        "angular_z": -0.42110159489123883
+      },
+      "ackermann_cmd": {
+        "speed": -0.10527539998292923,
+        "steering_angle": 0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 23.05905828200048,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.407467544078827,
+        "y": 2.534673258662224,
+        "yaw": 1.1789184808731077,
+        "odom_age_sec": 0.029351895000000003,
+        "odom_linear_x": -0.08500000089406967,
+        "odom_angular_z": 0.2633431851863861
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.14633775874768518,
+      "yaw_error_to_plan_end": 0.21892095286674684,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.10304727973297495,
+        "angular_z": -0.4121891189318998
+      },
+      "ackermann_cmd": {
+        "speed": -0.10304728150367737,
+        "steering_angle": 0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 23.157789485001558,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.4038185477256775,
+        "y": 2.525816723704338,
+        "yaw": 1.211362600326538,
+        "odom_age_sec": 0.028820605000000003,
+        "odom_linear_x": -0.09799999743700027,
+        "odom_angular_z": 0.3430420756340027
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.14336426091644544,
+      "yaw_error_to_plan_end": 0.25136507232017724,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.10102094846815753,
+        "angular_z": -0.4040837938726301
+      },
+      "ackermann_cmd": {
+        "speed": -0.101020947098732,
+        "steering_angle": 0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 23.257848311001,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.4004802107810974,
+        "y": 2.516863390803337,
+        "yaw": 1.2492338418960571,
+        "odom_age_sec": 0.028792355000000002,
+        "odom_linear_x": -0.09099999815225601,
+        "odom_angular_z": 0.387634813785553
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.14067001451500516,
+      "yaw_error_to_plan_end": 0.28923631388969623,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.2474247272999261,
+        "angular_z": -0.9896989091997044
+      },
+      "ackermann_cmd": {
+        "speed": 0.24742472171783447,
+        "steering_angle": -0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 23.359413924001274,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.398004472255707,
+        "y": 2.5093689411878586,
+        "yaw": 1.2895346879959106,
+        "odom_age_sec": 0.029486062,
+        "odom_linear_x": -0.06499999761581421,
+        "odom_angular_z": 0.407098650932312
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.13861006396529174,
+      "yaw_error_to_plan_end": 0.3295371599895498,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.24338679639326485,
+        "angular_z": -0.9735471855730594
+      },
+      "ackermann_cmd": {
+        "speed": 0.24338679015636444,
+        "steering_angle": -0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 23.45814729400081,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.397242248058319,
+        "y": 2.5067820996046066,
+        "yaw": 1.3106199502944946,
+        "odom_age_sec": 0.078950454,
+        "odom_linear_x": -0.052000001072883606,
+        "odom_angular_z": 0.4112648367881775
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.13791739265948708,
+      "yaw_error_to_plan_end": 0.3506224222881338,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.2415353004477355,
+        "angular_z": -0.966141201790942
+      },
+      "ackermann_cmd": {
+        "speed": 0.24153530597686768,
+        "steering_angle": -0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 23.557866578001267,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.397323787212372,
+        "y": 2.5070825070142746,
+        "yaw": 1.3106199502944946,
+        "odom_age_sec": 0.028742771,
+        "odom_linear_x": 0.006000000052154064,
+        "odom_angular_z": 0.0
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.13800129337995767,
+      "yaw_error_to_plan_end": 0.3506224222881338,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.24273461108069727,
+        "angular_z": -0.9709384443227891
+      },
+      "ackermann_cmd": {
+        "speed": 0.2427346110343933,
+        "steering_angle": -0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 23.657596904000457,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.3976630568504333,
+        "y": 2.508332297205925,
+        "yaw": 1.3149404525756836,
+        "odom_age_sec": 0.028523647000000003,
+        "odom_linear_x": 0.026000000536441803,
+        "odom_angular_z": 0.08772971481084824
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.13835728350570725,
+      "yaw_error_to_plan_end": 0.3549429245693228,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.24288227634872553,
+        "angular_z": -0.9715291053949021
+      },
+      "ackermann_cmd": {
+        "speed": 0.24288228154182434,
+        "steering_angle": -0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 23.757823104000636,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.399187743663788,
+        "y": 2.5140736550092697,
+        "yaw": 1.3167204856872559,
+        "odom_age_sec": 0.028640063,
+        "odom_linear_x": 0.06499999761581421,
+        "odom_angular_z": -0.0046514347195625305
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.14016303895843546,
+      "yaw_error_to_plan_end": 0.356722957680895,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.24625499217342567,
+        "angular_z": -0.9850199686937027
+      },
+      "ackermann_cmd": {
+        "speed": 0.24625499546527863,
+        "steering_angle": -0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 23.858054054000604,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.4017333388328552,
+        "y": 2.5236387699842453,
+        "yaw": 1.3110597133636472,
+        "odom_age_sec": 0.028907188,
+        "odom_linear_x": 0.11100000143051147,
+        "odom_angular_z": -0.07077668607234955
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.14366302013986776,
+      "yaw_error_to_plan_end": 0.3510621853572864,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.10073379655597463,
+        "angular_z": -0.4029351862238985
+      },
+      "ackermann_cmd": {
+        "speed": -0.10073379427194595,
+        "steering_angle": 0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 23.959089502001007,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.4050418734550476,
+        "y": 2.535718724131584,
+        "yaw": 1.2991294860839844,
+        "odom_age_sec": 0.029272395000000003,
+        "odom_linear_x": 0.12399999797344208,
+        "odom_angular_z": -0.13388700783252716
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.14883840734070283,
+      "yaw_error_to_plan_end": 0.33913195807762353,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.10468917035122291,
+        "angular_z": -0.41875668140489164
+      },
+      "ackermann_cmd": {
+        "speed": -0.10468917340040207,
+        "steering_angle": 0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 24.05907666100029,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.407864272594452,
+        "y": 2.5455203503370285,
+        "yaw": 1.2819449901580808,
+        "odom_age_sec": 0.029333187,
+        "odom_linear_x": 0.09099999815225601,
+        "odom_angular_z": -0.18744981288909912
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.15356420249444927,
+      "yaw_error_to_plan_end": 0.32194746215172,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.10835004429894521,
+        "angular_z": -0.43340017719578083
+      },
+      "ackermann_cmd": {
+        "speed": -0.10835004597902298,
+        "steering_angle": 0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 24.158039572001144,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.4093088507652283,
+        "y": 2.550227925181389,
+        "yaw": 1.261000156402588,
+        "odom_age_sec": 0.028899313000000003,
+        "odom_linear_x": 0.03200000151991844,
+        "odom_angular_z": -0.21161359548568726
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.15595942590831346,
+      "yaw_error_to_plan_end": 0.30100262839622705,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.11031086794663264,
+        "angular_z": -0.44124347178653056
+      },
+      "ackermann_cmd": {
+        "speed": -0.1103108674287796,
+        "steering_angle": 0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 24.258164188000592,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.4093074202537537,
+        "y": 2.550223395228386,
+        "yaw": 1.261000156402588,
+        "odom_age_sec": 0.028867896,
+        "odom_linear_x": -0.006000000052154064,
+        "odom_angular_z": 0.0
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.15595709160871069,
+      "yaw_error_to_plan_end": 0.30100262839622705,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.10999691411176228,
+        "angular_z": -0.4399876564470491
+      },
+      "ackermann_cmd": {
+        "speed": -0.10999691486358643,
+        "steering_angle": 0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 24.357851056000072,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.4092130064964294,
+        "y": 2.5499341934919357,
+        "yaw": 1.261000156402588,
+        "odom_age_sec": 0.028668438,
+        "odom_linear_x": 0.0,
+        "odom_angular_z": 0.0
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.15581040322037998,
+      "yaw_error_to_plan_end": 0.30100262839622705,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.10959173279211003,
+        "angular_z": -0.4383669311684401
+      },
+      "ackermann_cmd": {
+        "speed": -0.10959172993898392,
+        "steering_angle": 0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 24.45811271400089,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.4091197848320007,
+        "y": 2.549648329615593,
+        "yaw": 1.261000156402588,
+        "odom_age_sec": 0.028883354000000003,
+        "odom_linear_x": -0.006000000052154064,
+        "odom_angular_z": 0.0
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.15566578616019583,
+      "yaw_error_to_plan_end": 0.30100262839622705,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.10969052386714752,
+        "angular_z": -0.4387620954685901
+      },
+      "ackermann_cmd": {
+        "speed": -0.10969052463769913,
+        "steering_angle": 0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 24.55916828699992,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.4087113738059998,
+        "y": 2.548397347331047,
+        "yaw": 1.2609894275665283,
+        "odom_age_sec": 0.029428061000000002,
+        "odom_linear_x": -0.013000000268220901,
+        "odom_angular_z": 0.0021374511998146772
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.1550385107427138,
+      "yaw_error_to_plan_end": 0.3009918995601675,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.10915591353406019,
+        "angular_z": -0.43662365413624077
+      },
+      "ackermann_cmd": {
+        "speed": -0.10915591567754745,
+        "steering_angle": 0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 24.65781378200154,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.408303916454315,
+        "y": 2.5471473187208176,
+        "yaw": 1.2643074989318848,
+        "odom_age_sec": 0.028779063,
+        "odom_linear_x": -0.013000000268220901,
+        "odom_angular_z": 0.05126689746975899
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.15441992338066243,
+      "yaw_error_to_plan_end": 0.3043099709255239,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.10872377668498008,
+        "angular_z": -0.4348951067399203
+      },
+      "ackermann_cmd": {
+        "speed": -0.10872377455234528,
+        "steering_angle": 0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 24.75780527400093,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.4075209498405457,
+        "y": 2.5446985214948654,
+        "yaw": 1.2737658023834229,
+        "odom_age_sec": 0.028770937000000003,
+        "odom_linear_x": -0.03200000151991844,
+        "odom_angular_z": 0.10636167228221893
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.1532228094890491,
+      "yaw_error_to_plan_end": 0.31376827437706195,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.10809103114972078,
+        "angular_z": -0.4323641245988831
+      },
+      "ackermann_cmd": {
+        "speed": -0.10809103399515152,
+        "steering_angle": 0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 24.857783766001376,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.4059595465660095,
+        "y": 2.539641186594963,
+        "yaw": 1.2862759828567505,
+        "odom_age_sec": 0.028781854000000003,
+        "odom_linear_x": -0.052000001072883606,
+        "odom_angular_z": 0.12849783897399902
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.1508186556618222,
+      "yaw_error_to_plan_end": 0.32627845485038964,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.10622215681070707,
+        "angular_z": -0.4248886272428283
+      },
+      "ackermann_cmd": {
+        "speed": -0.10622216016054153,
+        "steering_angle": 0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 24.958020258000033,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.404209315776825,
+        "y": 2.533678814768791,
+        "yaw": 1.3040102720260618,
+        "odom_age_sec": 0.028822438000000002,
+        "odom_linear_x": -0.06499999761581421,
+        "odom_angular_z": 0.1939965784549713
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.14810888598608102,
+      "yaw_error_to_plan_end": 0.34401274401970094,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.10441059820466908,
+        "angular_z": -0.4176423928186763
+      },
+      "ackermann_cmd": {
+        "speed": -0.10441059619188309,
+        "steering_angle": 0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 25.057865000000675,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.4022340178489685,
+        "y": 2.5264282673597336,
+        "yaw": 1.3274561166763306,
+        "odom_age_sec": 0.028660480000000002,
+        "odom_linear_x": -0.07800000160932541,
+        "odom_angular_z": 0.2505720555782318
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.14498444440310285,
+      "yaw_error_to_plan_end": 0.3674585886699698,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.10220691397958133,
+        "angular_z": -0.4088276559183253
+      },
+      "ackermann_cmd": {
+        "speed": -0.10220691561698914,
+        "steering_angle": 0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 25.15798128300048,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.4003514647483826,
+        "y": 2.5187681168317795,
+        "yaw": 1.3562800884246824,
+        "odom_age_sec": 0.028672563,
+        "odom_linear_x": -0.07800000160932541,
+        "odom_angular_z": 0.3014412820339203
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.1418749235798633,
+      "yaw_error_to_plan_end": 0.3962825604183215,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.09990701244722545,
+        "angular_z": -0.3996280497889018
+      },
+      "ackermann_cmd": {
+        "speed": -0.09990701079368591,
+        "steering_angle": 0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 25.25785494200136,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.398634374141693,
+        "y": 2.510717675089836,
+        "yaw": 1.3890388011932373,
+        "odom_age_sec": 0.028823521,
+        "odom_linear_x": -0.08500000089406967,
+        "odom_angular_z": 0.3357832431793213
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.1387943506800239,
+      "yaw_error_to_plan_end": 0.4290412731868765,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.2445671754399214,
+        "angular_z": -0.9782687017596856
+      },
+      "ackermann_cmd": {
+        "speed": 0.2445671707391739,
+        "steering_angle": -0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 25.357800101000976,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.397244870662689,
+        "y": 2.5029504746198654,
+        "yaw": 1.4253756999969482,
+        "odom_age_sec": 0.028768355000000002,
+        "odom_linear_x": -0.07100000232458115,
+        "odom_angular_z": 0.3705486059188843
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.13599529401477642,
+      "yaw_error_to_plan_end": 0.46537817199058745,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.23901290580621973,
+        "angular_z": -0.9560516232248789
+      },
+      "ackermann_cmd": {
+        "speed": 0.2390129119157791,
+        "steering_angle": -0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 25.458251216999997,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.396824300289154,
+        "y": 2.5000496357679367,
+        "yaw": 1.4619538784027102,
+        "odom_age_sec": 0.028968063000000002,
+        "odom_linear_x": -0.013000000268220901,
+        "odom_angular_z": 0.3526260256767273
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.1349666273537195,
+      "yaw_error_to_plan_end": 0.5019563503963493,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.23659239567974424,
+        "angular_z": -0.946369582718977
+      },
+      "ackermann_cmd": {
+        "speed": 0.2365923970937729,
+        "steering_angle": -0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 25.55911766600002,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.396824300289154,
+        "y": 2.5000496357679367,
+        "yaw": 1.4619538784027102,
+        "odom_age_sec": 0.02928402,
+        "odom_linear_x": 0.0,
+        "odom_angular_z": 0.0
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.1349666273537195,
+      "yaw_error_to_plan_end": 0.5019563503963493,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.23754126414254634,
+        "angular_z": -0.9501650565701854
+      },
+      "ackermann_cmd": {
+        "speed": 0.23754125833511353,
+        "steering_angle": -0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 25.659282657001313,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.397065579891205,
+        "y": 2.50229911506176,
+        "yaw": 1.4712568521499636,
+        "odom_age_sec": 0.029435395000000003,
+        "odom_linear_x": 0.03200000151991844,
+        "odom_angular_z": 0.06953049451112747
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.13583362414491096,
+      "yaw_error_to_plan_end": 0.5112593241436028,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.23840235227486145,
+        "angular_z": -0.9536094090994458
+      },
+      "ackermann_cmd": {
+        "speed": 0.2384023517370224,
+        "steering_angle": -0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 25.75763552800163,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.397690713405609,
+        "y": 2.5084832161664963,
+        "yaw": 1.4721879959106443,
+        "odom_age_sec": 0.028622855000000003,
+        "odom_linear_x": 0.07100000232458115,
+        "odom_angular_z": -0.007302473299205303
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.13841238432717246,
+      "yaw_error_to_plan_end": 0.5121904679042835,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.242787054048533,
+        "angular_z": -0.971148216194132
+      },
+      "ackermann_cmd": {
+        "speed": 0.24278704822063446,
+        "steering_angle": -0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 25.85871472600047,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.398659884929657,
+        "y": 2.517973944544792,
+        "yaw": 1.466112732887268,
+        "odom_age_sec": 0.029068854,
+        "odom_linear_x": 0.10400000214576721,
+        "odom_angular_z": -0.07821089029312134
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.1427982644914619,
+      "yaw_error_to_plan_end": 0.5061152048809072,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.25032334031931186,
+        "angular_z": -1.0012933612772474
+      },
+      "ackermann_cmd": {
+        "speed": 0.2503233551979065,
+        "steering_angle": -0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 25.957855927999844,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.400103509426117,
+        "y": 2.531102940440178,
+        "yaw": 1.4542964696884155,
+        "odom_age_sec": 0.028818438000000002,
+        "odom_linear_x": 0.12999999523162842,
+        "odom_angular_z": -0.12669438123703003
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.14957861278020781,
+      "yaw_error_to_plan_end": 0.4942989416820547,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.10513723292371017,
+        "angular_z": -0.4205489316948407
+      },
+      "ackermann_cmd": {
+        "speed": -0.10513723641633987,
+        "steering_angle": 0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 26.05791108700032,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.4019078612327576,
+        "y": 2.5457294434309006,
+        "yaw": 1.4371287822723389,
+        "odom_age_sec": 0.028805063000000002,
+        "odom_linear_x": 0.14300000667572021,
+        "odom_angular_z": -0.1852015256881714
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.15795090086491242,
+      "yaw_error_to_plan_end": 0.477131254265978,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.11117192617333546,
+        "angular_z": -0.44468770469334185
+      },
+      "ackermann_cmd": {
+        "speed": -0.11117192357778549,
+        "steering_angle": 0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 26.157592663001196,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.4035648703575134,
+        "y": 2.55741648375988,
+        "yaw": 1.4155186414718626,
+        "odom_age_sec": 0.028710313,
+        "odom_linear_x": 0.10400000214576721,
+        "odom_angular_z": -0.2313520908355713
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.16513519203183397,
+      "yaw_error_to_plan_end": 0.4555211134655017,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.11665774795980152,
+        "angular_z": -0.4666309918392061
+      },
+      "ackermann_cmd": {
+        "speed": -0.11665774881839752,
+        "steering_angle": 0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 26.25808440400033,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.4046265482902527,
+        "y": 2.563832566142082,
+        "yaw": 1.3902273178100586,
+        "odom_age_sec": 0.028933187000000003,
+        "odom_linear_x": 0.057999998331069946,
+        "odom_angular_z": -0.2578778564929962
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.16919988233829192,
+      "yaw_error_to_plan_end": 0.43022978980369775,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.11933237239579017,
+        "angular_z": -0.4773294895831607
+      },
+      "ackermann_cmd": {
+        "speed": -0.11933237314224243,
+        "steering_angle": 0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 26.357717396000226,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.4049851298332214,
+        "y": 2.5657365769147873,
+        "yaw": 1.3774770498275755,
+        "odom_age_sec": 0.028668397,
+        "odom_linear_x": 0.006000000052154064,
+        "odom_angular_z": 0.0
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.17040690585981924,
+      "yaw_error_to_plan_end": 0.4174795218212146,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.12050308734430011,
+        "angular_z": -0.48201234937720044
+      },
+      "ackermann_cmd": {
+        "speed": -0.12050309032201767,
+        "steering_angle": 0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 26.457559597,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.4049851298332214,
+        "y": 2.5657365769147873,
+        "yaw": 1.3774770498275755,
+        "odom_age_sec": 0.028543688,
+        "odom_linear_x": 0.0,
+        "odom_angular_z": 0.0
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.17040690585981924,
+      "yaw_error_to_plan_end": 0.4174795218212146,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.11996646172531276,
+        "angular_z": -0.47986584690125106
+      },
+      "ackermann_cmd": {
+        "speed": -0.11996646225452423,
+        "steering_angle": 0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 26.557897630000298,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.4049851298332214,
+        "y": 2.5657365769147873,
+        "yaw": 1.3774770498275755,
+        "odom_age_sec": 0.028753771,
+        "odom_linear_x": 0.0,
+        "odom_angular_z": 0.0
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.17040690585981924,
+      "yaw_error_to_plan_end": 0.4174795218212146,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.11996646172531276,
+        "angular_z": -0.47986584690125106
+      },
+      "ackermann_cmd": {
+        "speed": -0.11996646225452423,
+        "steering_angle": 0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 26.65774562200022,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.4048561453819275,
+        "y": 2.565090700984001,
+        "yaw": 1.3770577907562256,
+        "odom_age_sec": 0.028795771,
+        "odom_linear_x": -0.013000000268220901,
+        "odom_angular_z": -0.008364317938685417
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.17000073936809965,
+      "yaw_error_to_plan_end": 0.41706026274986474,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.11996646172531276,
+        "angular_z": -0.47986584690125106
+      },
+      "ackermann_cmd": {
+        "speed": -0.11996646225452423,
+        "steering_angle": 0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 26.757828822001102,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.4046677947044373,
+        "y": 2.5641499012708664,
+        "yaw": 1.3772766590118408,
+        "odom_age_sec": 0.028749104,
+        "odom_linear_x": -0.013000000268220901,
+        "odom_angular_z": 0.004386924672871828
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.1694122530229244,
+      "yaw_error_to_plan_end": 0.41727913100548,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.11942543943015754,
+        "angular_z": -0.47770175772063017
+      },
+      "ackermann_cmd": {
+        "speed": -0.11942543834447861,
+        "steering_angle": 0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 26.857784106001418,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.4042930006980896,
+        "y": 2.5622694939374924,
+        "yaw": 1.3799479007720947,
+        "odom_age_sec": 0.028844313000000003,
+        "odom_linear_x": -0.01899999938905239,
+        "odom_angular_z": 0.03193384036421776
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.16824514968874232,
+      "yaw_error_to_plan_end": 0.4199503727657339,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.11846665391677014,
+        "angular_z": -0.47386661566708055
+      },
+      "ackermann_cmd": {
+        "speed": -0.11846665292978287,
+        "steering_angle": 0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 26.95790043099987,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.4035324454307556,
+        "y": 2.5583899468183517,
+        "yaw": 1.3855761289596555,
+        "odom_age_sec": 0.028751771000000002,
+        "odom_linear_x": -0.039000000804662704,
+        "odom_angular_z": 0.07403668016195297
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.1658727487102404,
+      "yaw_error_to_plan_end": 0.42557860095329464,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.11681740706859343,
+        "angular_z": -0.46726962827437374
+      },
+      "ackermann_cmd": {
+        "speed": -0.116817407310009,
+        "steering_angle": 0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 27.05906367100033,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.402441442012787,
+        "y": 2.5525885075330734,
+        "yaw": 1.3976120948791502,
+        "odom_age_sec": 0.029442186000000002,
+        "odom_linear_x": -0.06499999761581421,
+        "odom_angular_z": 0.13817603886127472
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.1624076704337036,
+      "yaw_error_to_plan_end": 0.4376145668727894,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.11465766204738656,
+        "angular_z": -0.45863064818954624
+      },
+      "ackermann_cmd": {
+        "speed": -0.11465766280889511,
+        "steering_angle": 0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 27.157711457000914,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.4009217619895935,
+        "y": 2.5438259094953537,
+        "yaw": 1.418157935142517,
+        "odom_age_sec": 0.028768187,
+        "odom_linear_x": -0.09099999815225601,
+        "odom_angular_z": 0.2277909368276596
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.15735564449132877,
+      "yaw_error_to_plan_end": 0.4581604071361562,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.11097672782896062,
+        "angular_z": -0.4439069113158425
+      },
+      "ackermann_cmd": {
+        "speed": -0.11097672581672668,
+        "steering_angle": 0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 27.257707908000157,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.399413526058197,
+        "y": 2.5337241142988205,
+        "yaw": 1.448179364204407,
+        "odom_age_sec": 0.028814188,
+        "odom_linear_x": -0.10400000214576721,
+        "odom_angular_z": 0.31785914301872253
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.15178269401413727,
+      "yaw_error_to_plan_end": 0.4881818361980461,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.10704907208540694,
+        "angular_z": -0.42819628834162776
+      },
+      "ackermann_cmd": {
+        "speed": -0.10704907029867172,
+        "steering_angle": 0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 27.3591070220009,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.3981565833091736,
+        "y": 2.522922083735466,
+        "yaw": 1.4846431016921997,
+        "odom_age_sec": 0.029437978000000004,
+        "odom_linear_x": -0.11100000143051147,
+        "odom_angular_z": 0.37946420907974243
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.1460916445748869,
+      "yaw_error_to_plan_end": 0.5246455736858389,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.1030702128384735,
+        "angular_z": -0.412280851353894
+      },
+      "ackermann_cmd": {
+        "speed": -0.1030702143907547,
+        "steering_angle": 0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 27.457664183000816,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.397343337535858,
+        "y": 2.5124762505292892,
+        "yaw": 1.5252771377563474,
+        "odom_age_sec": 0.028688313000000003,
+        "odom_linear_x": -0.10400000214576721,
+        "odom_angular_z": 0.41551822423934937
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.1408296227439028,
+      "yaw_error_to_plan_end": 0.5652796097499866,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.24799564314987038,
+        "angular_z": -0.9919825725994815
+      },
+      "ackermann_cmd": {
+        "speed": 0.24799564480781555,
+        "steering_angle": -0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 27.5590371720009,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.397058427333832,
+        "y": 2.50492624938488,
+        "yaw": 1.5697704553604126,
+        "odom_age_sec": 0.029338478,
+        "odom_linear_x": -0.057999998331069946,
+        "odom_angular_z": 0.4503883719444275
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.137136580422717,
+      "yaw_error_to_plan_end": 0.6097729273540518,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.24007886379058446,
+        "angular_z": -0.9603154551623378
+      },
+      "ackermann_cmd": {
+        "speed": 0.240078866481781,
+        "steering_angle": -0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 27.657766584001365,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.3970624804496765,
+        "y": 2.5026524513959885,
+        "yaw": 1.5905449390411377,
+        "odom_age_sec": 0.028814229,
+        "odom_linear_x": -0.006000000052154064,
+        "odom_angular_z": 0.0
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.13600850347368815,
+      "yaw_error_to_plan_end": 0.6305474110347769,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.23796841744121458,
+        "angular_z": -0.9518736697648583
+      },
+      "ackermann_cmd": {
+        "speed": 0.23796841502189636,
+        "steering_angle": -0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 27.759022823000123,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.397056519985199,
+        "y": 2.5029523819684982,
+        "yaw": 1.5905449390411377,
+        "odom_age_sec": 0.029290604,
+        "odom_linear_x": 0.006000000052154064,
+        "odom_angular_z": 0.0
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.13616039614940587,
+      "yaw_error_to_plan_end": 0.6305474110347769,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.23937496611369113,
+        "angular_z": -0.9574998644547645
+      },
+      "ackermann_cmd": {
+        "speed": 0.23937496542930603,
+        "steering_angle": -0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 27.857675651001045,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.3970035910606384,
+        "y": 2.5052302330732346,
+        "yaw": 1.5968588590621946,
+        "odom_age_sec": 0.028469272,
+        "odom_linear_x": 0.03200000151991844,
+        "odom_angular_z": 0.04190538078546524
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.13733648923294023,
+      "yaw_error_to_plan_end": 0.6368613310558338,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.24080107851274443,
+        "angular_z": -0.9632043140509777
+      },
+      "ackermann_cmd": {
+        "speed": 0.24080108106136322,
+        "steering_angle": -0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 27.957982476000325,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.396817147731781,
+        "y": 2.5121386498212814,
+        "yaw": 1.596709132194519,
+        "odom_age_sec": 0.028851187,
+        "odom_linear_x": 0.08500000089406967,
+        "odom_angular_z": -0.015840711072087288
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.14109045848917354,
+      "yaw_error_to_plan_end": 0.6367116041881583,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.24657481998581873,
+        "angular_z": -0.9862992799432749
+      },
+      "ackermann_cmd": {
+        "speed": 0.24657481908798218,
+        "steering_angle": -0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 28.059203215001617,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.3965486884117126,
+        "y": 2.5229683369398117,
+        "yaw": 1.5892878770828247,
+        "odom_age_sec": 0.029369894,
+        "odom_linear_x": 0.11699999868869781,
+        "odom_angular_z": -0.08998125046491623
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.14741625201968933,
+      "yaw_error_to_plan_end": 0.6292903490764639,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.10334430436263949,
+        "angular_z": -0.41337721745055794
+      },
+      "ackermann_cmd": {
+        "speed": -0.10334430634975433,
+        "steering_angle": 0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 28.157831877000717,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.396321475505829,
+        "y": 2.5373979061841965,
+        "yaw": 1.5759320259094236,
+        "odom_age_sec": 0.028918604,
+        "odom_linear_x": 0.14300000667572021,
+        "odom_angular_z": -0.14523421227931976
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.1565144102000464,
+      "yaw_error_to_plan_end": 0.6159344979030628,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.10995712273799971,
+        "angular_z": -0.43982849095199883
+      },
+      "ackermann_cmd": {
+        "speed": -0.10995712131261826,
+        "steering_angle": 0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 28.25797424400116,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.396302878856659,
+        "y": 2.5512342900037766,
+        "yaw": 1.5583001375198364,
+        "odom_age_sec": 0.028897104000000003,
+        "odom_linear_x": 0.12399999797344208,
+        "odom_angular_z": -0.1844155490398407
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.1658066408937865,
+      "yaw_error_to_plan_end": 0.5983026095134755,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.11711510592880715,
+        "angular_z": -0.4684604237152286
+      },
+      "ackermann_cmd": {
+        "speed": -0.11711510270833969,
+        "steering_angle": 0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 28.35804281900164,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.396458327770233,
+        "y": 2.560380980372429,
+        "yaw": 1.5360143184661865,
+        "odom_age_sec": 0.028979562,
+        "odom_linear_x": 0.07100000232458115,
+        "odom_angular_z": -0.23375484347343445
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.17216835243875278,
+      "yaw_error_to_plan_end": 0.5760167904598257,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.12202606486138717,
+        "angular_z": -0.4881042594455487
+      },
+      "ackermann_cmd": {
+        "speed": -0.12202606350183487,
+        "steering_angle": 0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 28.45752922900101,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.3966163992881775,
+        "y": 2.5642693489789963,
+        "yaw": 1.5123755931854248,
+        "odom_age_sec": 0.028571230000000003,
+        "odom_linear_x": 0.03200000151991844,
+        "odom_angular_z": -0.22645032405853271
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.17488495389962594,
+      "yaw_error_to_plan_end": 0.552378065179064,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.12337829996083743,
+        "angular_z": -0.49351319984334974
+      },
+      "ackermann_cmd": {
+        "speed": -0.12337829917669296,
+        "steering_angle": 0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 28.557876512000803,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.396655261516571,
+        "y": 2.5649228543043137,
+        "yaw": 1.502489447593689,
+        "odom_age_sec": 0.028761979000000003,
+        "odom_linear_x": 0.0,
+        "odom_angular_z": 0.0
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.17533751838839737,
+      "yaw_error_to_plan_end": 0.5424919195873281,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.12373701432602721,
+        "angular_z": -0.49494805730410885
+      },
+      "ackermann_cmd": {
+        "speed": -0.12373701483011246,
+        "steering_angle": 0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 28.65799521200097,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.396655261516571,
+        "y": 2.5649228543043137,
+        "yaw": 1.502489447593689,
+        "odom_age_sec": 0.028895438000000002,
+        "odom_linear_x": 0.0,
+        "odom_angular_z": 0.0
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.17533751838839737,
+      "yaw_error_to_plan_end": 0.5424919195873281,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.12343761294543176,
+        "angular_z": -0.49375045178172705
+      },
+      "ackermann_cmd": {
+        "speed": -0.12343761324882507,
+        "steering_angle": 0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 28.75822712000081,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.396655261516571,
+        "y": 2.5649228543043137,
+        "yaw": 1.502489447593689,
+        "odom_age_sec": 0.028906188000000003,
+        "odom_linear_x": 0.0,
+        "odom_angular_z": 0.0
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.17533751838839737,
+      "yaw_error_to_plan_end": 0.5424919195873281,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.12343761294543176,
+        "angular_z": -0.49375045178172705
+      },
+      "ackermann_cmd": {
+        "speed": -0.12343761324882507,
+        "steering_angle": 0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 28.857473280000704,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.39663428068161,
+        "y": 2.5646222084760666,
+        "yaw": 1.502489447593689,
+        "odom_age_sec": 0.028671729000000003,
+        "odom_linear_x": -0.006000000052154064,
+        "odom_angular_z": 0.0
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.17513126699816386,
+      "yaw_error_to_plan_end": 0.5424919195873281,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.12343761294543176,
+        "angular_z": -0.49375045178172705
+      },
+      "ackermann_cmd": {
+        "speed": -0.12343761324882507,
+        "steering_angle": 0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 28.95779802100151,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.39661306142807,
+        "y": 2.5643196552991867,
+        "yaw": 1.502489447593689,
+        "odom_age_sec": 0.028865854000000003,
+        "odom_linear_x": -0.006000000052154064,
+        "odom_angular_z": 0.0
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.17492405753066176,
+      "yaw_error_to_plan_end": 0.5424919195873281,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.1232924119667074,
+        "angular_z": -0.4931696478668296
+      },
+      "ackermann_cmd": {
+        "speed": -0.12329240888357162,
+        "steering_angle": 0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 29.059405510000943,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.396434247493744,
+        "y": 2.561749503016472,
+        "yaw": 1.5038113594055176,
+        "odom_age_sec": 0.029647727000000002,
+        "odom_linear_x": -0.03200000151991844,
+        "odom_angular_z": 0.0199685487896204
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.1731742692749373,
+      "yaw_error_to_plan_end": 0.5438138313991567,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.12225004438379561,
+        "angular_z": -0.48900017753518243
+      },
+      "ackermann_cmd": {
+        "speed": -0.1222500428557396,
+        "steering_angle": 0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 29.15818408800078,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.3960160613059998,
+        "y": 2.55556158721447,
+        "yaw": 1.5097417831420898,
+        "odom_age_sec": 0.029107146,
+        "odom_linear_x": -0.07100000232458115,
+        "odom_angular_z": 0.0903836041688919
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.16903952916645765,
+      "yaw_error_to_plan_end": 0.549744255135729,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.11950745351751063,
+        "angular_z": -0.4780298140700425
+      },
+      "ackermann_cmd": {
+        "speed": -0.11950745433568954,
+        "steering_angle": 0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 29.259177578000163,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.3955172896385193,
+        "y": 2.5470374077558517,
+        "yaw": 1.5266035795211792,
+        "odom_age_sec": 0.029457269,
+        "odom_linear_x": -0.09099999815225601,
+        "odom_angular_z": 0.19849994778633118
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.1635009463235457,
+      "yaw_error_to_plan_end": 0.5666060515148184,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.11547601228861487,
+        "angular_z": -0.4619040491544595
+      },
+      "ackermann_cmd": {
+        "speed": -0.11547601222991943,
+        "steering_angle": 0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 29.357848198000283,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.3951441645622253,
+        "y": 2.537417933344841,
+        "yaw": 1.5542131662368774,
+        "odom_age_sec": 0.028858980000000003,
+        "odom_linear_x": -0.09099999815225601,
+        "odom_angular_z": 0.2929479479789734
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.1574275177228191,
+      "yaw_error_to_plan_end": 0.5942156382305166,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.11078091727365873,
+        "angular_z": -0.4431236690946349
+      },
+      "ackermann_cmd": {
+        "speed": -0.11078091710805893,
+        "steering_angle": 0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 29.457582398999875,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.395058810710907,
+        "y": 2.5272758454084396,
+        "yaw": 1.5876994132995603,
+        "odom_age_sec": 0.028613396000000003,
+        "odom_linear_x": -0.10400000214576721,
+        "odom_angular_z": 0.3484775722026825
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.1511775883034491,
+      "yaw_error_to_plan_end": 0.6277018852931995,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": -0.10657444469439271,
+        "angular_z": -0.42629777877757086
+      },
+      "ackermann_cmd": {
+        "speed": -0.10657444596290588,
+        "steering_angle": 0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 29.557832307000353,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.395359456539154,
+        "y": 2.5160665959119797,
+        "yaw": 1.626112103462219,
+        "odom_age_sec": 0.028818188,
+        "odom_linear_x": -0.11100000143051147,
+        "odom_angular_z": 0.3928528428077698
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.14445428727365542,
+      "yaw_error_to_plan_end": 0.6661145754558582,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.25449637718030427,
+        "angular_z": -1.017985508721217
+      },
+      "ackermann_cmd": {
+        "speed": 0.25449636578559875,
+        "steering_angle": -0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 29.658907463001015,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.396009862422943,
+        "y": 2.506193444132805,
+        "yaw": 1.6695463657379148,
+        "odom_age_sec": 0.029190729000000002,
+        "odom_linear_x": -0.08500000089406967,
+        "odom_angular_z": 0.4451490044593811
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.13867998669833903,
+      "yaw_error_to_plan_end": 0.709548837731554,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.24298678432360413,
+        "angular_z": -0.9719471372944165
+      },
+      "ackermann_cmd": {
+        "speed": 0.2429867833852768,
+        "steering_angle": -0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 29.758021749001273,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.3965036273002625,
+        "y": 2.50161175429821,
+        "yaw": 1.7135913372039795,
+        "odom_age_sec": 0.028994771000000003,
+        "odom_linear_x": -0.026000000536441803,
+        "odom_angular_z": 0.4221765398979187
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.13599320775529217,
+      "yaw_error_to_plan_end": 0.7535938091976186,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.23750353055063356,
+        "angular_z": -0.9500141222025342
+      },
+      "ackermann_cmd": {
+        "speed": 0.2375035285949707,
+        "steering_angle": -0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 29.857508867000433,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.3964595198631287,
+        "y": 2.501911923289299,
+        "yaw": 1.7135913372039795,
+        "odom_age_sec": 0.02868623,
+        "odom_linear_x": 0.006000000052154064,
+        "odom_angular_z": 0.0
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.1361763816209971,
+      "yaw_error_to_plan_end": 0.7535938091976186,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.23934804564931422,
+        "angular_z": -0.9573921825972569
+      },
+      "ackermann_cmd": {
+        "speed": 0.23934803903102875,
+        "steering_angle": -0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 29.957783608000682,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.3964595198631287,
+        "y": 2.501911923289299,
+        "yaw": 1.7135913372039795,
+        "odom_age_sec": 0.028798312000000003,
+        "odom_linear_x": 0.0,
+        "odom_angular_z": 0.0
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.1361763816209971,
+      "yaw_error_to_plan_end": 0.7535938091976186,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.23967043165295493,
+        "angular_z": -0.9586817266118197
+      },
+      "ackermann_cmd": {
+        "speed": 0.23967042565345764,
+        "steering_angle": -0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    },
+    {
+      "elapsed_sec": 30.05827722400136,
+      "pose_map": {
+        "available": true,
+        "frame_id": "map",
+        "source_frame": "odom",
+        "x": 2.3958346247673035,
+        "y": 2.506111904978752,
+        "yaw": 1.7182797193527222,
+        "odom_age_sec": 0.028883021000000002,
+        "odom_linear_x": 0.057999998331069946,
+        "odom_angular_z": 0.03450464457273483
+      },
+      "goal": null,
+      "plan_end": {
+        "x": 2.5157566366996607,
+        "y": 2.4362445430622923,
+        "yaw": 0.9599975280063608
+      },
+      "distance_to_goal": null,
+      "yaw_error_to_goal": null,
+      "distance_to_plan_end": 0.13879026337272365,
+      "yaw_error_to_plan_end": 0.7582821913463613,
+      "plan_end_to_goal": null,
+      "plan_end_yaw_to_goal": null,
+      "cmd_vel": {
+        "linear_x": 0.242403481345084,
+        "angular_z": -0.969613925380336
+      },
+      "ackermann_cmd": {
+        "speed": 0.24240347743034363,
+        "steering_angle": -0.5195767879486084
+      },
+      "planner_status": "plan_ok hybrid_astar points=121 iterations=43 obstacles=0 planning_ms=56.888 goal_error=0.066 yaw_error=0.960",
+      "tracker_status": "external_plan_ok points=121 length=3.600 goal=(2.516,2.436,0.960)"
+    }
+  ]
+}
\ No newline at end of file
diff --git a/tf_bag/metadata.yaml b/tf_bag/metadata.yaml
new file mode 100644
index 0000000..fbcf22a
--- /dev/null
+++ b/tf_bag/metadata.yaml
@@ -0,0 +1,26 @@
+rosbag2_bagfile_information:
+  version: 5
+  storage_identifier: sqlite3
+  duration:
+    nanoseconds: 71999265360
+  starting_time:
+    nanoseconds_since_epoch: 1784703364769624477
+  message_count: 598
+  topics_with_message_count:
+    - topic_metadata:
+        name: /tf
+        type: tf2_msgs/msg/TFMessage
+        serialization_format: cdr
+        offered_qos_profiles: "- history: 3\n  depth: 0\n  reliability: 1\n  durability: 2\n  deadline:\n    sec: 9223372036\n    nsec: 854775807\n  lifespan:\n    sec: 9223372036\n    nsec: 854775807\n  liveliness: 1\n  liveliness_lease_duration:\n    sec: 9223372036\n    nsec: 854775807\n  avoid_ros_namespace_conventions: false"
+      message_count: 598
+  compression_format: ""
+  compression_mode: ""
+  relative_file_paths:
+    - tf_bag_0.db3
+  files:
+    - path: tf_bag_0.db3
+      starting_time:
+        nanoseconds_since_epoch: 1784703364769624477
+      duration:
+        nanoseconds: 71999265360
+      message_count: 598
\ No newline at end of file
diff --git a/tf_bag/tf_bag_0.db3 b/tf_bag/tf_bag_0.db3
new file mode 100644
index 0000000..8586456
Binary files /dev/null and b/tf_bag/tf_bag_0.db3 differ