first commit

configs/ackermann.py

@@ -0,0 +1,35 @@
+"""Config Ackermann (xe có bánh lái). Chỉnh số liệu theo robot thực tế."""
+
+from mprim_config import RobotConfig, KinematicType
+
+config = RobotConfig(
+    name="ackermann",                       # tên file output: ackermann.mprim
+    kinematic_type=KinematicType.ACKERMANN,  # xe bánh lái -> nội suy cung tròn unicycle,
+                                             # có kiểm tra min_turning_radius
+
+    resolution_m=0.05,          # [m] PHẢI khớp costmap resolution
+    numberofangles=16,          # 16 góc heading -> 1 bước góc = 22.5 độ
+
+    forward_short_m=0.20,       # [m] bước tiến ngắn (xe dài nên bước ngắn cũng dài hơn AMR)
+    forward_long_m=0.60,        # [m] bước tiến dài
+    backward_m=0.20,            # [m] bước lùi thẳng
+
+    min_turning_radius_m=0.45,  # [m] bán kính quay tối thiểu VẬT LÝ, đo từ robot thật
+    turn_steps=(1,),            # chỉ rẽ 22.5 độ mỗi primitive
+    turn_radius_m=0.70,         # [m] cao hơn min_turning_radius làm dự phòng quantize; với
+                                #     lưới 5cm cần >= ~0.66 để cung không bị méo (validate
+                                #     sẽ cảnh báo nếu thiếu)
+
+    has_backward_prims=True,        # cho phép lùi thẳng
+    has_backward_turn_prims=True,   # xe bánh lái cần lùi + đánh lái để quay đầu
+    has_turn_in_place_prims=False,  # xe Ackermann KHÔNG quay tại chỗ được
+
+    # SỐ NGUYÊN (SBPL đọc %d). Ackermann lùi tốn hơn nhiều (khó điều khiển, hạn chế tầm nhìn).
+    forwardcostmult=1,          # tiến thẳng — mốc chuẩn
+    backwardcostmult=6,         # lùi thẳng
+    forwardandturncostmult=3,   # tiến + đánh lái
+    backwardandturncostmult=8,  # lùi + đánh lái — đắt nhất, chỉ dành cho quay đầu
+    turninplacecostmult=5,      # không dùng khi has_turn_in_place_prims=False
+
+    pose_spacing_m=0.05,        # [m] khoảng cách pose trên path
+)

configs/differential.py

@@ -0,0 +1,46 @@
+"""Config differential drive (2 bánh vi sai). Chỉnh số liệu theo robot thực tế.
+
+Cost mult lấy theo bộ tuned unicycle_highcost_5cm.mprim (backward=40, turninplace=20,
+forwardandturn=2): lùi và quay tại chỗ đắt tới mức chỉ được chọn khi không còn đường
+khác — an toàn cho AMR có cảm biến chính hướng về phía trước. Muốn robot thoải mái
+lùi/xoay hơn thì hạ dần (vd 5/5/3) nhưng đó là quyết định vận hành có chủ đích.
+"""
+
+from mprim_config import RobotConfig, KinematicType
+
+config = RobotConfig(
+    name="differential",                    # tên file output: differential.mprim
+    kinematic_type=KinematicType.DIFFERENTIAL,  # 2 bánh vi sai -> nội suy unicycle
+
+    resolution_m=0.05,          # [m] PHẢI khớp costmap resolution (costmap_global_params.yaml)
+    numberofangles=16,          # 16 góc heading -> 1 bước góc = 22.5 độ
+
+    forward_short_m=0.10,       # [m] bước tiến ngắn ~2 cell, để tinh chỉnh gần goal
+    forward_long_m=0.40,        # [m] bước tiến dài ~8 cell — quãng đường 1 chu kỳ điều khiển
+                                #     ở tốc độ hành trình
+    backward_m=0.10,            # [m] bước lùi thẳng
+
+    # Rẽ = cung tròn: (1,) nghĩa là chỉ có primitive rẽ 22.5 độ (giống bộ tham chiếu).
+    # Thêm 2 vào tuple nếu cần rẽ 45 độ — đổi lại branching factor tăng, planner chậm hơn.
+    turn_steps=(1,),
+    turn_radius_m=0.70,         # [m] >= max_vel/max_yawrate = 0.4/0.6 ~ 0.67 để local planner
+                                #     bám cung không phải giảm tốc; >= 0.66 để quantize 5cm
+                                #     không làm méo cung
+
+    has_backward_prims=True,        # cho phép lùi thẳng (cost 40 nên chỉ dùng khi kẹt)
+    has_backward_turn_prims=False,  # lùi + rẽ tắt: hành động mù cảm biến, bật lại nếu thật cần
+    has_turn_in_place_prims=True,   # cho phép quay tại chỗ (cost 20 nên hiếm khi được chọn)
+
+    # SỐ NGUYÊN (SBPL đọc %d)
+    forwardcostmult=1,          # tiến thẳng — mốc chuẩn
+    backwardcostmult=40,        # lùi 0.1m "đắt" bằng tiến 4m -> chỉ lùi khi không còn cách
+    forwardandturncostmult=2,   # rẽ đắt gấp đôi đi thẳng -> ưu tiên đường ít cua
+    backwardandturncostmult=40, # (không dùng khi has_backward_turn_prims=False)
+    turninplacecostmult=20,     # xoay 22.5 độ "đắt" bằng tiến ~4m -> tránh xoay giữa đường
+
+    pose_spacing_m=0.05,        # [m] = resolution -> path SBPL trả ra có pose cách đều ~5cm
+
+    # giới hạn động học thật (hybrid_local_planner_params.yaml: robot_max_v_ac / robot_max_w_ac)
+    max_vel_mps=0.4,            # [m/s] vận tốc dài tối đa
+    max_yawrate_radps=0.6,      # [rad/s] vận tốc xoay tối đa
+)

configs/omnidirectional.py

@@ -0,0 +1,38 @@
+"""Config omnidirectional (mecanum/omni/swerve). Chỉnh số liệu theo robot thực tế."""
+
+from mprim_config import RobotConfig, KinematicType
+
+config = RobotConfig(
+    name="omnidirectional",                 # tên file output: omnidirectional.mprim
+    kinematic_type=KinematicType.OMNIDIRECTIONAL,  # holonomic -> nội suy tuyến tính x/y/theta
+
+    resolution_m=0.05,          # [m] PHẢI khớp costmap resolution
+    numberofangles=16,          # 16 góc heading -> 1 bước góc = 22.5 độ
+
+    forward_short_m=0.10,       # [m] bước tiến ngắn ~2 cell
+    forward_long_m=0.40,        # [m] bước tiến dài ~8 cell
+    backward_m=0.10,            # [m] bước lùi thẳng
+
+    turn_steps=(1,),            # chỉ có primitive rẽ 22.5 độ; thêm 2 nếu cần rẽ 45 độ
+    turn_radius_m=0.70,         # [m] bán kính cung rẽ (omni vẫn dùng cung cho mượt)
+
+    sidestep_m=0.15,            # [m] đặc thù omni: đi ngang giữ nguyên heading
+    diagonal_m=0.15,            # [m] đặc thù omni: đi chéo 45 độ giữ nguyên heading
+
+    has_backward_prims=True,        # cho phép lùi thẳng
+    has_backward_turn_prims=True,   # omni lùi không mù như diff-drive, giữ bật
+    has_turn_in_place_prims=True,   # cho phép quay tại chỗ
+
+    # SỐ NGUYÊN (SBPL đọc %d). Omni lùi/xoay "rẻ" hơn diff-drive vì không cần quay đầu.
+    forwardcostmult=1,          # tiến thẳng — mốc chuẩn
+    backwardcostmult=3,         # lùi rẻ hơn diff-drive (robot đối xứng, không mù hẳn)
+    forwardandturncostmult=3,   # vừa đi vừa rẽ
+    backwardandturncostmult=4,  # lùi + rẽ
+    turninplacecostmult=4,      # quay tại chỗ
+    sidestepcostmult=2,         # đi ngang — hơi đắt hơn tiến để không "cua" bằng sidestep
+    diagonalcostmult=2,         # đi chéo 45 độ
+
+    pose_spacing_m=0.05,        # [m] khoảng cách pose trên path
+
+    # chưa khai báo max_vel/max_yawrate: bỏ qua sanity-check động học
+)