optimal

2026-07-30 09:24:47 +07:00
parent 4762a3032c
commit d8babff20b
50 changed files with 3280 additions and 119 deletions
--- a/launch/README.md
+++ b/launch/README.md
@@ -0,0 +1,110 @@
+# Chạy move_base2 trên sim
+
+Bộ launch này thay cho chuỗi lệnh của `move_base` cũ. Không sửa gì ở đường chạy cũ — hai runtime
+chạy song song được, chọn bằng lệnh `roslaunch` nào được gọi.
+
+## Đường chạy cũ (giữ nguyên, để đối chiếu)
+
+```bash
+roslaunch amr_startup robot_maze_world.launch
+roslaunch amr_startup rviz.launch
+roslaunch robot_depth_image_proc depth_image_proc_gazebo.launch
+export PNKX_NAV_CORE_CONFIG_DIR=/home/duongtd/T800_ws/src/AMR_T800/pnkx_nav_core/config
+roslaunch amr_startup amr_control.launch
+```
+
+## Đường chạy move_base2
+
+```bash
+cd /home/duongtd/T800_ws && source devel/setup.bash
+roslaunch move_base2 move_base2_sim.launch
+```
+
+Một lệnh, không cần `export` gì. Muốn tách terminal như cũ thì chạy từng phần:
+
+```bash
+# Terminal 1 — Gazebo + model T800 + world (dùng lại launch cũ, không đổi)
+roslaunch amr_startup robot_maze_world.launch
+
+# Terminal 2 — rviz (dùng lại config cũ)
+rviz -d $(rospack find amr_startup)/rviz/navigation.rviz
+
+# Terminal 3 — depth image -> point cloud (dùng lại launch cũ)
+roslaunch robot_depth_image_proc depth_image_proc_gazebo.launch
+
+# Terminal 4 — nav stack trên move_base2. KHÔNG export gì: launch tự đặt config dir.
+roslaunch move_base2 move_base2_control.launch
+```
+
+### Tham số
+
+| Arg | Mặc định | Ghi chú |
+|---|---|---|
+| `world` | `maze` | `maze` \| `warehouse` \| `sehc` \| `empty` |
+| `robot_type` | `T800` | quyết định footprint; `T800` = `[[0.583,±0.48],[-0.583,±0.48]]` |
+| `gui` | `true` | `false` = `gzserver` headless, chạy được qua ssh |
+| `rviz` | `true` | |
+| `depth_proc` | `true` | tắt khi không cần đường depth camera |
+
+```bash
+roslaunch move_base2 move_base2_sim.launch world:=warehouse gui:=false
+```
+
+## Đổi qua lại giữa hai runtime bằng cách nào
+
+`amr_control` nạp plugin navigation bằng cách tra symbol **`MoveBase`** — tên này hard-code trong
+host (`amr_control.cpp:244-246`), không đọc từ config. Thư viện nào được nạp thì do khoá
+`MoveBase: library_path` trong `move_base_common_params.yaml` quyết định, và file đó được tìm qua
+`PNKX_NAV_CORE_CONFIG_DIR`.
+
+Vì vậy chuyển đổi = trỏ `PNKX_NAV_CORE_CONFIG_DIR` sang cây khác:
+
+| Runtime | `PNKX_NAV_CORE_CONFIG_DIR` | `MoveBase: library_path` |
+|---|---|---|
+| cũ | `pnkx_nav_core/config` | `libmove_base` |
+| mới | `move_base2/config/runtime` | `libmove_base2` |
+
+`move_base2/config/runtime/` là **symlink tới toàn bộ** `pnkx_nav_core/config/`, trừ đúng một file
+thật: `move_base_common_params.yaml`. Chọn symlink thay vì copy là có lý do — `CLAUDE.md` cảnh báo
+rõ về hai cây config trùng tên ("file khớp đầu tiên thắng", "sửa mãi không có tác dụng vì sửa nhầm
+bản cũ"). Với symlink, sửa tham số ở cây gốc là cả hai đường chạy cùng thấy; chỉ đúng một dòng thật
+sự khác nhau.
+
+`libmove_base2.so` export **cả hai** alias `MoveBase` và `MoveBase2`, nên host không phải sửa dòng
+nào.
+
+## Xem gì khi chạy
+
+Log khởi động của `amr_node` phải có, theo thứ tự:
+
+```
+[move_base2] dùng schema mới ... | ... đọc theo schema gen-1 của move_base_common_params.yaml
+[move_base2] PlannerRunner: global planner đang dùng là '...'
+[move_base2] ControllerRunner: local planner đang dùng là '...'
+[move_base2] NavigationRuntime dựng xong: <toàn bộ config>
+```
+
+Trên rviz: `/global_costmap/costmap/plan` và `/local_costmap/costmap/footprint` có dữ liệu.
+
+**Chưa có:** lưới costmap trên rviz. `getGlobalData`/`getLocalData` của move_base2 hiện điền `plan`
+và `footprint` nhưng chưa chuyển costmap sang `OccupancyGrid` — xem `@warning` ở
+`NavigationServer::fillPlannerData`. Path và footprint hiện được, costmap thì không.
+
+## Nếu treo ở lúc khởi động
+
+`amr_control.cpp:252-258` **chờ vô hạn** `getFeedback()->is_ready` sau khi gọi `initialize()`:
+
+```cpp
+do { r.sleep(); ros::spinOnce(); } while (ros::ok() && !move_base_ptr_->getFeedback()->is_ready);
+```
+
+`NavigationRuntime::build()` hỏng thì `is_ready` giữ nguyên `false` và vòng lặp này quay mãi. Triệu
+chứng là `amr_node` im lặng không tiến thêm. Đọc log ngay phía trên nó — `initialize()` luôn in lý
+do thất bại trước khi trả về:
+
+```
+[move_base2] initialize() thất bại: <lý do>
+```
+
+Nguyên nhân hay gặp: sai `PNKX_NAV_CORE_CONFIG_DIR`, thiếu `library_path` cho planner trong YAML,
+hoặc `.so` của planner không có trong `devel/lib`.
--- a/launch/move_base2_control.launch
+++ b/launch/move_base2_control.launch
@@ -0,0 +1,66 @@
+<?xml version="1.0"?>
+<!--
+  Nav stack chạy trên move_base2. Bản độc lập, viết theo mẫu `amr_startup/launch/amr_control.launch`
+  nhưng KHÔNG include nó — để đổi thứ gì ở đây cũng không đụng vào đường chạy của move_base cũ.
+
+  Khác biệt so với bản gốc đúng hai chỗ:
+    1. `PNKX_NAV_CORE_CONFIG_DIR` trỏ vào cây overlay của move_base2 (xem config/runtime/README).
+    2. Không cần `export` thủ công nữa — launch tự đặt.
+
+  Mọi tham số PHÍA ROS (footprint, costmap, localization, mqtt) vẫn đọc từ `amr_startup/config`,
+  đúng như bản gốc: đó là cây config của phía ROS, không phải của nav core.
+-->
+<launch>
+  <arg name="robot_type" default="T800" doc="T800 | imr | hook_150 | APM" />
+  <arg name="local_planner"  default="pnkx"   doc="dwa | dwb | eband | base | teb | pose | pnkx" />
+  <arg name="global_planner" default="custom" doc="sbpl | two_points | custom" />
+  <arg name="global_plan_msg_type" default="vda5050_msgs::Order" doc="nav_msgs::Path | vda5050_msgs::Order" />
+  <arg name="primitive_filename" default="$(find amr_startup)/config/mprim/unicycle_highcost_5cm.mprim" />
+  <arg name="prefix" default="" doc="Prefix cho tf frame của robot" />
+
+  <!--
+    ĐIỂM KHÁC BIỆT DUY NHẤT của cả đường chạy: nav core đọc config từ đây.
+    Cây overlay là symlink tới toàn bộ `pnkx_nav_core/config/`, trừ đúng một file thật
+    `move_base_common_params.yaml` với `MoveBase: library_path: libmove_base2`.
+  -->
+  <env name="PNKX_NAV_CORE_CONFIG_DIR" value="$(find move_base2)/config/runtime" />
+
+  <param name="rosconsole_config_file" value="$(find amr_startup)/rosconsole.config" />
+  <rosparam file="$(find amr_startup)/config/mqtt_general.yaml" command="load" />
+
+  <node pkg="amr_control" type="amr_control_node" respawn="false" name="amr_node" output="screen" clear_params="true">
+    <rosparam param="footprint" if="$(eval robot_type == 'imr')">
+      [[0.412, -0.304], [0.412, 0.304], [-0.412, 0.304], [-0.412, -0.304]]
+    </rosparam>
+    <rosparam param="footprint" if="$(eval robot_type == 'hook_150')">
+      [[0.511,-0.1955],[0.511,0.1955],[-0.511,0.1955],[-0.511,-0.1955]]
+    </rosparam>
+    <rosparam param="footprint" if="$(eval robot_type == 'T800')">
+      [[0.583,-0.48],[0.583,0.48],[-0.583,0.48],[-0.583,-0.48]]
+    </rosparam>
+    <rosparam param="footprint" if="$(eval robot_type == 'APM')">
+      [[1.65,-0.4],[1.65,0.4],[-0.35,0.4],[-0.35,-0.4]]
+    </rosparam>
+
+    <param name="primitive_filename" value="$(arg primitive_filename)" />
+    <param name="global_plan_msg_type" value="$(arg global_plan_msg_type)" />
+
+    <rosparam file="$(find amr_startup)/config/maker_sources.yaml" command="load" />
+    <rosparam file="$(find amr_startup)/config/move_base_common_params.yaml" command="load" />
+    <rosparam file="$(find amr_startup)/config/$(arg global_planner)_global_params.yaml" command="load" />
+    <rosparam file="$(find amr_startup)/config/$(arg local_planner)_local_planner_params.yaml" command="load" />
+
+    <rosparam file="$(find amr_startup)/config/costmap_common_params.yaml" command="load" ns="global_costmap" subst_value="true" />
+    <rosparam file="$(find amr_startup)/config/costmap_global_params.yaml" command="load" />
+    <rosparam file="$(find amr_startup)/config/costmap_global_params_plugins_no_virtual_walls.yaml" command="load" />
+
+    <rosparam file="$(find amr_startup)/config/costmap_common_params.yaml" command="load" ns="local_costmap" subst_value="true" />
+    <rosparam file="$(find amr_startup)/config/costmap_local_params.yaml" command="load" subst_value="true" />
+    <rosparam file="$(find amr_startup)/config/costmap_local_params_plugins_no_virtual_walls.yaml" command="load" />
+
+    <rosparam file="$(find amr_startup)/config/localization.yaml" command="load" />
+
+    <remap from="map" to="/map" />
+    <remap from="marker" to="move_base_node/markers" />
+  </node>
+</launch>
--- a/launch/move_base2_sim.launch
+++ b/launch/move_base2_sim.launch
@@ -0,0 +1,54 @@
+<?xml version="1.0"?>
+<!--
+  Một lệnh duy nhất thay cho năm lệnh của đường chạy cũ:
+
+      roslaunch amr_startup robot_maze_world.launch
+      roslaunch amr_startup rviz.launch
+      roslaunch robot_depth_image_proc depth_image_proc_gazebo.launch
+      export PNKX_NAV_CORE_CONFIG_DIR=.../pnkx_nav_core/config
+      roslaunch amr_startup amr_control.launch
+
+  Ba phần đầu là hạ tầng mô phỏng KHÔNG đổi giữa move_base cũ và move_base2 (cùng world, cùng model
+  URDF T800, cùng rviz, cùng depth proc) nên được include lại chứ không chép lại — chép lại chỉ tạo
+  thêm một bản sao phải đồng bộ bằng tay.
+
+  Phần thứ tư và năm là thứ thực sự khác, và nằm trong `move_base2_control.launch` của gói này.
+-->
+<launch>
+  <arg name="world"      default="maze" doc="maze | warehouse | sehc | empty" />
+  <arg name="robot_type" default="T800" />
+  <arg name="gui"        default="true" doc="false = chạy gzserver headless" />
+  <arg name="rviz"       default="true" />
+  <arg name="depth_proc" default="true" doc="Bật khi cần đường depth camera -> costmap" />
+
+  <!-- 1. Gazebo + model robot T800 + world -->
+  <include file="$(find amr_startup)/launch/robot_maze_world.launch" if="$(eval world == 'maze')">
+    <arg name="gui" value="$(arg gui)" />
+    <arg name="robot_type" value="$(arg robot_type)" />
+  </include>
+  <include file="$(find amr_startup)/launch/ware_houses_world.launch" if="$(eval world == 'warehouse')">
+    <arg name="gui" value="$(arg gui)" />
+    <arg name="robot_type" value="$(arg robot_type)" />
+  </include>
+  <include file="$(find amr_startup)/launch/sehc_world.launch" if="$(eval world == 'sehc')">
+    <arg name="gui" value="$(arg gui)" />
+    <arg name="robot_type" value="$(arg robot_type)" />
+  </include>
+  <include file="$(find amr_startup)/launch/robot_empty_world.launch" if="$(eval world == 'empty')">
+    <arg name="gui" value="$(arg gui)" />
+    <arg name="robot_type" value="$(arg robot_type)" />
+  </include>
+
+  <!-- 2. rviz — dùng lại đúng config đang đọc /global_costmap/costmap, /plan, /footprint -->
+  <node if="$(arg rviz)" name="rviz" pkg="rviz" type="rviz"
+        args="-d $(find amr_startup)/rviz/navigation.rviz" />
+
+  <!-- 3. Depth image -> point cloud cho VoxelLayer -->
+  <include file="$(find robot_depth_image_proc)/launch/depth_image_proc_gazebo.launch"
+           if="$(arg depth_proc)" />
+
+  <!-- 4 + 5. Nav stack trên move_base2 (tự đặt PNKX_NAV_CORE_CONFIG_DIR) -->
+  <include file="$(find move_base2)/launch/move_base2_control.launch">
+    <arg name="robot_type" value="$(arg robot_type)" />
+  </include>
+</launch>