HiepLM/pnkx_nav_core
1
1
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

add new file nav_c_api2 and function

Browse Source
This commit is contained in:
duongtd
2026-08-06 19:50:46 +07:00
parent 565e18bfb8
commit ad28a413cc
34 changed files with 2439 additions and 216 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
CMAKE_BUILD_README.md
View File

@@ -356,7 +356,8 @@ export PNKX_NAV_CORE_CONFIG_DIR=/path/to/config
# Chỉ định workspace directory # Chỉ định workspace directory
export PNKX_NAV_CORE_DIR=/path/to/pnkx_nav_core export PNKX_NAV_CORE_DIR=/path/to/pnkx_nav_core
] không install)
# LD_LIBRARY_PATH (nếu không install)
export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/path/to/pnkx_nav_core/build/lib export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/path/to/pnkx_nav_core/build/lib
``` ```

79
CMakeLists.txt
View File

@@ -150,6 +150,83 @@ if (NOT TARGET robot_clear_costmap_recovery)
add_subdirectory(${CMAKE_SOURCE_DIR}/src/Navigations/Libraries/robot_clear_costmap_recovery) add_subdirectory(${CMAKE_SOURCE_DIR}/src/Navigations/Libraries/robot_clear_costmap_recovery)
endif() endif()
# 3. Packages being migrated from AMR_T800/Test
#
# Prefer the future in-tree location. Until the migration is physically done,
# keep the current sibling Test directory buildable from this root CMake file.
option(PNKX_NAV_CORE_BUILD_TEST_PACKAGES
"Build navigation packages currently kept in AMR_T800/Test"
ON)
option(PNKX_NAV_CORE_BUILD_CATKIN_TEST_PACKAGES
"Build Test packages that require a catkin workspace instead of standalone CMake"
OFF)
set(PNKX_NAV_CORE_TEST_PACKAGES_DIR ""
CACHE PATH "Directory containing the migrated navigation Test packages")
if(NOT PNKX_NAV_CORE_TEST_PACKAGES_DIR)
if(IS_DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}/Test")
set(PNKX_NAV_CORE_TEST_PACKAGES_DIR "${CMAKE_CURRENT_SOURCE_DIR}/Test")
elseif(IS_DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}/../Test")
set(PNKX_NAV_CORE_TEST_PACKAGES_DIR "${CMAKE_CURRENT_SOURCE_DIR}/../Test")
endif()
endif()
function(pnkx_add_test_package package_name)
set(package_dir "${PNKX_NAV_CORE_TEST_PACKAGES_DIR}/${package_name}")
if(EXISTS "${package_dir}/CMakeLists.txt")
message(STATUS "[pnkx_nav_core] Adding migrated Test package: ${package_name}")
add_subdirectory("${package_dir}" "${CMAKE_BINARY_DIR}/Test/${package_name}")
else()
message(WARNING
"[pnkx_nav_core] Test package '${package_name}' was not found in "
"'${PNKX_NAV_CORE_TEST_PACKAGES_DIR}'.")
endif()
endfunction()
if(PNKX_NAV_CORE_BUILD_TEST_PACKAGES)
if(NOT PNKX_NAV_CORE_TEST_PACKAGES_DIR)
message(FATAL_ERROR
"PNKX_NAV_CORE_BUILD_TEST_PACKAGES is ON, but no Test package directory was found. "
"Set PNKX_NAV_CORE_TEST_PACKAGES_DIR explicitly.")
endif()
# Foundation and message packages.
pnkx_add_test_package(angles)
pnkx_add_test_package(image_geometry)
pnkx_add_test_package(grid_map_core)
pnkx_add_test_package(vda5050_msgs)
# Planner libraries and plugins. SBPL must precede its lattice plugin.
pnkx_add_test_package(sbpl)
pnkx_add_test_package(sbpl_lattice_planner)
pnkx_add_test_package(base_local_planner)
pnkx_add_test_package(hybrid_local_planner)
pnkx_add_test_package(mppi_local_planner)
pnkx_add_test_package(priest_local_planner)
pnkx_add_test_package(stanley_local_planner)
# Supporting runtime packages and test utilities.
pnkx_add_test_package(depth_image_proc)
pnkx_add_test_package(action_core)
pnkx_add_test_package(recovery_core)
pnkx_add_test_package(mission_adapters)
pnkx_add_test_package(nav_test_harness)
# Must be last: it consumes planners, actions, recovery and mission APIs.
pnkx_add_test_package(move_base2)
endif()
# These packages call find_package(catkin REQUIRED ...) for T800 packages.
# Keep them outside the standalone graph until their dependencies export
# *Config.cmake files, or build them through a catkin workspace.
if(PNKX_NAV_CORE_BUILD_CATKIN_TEST_PACKAGES)
pnkx_add_test_package(deep_mpc_local_planner)
pnkx_add_test_package(nav_ros_bridge)
pnkx_add_test_package(depth_local_costmap_noetic_test)
endif()
# 2. Main packages (phụ thuộc vào cores) # 2. Main packages (phụ thuộc vào cores)
# message(STATUS "[move_base] Shared library configured") # message(STATUS "[move_base] Shared library configured")
if (NOT TARGET move_base) if (NOT TARGET move_base)
@@ -164,5 +241,3 @@ endif()
message(STATUS "========================================") message(STATUS "========================================")
message(STATUS "All packages configured successfully") message(STATUS "All packages configured successfully")
message(STATUS "========================================") message(STATUS "========================================")

6
config/config/voxel_layer_params.yaml
View File

@@ -8,4 +8,8 @@ voxel_layer:
unknown_threshold: 15.0 unknown_threshold: 15.0
mark_threshold: 0 mark_threshold: 0
combination_method: 1 combination_method: 1
frustum_clearing_enabled: true
frustum_clearing_pixel_step: 8
frustum_min_range: 0.20
frustum_max_range: 3.0
frustum_depth_camera_topic: /camera/depth/data

52
config/costmap_common_params.yaml
View File

@@ -1,5 +1,7 @@
robot_base_frame: base_link robot_base_frame: base_link
transform_tolerance: 1.0 transform_tolerance: 1.0
performance_metrics_enabled: true
performance_metrics_period: 5.0
obstacle_range: 3.0 obstacle_range: 3.0
#mark_threshold: 1 #mark_threshold: 1
publish_voxel_map: true publish_voxel_map: true
@@ -19,7 +21,7 @@ virtual_walls_map:
lethal_cost_threshold: 100 lethal_cost_threshold: 100
obstacles: obstacles:
observation_sources: b_scan_marking b_scan_clearing pc_marking pc_clearing observation_sources: b_scan pc_marking pc_clearing pc_r_marking pc_r_clearing
# f_scan_marking: f_scan_marking f_scan_clearing # f_scan_marking: f_scan_marking f_scan_clearing
# topic: /f_scan # topic: /f_scan
# data_type: LaserScan # data_type: LaserScan
@@ -36,20 +38,13 @@ obstacles:
# inf_is_valid: true # inf_is_valid: true
# min_obstacle_height: 0.0 # min_obstacle_height: 0.0
# max_obstacle_height: 0.25 # max_obstacle_height: 0.25
b_scan_marking: b_scan:
topic: /b_scan
data_type: LaserScan
clearing: false
marking: true
inf_is_valid: true
min_obstacle_height: 0.0
max_obstacle_height: 0.25
b_scan_clearing:
topic: /b_scan topic: /b_scan
data_type: LaserScan data_type: LaserScan
clearing: true clearing: true
marking: false marking: true
inf_is_valid: true inf_is_valid: true
frustum_clearing_enabled: false
min_obstacle_height: 0.0 min_obstacle_height: 0.0
max_obstacle_height: 0.25 max_obstacle_height: 0.25
@@ -59,6 +54,7 @@ obstacles:
clearing: false clearing: false
marking: true marking: true
inf_is_valid: false inf_is_valid: false
frustum_clearing_enabled: false
observation_persistence: 0.0 observation_persistence: 0.0
expected_update_rate: 0.5 expected_update_rate: 0.5
obstacle_range: 2.5 obstacle_range: 2.5
@@ -67,14 +63,38 @@ obstacles:
max_obstacle_height: 1.0 max_obstacle_height: 1.0
pc_clearing: pc_clearing:
topic: /camera/depth/points_clear topic: /camera/depth/data
data_type: DepthCameraData
clearing: false
marking: true
frustum_clearing_enabled: true
frustum_clearing_pixel_step: 8
frustum_min_range: 0.20
frustum_max_range: 3.5
pc_r_marking:
topic: /camera_right/depth/points_proc
data_type: PointCloud2 data_type: PointCloud2
clearing: true clearing: false
marking: false marking: true
inf_is_valid: false inf_is_valid: false
frustum_clearing_enabled: false
observation_persistence: 0.0 observation_persistence: 0.0
expected_update_rate: 0.5 expected_update_rate: 0.5
obstacle_range: 2.5 obstacle_range: 2.5
raytrace_range: 3.0 raytrace_range: 3.0
min_obstacle_height: -0.5 min_obstacle_height: 0.1
max_obstacle_height: 5.0 max_obstacle_height: 1.0
pc_r_clearing:
topic: /camera_right/depth/data
data_type: DepthCameraData
clearing: false
marking: true
frustum_clearing_enabled: true
frustum_clearing_pixel_step: 8
frustum_min_range: 0.20
frustum_max_range: 3.5
# Depth camera clearing is handled by VoxelLayer frustum clearing from
# /camera/depth/image_raw + /camera/depth/camera_info. /camera_right/depth/data

49
config/hybrid_local_planner_params.yaml
View File

@@ -1,32 +1,17 @@
LocalPlannerAdapter:
library_path: liblocal_planner_adapter
yaw_goal_tolerance: 0.017
xy_goal_tolerance: 0.03
min_approach_linear_velocity: 0.06
HybridLocalPlanner: HybridLocalPlanner:
# base_local_planner: "hybrid_local_planner/HybridLocalPlanner" # base_local_planner: "hybrid_local_planner/HybridLocalPlanner"
# HybridLocalPlanner: # HybridLocalPlanner:
library_path: libhybrid_local_planner library_path: libhybrid_local_planner
# Robot # Robot
robot_max_v_ac: 0.4 max_forward_velocity: 1.0
robot_max_w_ac: 0.6 max_reverse_velocity: 0.25 # [m/s], positive magnitude
robot_max_v_pt: 1.0 avoidance_max_velocity: 0.4
robot_max_w_pt: 0.6 max_angular_velocity: 0.6
robot_min_v: -0.5 min_in_place_angular_velocity: 0.3
robot_min_w: 0.05 acc_lim_x: 0.1
robot_max_v_backwards_pt: -0.25 decel_lim_x: 0.5
acc_lim_x: 1.0
acc_lim_theta: 2.0 acc_lim_theta: 2.0
min_turn_radius: 0.0 min_turn_radius: 0.0
turn_around_priority: true
stop_dist: 0.5
dec_dist: 1.0
max_acceleration: 0.1
max_deceleration: 0.5
max_d_yawrate: 3.2
max_in_place_yawrate: 0.6
min_in_place_yawrate: 0.3
robot_radius: 0.1 robot_radius: 0.1
footprint_padding: 0.08 footprint_padding: 0.08
use_footprint: true use_footprint: true
@@ -39,26 +24,32 @@ HybridLocalPlanner:
max_global_plan_lookahead_dist: 3.0 max_global_plan_lookahead_dist: 3.0
global_plan_viapoint_sep: 0.5 global_plan_viapoint_sep: 0.5
global_plan_goal_sep: 0.05 global_plan_goal_sep: 0.05
global_plan_prune_distance: 0.0
predict_time: 3.0 predict_time: 3.0
sim_period: 0.1 sim_period: 0.1
sim_time_samples: 10 sim_time_samples: 10
sim_direction: M_PI / 2.0
# Optimization # Optimization
w_vel: 0.8
w_omega: 2.5 w_omega: 2.5
obs_cost_gain: 1.3 obs_cost_gain: 1.3
path_cost_gain: 2.0 path_cost_gain: 0.5
to_goal_cost_gain: 0.8 to_goal_cost_gain: 0.8
speed_cost_gain: 0.8 speed_cost_gain: 0.5
# Obstacles # Obstacles
obs_range: 2.5 obs_range: 2.5
#GeneralSetting #GeneralSetting
segment_transition_threshold: 0.01 segment_transition_threshold: 0.03 # [m], also used as DWA stop distance for intermediate segments
calibration_factor: 1.5 rotation_segment_yaw_threshold: 0.05 # [rad], ignore tiny yaw noise at duplicate positions
direction_change_hysteresis: 0.15 # normalized dot product
use_obstacle_avoidance: true use_obstacle_avoidance: true
slow_velocity_th: 0.1 slow_velocity_th: 0.1
turn_direction_th: 0.1 turn_direction_th: 0.1
avoidance_clear_hold_time: 0.5 # [s]
rejoin_blend_time: 0.8 # [s]
rejoin_max_velocity: 0.25 # [m/s]
rejoin_heading_tolerance: 0.35 # [rad]
rejoin_path_tolerance: 0.20 # [m]
pp_max_omega_correction: 0.15 # [rad/s]
rejoin_acc_lim_theta: 1.0 # [rad/s^2]
rejoin_lookahead_distance: 0.8 # [m]

42
config/mission_adapters_params.yaml Normal file
View File

@@ -0,0 +1,42 @@
# Tham số RUNTIME của mission layer (gói `mission_adapters`).
#
# Đây là bản đang có hiệu lực. Bản trong `Test/mission_adapters/test/config/` chỉ phục vụ test và
# chỉ được đọc khi chạy kèm PNKX_NAV_CORE_CONFIG_DIR trỏ vào đúng thư mục đó.
mission_adapters:
# Nguồn mission. Thêm một loại nguồn mới = thêm một entry ở đây + một plugin .so, không phải sửa
# code của gói.
# name: tên instance, cũng là namespace param riêng của nó
# type: tên symbol export bằng BOOST_DLL_ALIAS, phải có khoá library_path tương ứng bên dưới
mission_sources:
- {name: goal_src, type: GoalSourceAdapter}
- {name: vda5050_src, type: VDA5050SourceAdapter}
# Trần thời gian cho MỘT chặng (navigation + action), tính từ lúc chặng được giao. [s]
# 0 = tắt. Quá hạn thì chặng bị đánh dấu thất bại VÀ navigation được bảo dừng — lưới cuối cho
# trường hợp navigation không bao giờ báo kết quả về.
mission_timeout: 0.0
# Một chặng thất bại thì xử lý phần còn lại của hàng đợi thế nào.
# true = xoá sạch hàng đợi (mặc định, an toàn cho tuyến đường tuần tự kiểu VDA5050: không tới
# được node n thì chạy tiếp chặng n+1 là cắt ngang đoạn chưa được cho phép đi)
# false = chỉ bỏ chặng lỗi rồi chạy tiếp. Chỉ đặt false khi các mission trong hàng đợi ĐỘC LẬP
# với nhau, không phải các chặng của cùng một tuyến.
clear_queue_on_failure: true
# Param riêng của từng instance nguồn mission, đặt theo `name` ở trên.
vda5050_src:
# Frame gán cho goal/start sinh ra từ nodePosition của VDA5050.
# Lưu ý: `mapId` trong VDA5050 là danh tính bản đồ, KHÔNG phải frame TF, nên không dùng làm frame.
global_frame: map
# Bảng symbol -> thư viện cho Boost.DLL.
#
# Thiếu khoá library_path là nguyên nhân phổ biến nhất của lỗi "plugin build xong nhưng runtime báo
# không tìm thấy". Tên không có đuôi .so được resolve qua PNKX_NAV_CORE_LIBRARY_PATH / devel/lib /
# LD_LIBRARY_PATH — tức phải `source devel/setup.bash` trước khi chạy.
GoalSourceAdapter:
library_path: libmission_adapters_goal_source
VDA5050SourceAdapter:
library_path: libmission_adapters_vda5050_source

51
config/move_base_common_params.yaml
View File

@@ -1,43 +1,18 @@
# position_planner_name: PNKXLocalPlanner position_planner_name: PriestLocalPlanner #HybridLocalPlanner MPPILocalPlanner PriestLocalPlanner PNKXLocalPlanner
# docking_planner_name: PNKXDockingLocalPlanner docking_planner_name: PNKXDockingLocalPlanner #StanleyDockingLocalPlanner PNKXDockingLocalPlanner
# position_planner_name: StanleyLocalPlanner
# docking_planner_name: StanleyDockingLocalPlanner
position_planner_name: MPPILocalPlanner #HybridLocalPlanner MPPILocalPlanner
docking_planner_name: PNKXDockingLocalPlanner
go_straight_planner_name: PNKXGoStraightLocalPlanner go_straight_planner_name: PNKXGoStraightLocalPlanner
rotate_planner_name: PNKXRotateLocalPlanner rotate_planner_name: PNKXRotateLocalPlanner
base_local_planner: LocalPlannerAdapter base_local_planner: LocalPlannerAdapter
base_global_planner: CustomPlanner base_global_planner: CustomPlanner
# PNKXLocalPlanner: PriestLocalPlanner:
# base_local_planner: LocalPlannerAdapter
# base_global_planner: SBPLLatticePlanner
# PNKXDockingLocalPlanner:
# base_local_planner: LocalPlannerAdapter
# base_global_planner: CustomPlanner
# StanleyLocalPlanner:
# base_local_planner: LocalPlannerAdapter
# base_global_planner: CustomPlanner
# StanleyDockingLocalPlanner:
# base_local_planner: LocalPlannerAdapter
# base_global_planner: CustomPlanner
# HybridLocalPlanner:
# base_local_planner: LocalPlannerAdapter
# base_global_planner: CustomPlanner
MPPILocalPlanner:
base_local_planner: LocalPlannerAdapter base_local_planner: LocalPlannerAdapter
base_global_planner: SBPLLatticePlanner base_global_planner: CustomPlanner #CustomPlanner SBPLLatticePlanner
PNKXDockingLocalPlanner: PNKXDockingLocalPlanner:
base_local_planner: LocalPlannerAdapter base_local_planner: LocalPlannerAdapter
base_global_planner: TwoPointsPlanner base_global_planner: TwoPointsPlanner
PNKXGoStraightLocalPlanner: PNKXGoStraightLocalPlanner:
base_local_planner: LocalPlannerAdapter base_local_planner: LocalPlannerAdapter
base_global_planner: TwoPointsPlanner base_global_planner: TwoPointsPlanner
@@ -47,18 +22,28 @@ PNKXRotateLocalPlanner:
base_global_planner: TwoPointsPlanner base_global_planner: TwoPointsPlanner
### replanning ### replanning
controller_frequency: 30.0 # run controller at 15.0 Hz controller_frequency: 30.0 # run controller at 30.0 Hz
controller_patience: 0.0 # if the controller failed, clear obstacles and retry; after 15.0 s, abort and replan controller_patience: 0.0 # if the controller failed, clear obstacles and retry; after 15.0 s, abort and replan
planner_frequency: 0.0 # don't continually replan (only when controller failed) planner_frequency: 0.0 # don't continually replan (only when controller failed)
planner_patience: 2.0 # if the first planning attempt failed, abort planning retries after 5.0 s... planner_patience: 2.0 # if the first planning attempt failed, abort planning retries after 5.0 s...
max_planning_retries: 0 # ... or after 10 attempts (whichever happens first) max_planning_retries: 0 # ... or after 10 attempts (whichever happens first)
oscillation_timeout: -1 # abort controller and trigger recovery behaviors after 30.0 s oscillation_timeout: -1 # abort controller and trigger recovery behaviors after 30.0 s
oscillation_distance: 0.5 oscillation_distance: 0.5
### recovery behaviors ## recovery behaviors
recovery_behavior_enabled: true #
# Recovery của move_base cũ đã dừng: bộ behavior gen-2 (tick-based) khai ở
# `recovery_behaviors_params.yaml` và do `recovery_core::RecoveryRegistry` nạp, không phải khoá
# `recovery_behaviors` ở đây.
#
# Danh sách gen-1 đã được gỡ hẳn thay vì để lại: các entry cũ trỏ tên alias `RotateRecovery` /
# `ClearCostmapRecovery` vào file .so gen-2, trong khi loader ở đây import theo chữ ký gen-1
# (`robot_nav_core::RecoveryBehavior`). Boost.DLL không kiểm kiểu qua ranh giới .so, nên hai bên
# không bao giờ gặp nhau ở compile time và lỗi chỉ hiện ra lúc chạy. Giữ lại khoá cũng làm hai file
# config tranh nhau cùng một tên alias.
recovery_behavior_enabled: false
recovery_behaviors: [ recovery_behaviors: [
{name: aggressive_reset, type: ClearCostmapRecovery}, {name: aggressive_reset, type: ClearCostmapRecovery},
{name: conservative_reset, type: ClearCostmapRecovery} {name: conservative_reset, type: ClearCostmapRecovery},
] ]
conservative_reset: conservative_reset:

192
config/mppi_local_planner_params.yaml
View File

@@ -7,120 +7,92 @@ LocalPlannerAdapter:
MPPILocalPlanner: MPPILocalPlanner:
library_path: libmppi_local_planner library_path: libmppi_local_planner
# Robot limits [m/s, rad/s, m/s^2, rad/s^2] # Robot limits [m/s, rad/s]
max_velocity: 0.5
min_velocity: 0.0 min_velocity: 0.0
max_y_velocity: 0.0 # Cao hon reference_velocity mot khoang dem (headroom). Neu de sat ref thi
min_command_velocity: 0.05 # nua tren cua nhieu sampling bi clamp o max, keo van toc trung binh xuong
max_angular_velocity: 1.9 # (do duoc: ref0.45/max0.50 -> chi dat 0.33; max0.65 -> dat 0.41). Robot chay
min_angular_velocity: 0.05 # duoc >0.6 m/s nen dat 0.65.
max_acceleration: 0.5 max_velocity: 0.65
max_angular_acceleration: 1.0 max_angular_velocity: 1.0
max_vel_trans: 0.5 robot_radius: 0.30
obstacle_point_radius: 0.08
footprint_padding: 0.05
# MPPI core
# Horizon = model_dt * time_steps = 3.6 s ~ 1.8 m @0.5 m/s.
# Truoc day 1.4 s (0.7 m) qua ngan: robot chi "thay" vat can khi da sat, khong con
# cho de vong qua -> hoac ket hoac lang nhang. Do thuc te: 0.7m/1.2m khong toi dich;
# 1.8m toi dich voi quang duong chi dai hon 6% so voi duong thang.
# Khong tang dt qua 0.15: dt=0.20 lam dao dong tang gap 4 lan.
model_dt: 0.2
time_steps: 20
batch_size: 220
# So thread OpenMP. De mac dinh (= 16 core) thi tranh chap voi gazebo/rviz/perception
# lam 1 chu ky solveMPPI cham tu 0.9 ms len ~25 ms.
num_threads: 4
param_lambda: 100.0
param_alpha: 0.98
param_exploration: 0.05
vx_std: 0.2 vx_std: 0.2
vy_std: 0.2 wz_std: 0.25
wz_std: 0.4 reference_velocity: 0.48
motion_model: 0
min_turning_radius: 0.2
# Goal tolerance [m, rad] # Path and goal
xy_goal_tolerance: 0.1 # Phai lon hon horizon (1.8 m), neu khong terminal goal_weight se keo robot ve
yaw_goal_tolerance: 0.1 # diem cat cua reference path va lam no giam toc moi chu ky.
stopped_xy_velocity: 0.03
stopped_theta_velocity: 0.03
# MPPI horizon and global-plan handling
model_dt: 0.05
time_steps: 56
batch_size: 1000
iteration_count: 1
temperature: 0.3
gamma: 0.1
retry_attempt_limit: 10
regenerate_noises: false
open_loop: false
controller_frequency: 30.0
prune_distance: 1.5
max_robot_pose_search_dist: 0.0
max_global_plan_lookahead_dist: 3.0 max_global_plan_lookahead_dist: 3.0
global_plan_prune_distance: 0.2 xy_goal_tolerance: 0.10
sim_period: 0.1 yaw_goal_tolerance: 0.10
# Costmap safety # Costmap and cost weights
use_costmap_collision_check: true obstacle_range: 2.5
allow_unknown: true obstacle_cost_threshold: 150
lethal_cost: 253 obstacle_cell_step: 2
inflation_score_distance: 0.3 max_obstacle_points: 180
consider_footprint: true obstacle_influence_radius: 1.0
# Cost vat can gio CONG DON tren tung cell (truoc day lay max), nen thang nay phai
# nho hon nhieu. Giu 85 voi che do sum lam cost vot len ~180x85, ap dao moi thanh
# phan khac -> dao dong manh va di vong xa 45%. Do thuc te tai T=24/dt=0.15:
# weight=10 cho dao dong thap nhat va khoang ho 0.48 m (du 13 cm).
obstacle_cost_use_sum: true
obstacle_weight: 10.0
collision_cost: 1000000.0
progress_weight: 8.0
terminal_progress_weight: 20.0
goal_weight: 30.0
# Phat v^2: gan 0 vi no truc tiep keo van toc xuong (phan truc giac voi robot
# muon chay nhanh). Giu nho de van co chut smoothing.
linear_control_weight: 0.2
angular_control_weight: 0.2
control_change_weight: 1.0
# Giam toc khi lech huong: ha tu 0.8 xuong 0.3. Truoc day khi ne vat can
# (yaw error lon) robot bo <0.1 m/s. 0.3 van giam toc vao cua nhung khong bo.
yaw_error_slowdown: 0.1
min_tracking_velocity: 0.08
# Critic defaults ported from the legacy MPPI dynamic_reconfigure files. # Phat hien ket: MPPI co the tra lenh hop le nhung ~0 mai o cuc tieu dia phuong.
constraint_enabled: true # Qua nguong nay controller bao that bai -> move_base PLANNING roi CLEARING.
constraint_power: 1.0 stuck_velocity_threshold: 0.03
constraint_weight: 4.0 stuck_omega_threshold: 0.05
stuck_patience_cycles: 40
cost_enabled: true # --- Covariance thich nghi (giai doan 2) ---
cost_power: 1.0 # Moi chu ky fit lai do rong nhieu cho TUNG buoc thoi gian tu chinh batch.
cost_weight: 3.81 # vx_std/wz_std o tren chi con la gia tri khoi tao va gia tri sau reset.
cost_critical_cost: 300.0 adaptive_covariance: true
cost_near_collision_cost: 253 gaussian_fitting_lambda: 5.0 # phai < param_lambda de trong so du nhon
cost_collision_cost: 1000000.0 # San chon bang thuc nghiem: thap hon -> muot hon nhung cham va sat vat can hon.
cost_near_goal_distance: 1.0 min_vx_std: 0.08
cost_trajectory_point_step: 2 max_vx_std: 0.35
min_wz_std: 0.10
goal_enabled: true max_wz_std: 0.60
goal_power: 1.0 # Khi bi ket, no rong sigma dan qua tung chu ky de thoat cuc tieu dia phuong.
goal_weight: 5.0 stuck_sigma_inflation: 1.6
goal_threshold_to_consider: 1.4 hard_obstacle_cost_threshold: 254
stage_xy_weight: 60.0
goal_angle_enabled: true stage_yaw_weight: 1.0
goal_angle_power: 1.0 stage_speed_weight: 12.0
goal_angle_weight: 3.0 terminal_xy_weight: 90.0
goal_angle_threshold_to_consider: 0.5 terminal_yaw_weight: 1.0
terminal_speed_weight: 12.0
obstacles_enabled: false
obstacles_power: 1.0
obstacles_weight: 0.0
obstacles_repulsion_weight: 1.5
obstacles_collision_cost: 10000.0
obstacles_collision_margin_distance: 0.1
obstacles_near_goal_distance: 0.5
path_align_enabled: true
path_align_power: 1.0
path_align_weight: 8.0
path_align_max_path_occupancy_ratio: 0.07
path_align_threshold_to_consider: 0.5
path_align_offset_from_furthest: 20
path_align_trajectory_point_step: 4
path_align_use_path_orientations: false
path_angle_enabled: true
path_angle_power: 1.0
path_angle_weight: 2.0
path_angle_max_angle_to_furthest: 1.2
path_angle_threshold_to_consider: 0.5
path_angle_offset_from_furthest: 4
path_angle_mode: 0
path_follow_enabled: true
path_follow_power: 1.0
path_follow_weight: 5.0
path_follow_threshold_to_consider: 1.4
path_follow_offset_from_furthest: 6
prefer_forward_enabled: true
prefer_forward_power: 1.0
prefer_forward_weight: 1.0
prefer_forward_threshold_to_consider: 0.5
twirling_enabled: false
twirling_power: 1.0
twirling_weight: 0.0
twirling_threshold_to_consider: 0.5
velocity_deadband_enabled: false
velocity_deadband_power: 1.0
velocity_deadband_weight: 0.0
velocity_deadband_deadband_velocities: [0.0, 0.0, 0.0]
publish_debug: false

16
config/mprim/mqtt_general.yaml
View File

@@ -1,8 +1,8 @@
MQTT: # MQTT:
Name: T800 # Name: T800
Host: 172.20.235.170 # Host: 172.20.235.170
Port: 1885 # Port: 1885
Client_ID: T800 # Client_ID: T800
Username: robotics # Username: robotics
Password: robotics # Password: robotics
Keep_Alive: 60 # Keep_Alive: 60

56
config/priest_local_planner_params.yaml Normal file
View File

@@ -0,0 +1,56 @@
PriestLocalPlanner:
library_path: libpriest_local_planner
# ================= Horizon / optimizer =================
t_fin: 7.0 # [s] planning horizon
num_steps: 30 # trajectory samples over the horizon
num_batch: 60 # trajectories per CEM iteration
initial_up_sampling: 5 # candidate oversampling factor
ellite_num: 40 # elites ranked by projection residual
num_warm: 20 # warm-started trajectories between cycles
max_proj_iter: 8 # alternating projection iterations
max_cem_iter: 4 # CEM iterations per control cycle
optimizer_threads: 4 # bounded workers; avoid severe overhead from using all CPU cores
solve_time_warn_ms: 30.0 # [ms] leaves margin inside the 30 Hz controller period
lateral_std: 0.8 # [m] std of lateral waypoint perturbations
# ================= Robot limits (differential drive) =================
max_velocity: 0.5 # [m/s] forward velocity limit
max_angular_velocity: 0.4 # [rad/s] also enforced inside the optimizer as a
# lateral-acceleration clamp |a_n| <= omega_max * v
max_acceleration: 0.5 # [m/s^2] used in the trajectory projection
reference_velocity: 0.4 # [m/s] desired cruise speed along the path
v_eps: 0.05 # [m/s] below this speed the curvature clamp/cost is skipped
v_min_seed: 0.05 # [m/s] initial-velocity seed along yaw when the robot is stopped
# Trajectory -> differential (v, omega) conversion with curvature feedforward:
# omega = clamp(omega_traj, +-max_angular_velocity) - heading_gain * heading_error
heading_gain: 1.5 # [1/s] feedback gain on the heading error
heading_error_stop: 1.2 # [rad] above this, rotate in place (v = 0)
min_in_place_yawrate: 0.2 # [rad/s]
# ================= Goal / plan =================
xy_goal_tolerance: 0.10 # [m]
yaw_goal_tolerance: 0.10 # [rad]
max_global_plan_lookahead_dist: 3.0 # [m]
# ================= Obstacles (from local costmap) =================
num_obs: 40 # obstacles in the CEM cost
num_obs_proj: 30 # closest obstacles inside the projection
obstacle_range: 2.5 # [m] costmap scan radius around the robot
robot_radius: 0.50 # [m]
obstacle_point_radius: 0.08 # [m]
footprint_padding: 0.05 # [m]
obstacle_cost_threshold: 150
obstacle_cell_step: 2
max_obstacle_points: 150
# ================= Cost weights =================
weight_smoothness: 0.1
weight_track: 0.2
weight_obs: 1.2
weight_clearance: 3.0 # bounded proximity term in (0,1]: 1 at an obstacle
# centre, 0.5 on the collision boundary, 0 out of range
weight_angular: 0.5 # hinge penalty on |omega| > max_angular_velocity
cem_lamda: 0.9 # CEM temperature
cem_alpha: 0.7 # CEM mean/cov blending factor

170
config/recovery_behaviors_params.yaml Normal file
View File

@@ -0,0 +1,170 @@
# Bộ recovery behavior (gen-2, tick-based) và tham số vận hành của chúng.
#
# Đây là cây config RUNTIME. Bản dùng cho test của gói nằm ở
# `Test/recovery_core/test/config/recovery_behaviors_params.yaml` — sửa tham số vận hành thì sửa ở
# đây, đừng sửa bản test.
#
# Danh sách này được `recovery_core::RecoveryRegistry` đọc; caller (RecoveryRunner của move_base2)
# truyền namespace `recovery` vào. Khoá `recovery_behaviors:` trong `move_base_common_params.yaml`
# thuộc về move_base cũ và KHÔNG liên quan tới file này.
recovery:
# `behaviors` là registry mọi plugin/instance CÓ THỂ dùng. Thứ tự ở đây chỉ là thứ tự nạp,
# không còn quyết định luồng chạy. `routes` bên dưới chọn riêng chuỗi theo trigger.
#
# wait : không di chuyển. Vật cản động (người, xe khác) tự đi qua là xong.
# clear : xoá vật cản đã tích trong costmap, gần trước rồi xa sau.
# rotate : quay tại chỗ cho costmap nhìn lại xung quanh.
# back_up: LÙI — hướng robot không có sensor, nên xếp cuối cùng.
behaviors:
- {name: wait, type: WaitRecovery}
- {name: clear, type: ClearCostmapRecovery}
# Đặt schema trước plugin: chưa có `DetourPathRecovery`/library_path thì RecoveryRunner bỏ
# entry này cùng warning, và bỏ nó khỏi từng route. Khi plugin SBPL được thêm sau này, không
# cần sửa move_base2 hay route.
- {name: detour_path, type: DetourPathRecovery}
- {name: rotate, type: RotateRecovery}
- {name: back_up, type: BackUpRecovery}
# trigger -> tên behavior thử tuần tự; lần lỗi kế tiếp cùng trigger mới tiến tới phần tử tiếp
# theo của route đó. Đường quay về sau khi một behavior kết thúc (contract 2026-08-04b):
# - planning_failed -> quay về PLANNING lập plan lại (không có plan nào để bám);
# - controlling/oscillation-> quay về CONTROLLING bám tiếp GLOBAL PATH CŨ, KHÔNG lập plan lại
# (thay plan trong lúc chạy là việc riêng của planner_frequency);
# riêng khi leo thang từ PLANNING vì mất pose thì về PLANNING;
# - behavior họ detour (output kPath, vd DetourPathRecovery) nộp path mới -> path đó được áp
# và robot bám tiếp — cách duy nhất một recovery được thay plan.
# Robot đi được quá `oscillation_distance` thì mọi cursor route được reset — ngân sách recovery
# đầy lại theo tiến độ thật (2026-08-04).
#
# `controlling_failed` từ 2026-08-04 nghĩa là MẤT DỮ LIỆU/LỆNH: controller không ra lệnh (kể cả
# mất pose) quá controller_patience, hoặc mất pose ngay trong PLANNING. Route chỉ có [wait] là
# chủ đích: đứng yên chờ một nhịp cho dữ liệu quay lại; hết wait mà dữ liệu vẫn chưa về thì
# cursor cạn -> ABORTED, robot dừng hẳn và báo fail; dữ liệu về thì lập plan chạy tiếp.
# `planning_failed` giữ đúng nghĩa "planner CÓ dữ liệu nhưng không tìm được đường".
# `path_blocked` / `off_path` là TUỲ CHỌN: xoá hai dòng đó = tắt giám sát tuyến, runtime chạy y
# như trước 2026-08-04d. Chúng dùng chung `detour_path` nhưng giữ cursor riêng — mỗi nguyên nhân
# một ngân sách, và log phân biệt được vì sao robot dừng.
routes:
planning_failed: [wait, clear]
controlling_failed: [detour_path]
oscillation: [wait]
# Cursor reset (2026-08-05): recovery THÀNH CÔNG + NỘP PATH MỚI (detour) thì cursor của trigger
# đó reset NGAY — lần chắn kế tiếp là sự cố mới, detour được chạy lại. Từ chối/thất bại mới ăn
# vào ngân sách route. Route được phép LẶP behavior nếu muốn cấp thêm lượt thử giữa các nhịp
# wait/nhích-lại-gần.
#
# Cạn route KHÔNG còn nghĩa là ABORT (contract 2026-08-05e): `path_blocked`/`off_path` là DỰ
# BÁO, plan vẫn hợp lệ và controller vẫn ra lệnh được, nên robot bám tiếp plan cũ và trigger bị
# KHOÁ tới khi đi được quá `oscillation_distance`. Đường ABORT hợp lệ duy nhất đi qua
# `controlling_failed` (controller thật sự hết ra lệnh quá controller_patience) hoặc
# `planning_failed`.
path_blocked: [detour_path]
off_path: [detour_path]
wait:
wait_duration: 3.0 # [s] đợi vật cản động đi qua
# Xoá vật cản trong vùng vuông cạnh reset_distance quanh robot.
clear:
reset_distance: 3.0 # [m] cạnh vùng xoá
invert_area_to_clear: false # false = xoá BÊN TRONG vùng
affected_maps: both # local | global | both
layer_names: [obstacles] # phải khớp `plugins:` của costmap; sai tên -> log kèm tên layer thật
rotate:
full_rotation: true # quay đủ 2*pi để costmap thấy toàn bộ xung quanh
angular_speed: 0.4 # [rad/s] độ lớn; dấu do goal.angle quyết định
acc_lim_theta: 0.8 # [rad/s^2] ramp, tránh giật khi có tải
sim_granularity: 0.1 # [rad] bước quét footprint dọc cung lúc start
timeout: 20.0 # [s] lưới cuối nếu robot bị giữ cơ học
back_up:
backup_distance: 0.1 # [m] quãng lùi mặc định
backup_distance_max: 1.0 # [m] trần cứng, chặn cả goal.distance lẫn param
linear_speed: 0.1 # [m/s] ĐỘ LỚN; dấu âm (lùi) do plugin đặt
acc_lim_x: 0.3 # [m/s^2]
timeout: 15.0 # [s]
# Lập đường vòng quanh vật cản (gói sbpl_recovery, họ kPath) rồi NỐI LẠI plan hiện hành — đường
# vòng được bám ngay, không replan (contract 2026-08-04b). Chưa nằm trong route nào ở trên: thêm
# `detour_path` vào route cần nó (vd `controlling_failed: [wait, detour_path]`) để kích hoạt.
detour_path:
# Instance planner RIÊNG, không dùng chung SBPLLatticePlanner của backup: detour gọi makePlan
# ĐỒNG BỘ trên control thread, nên `allocated_time` của instance này là trần treo control loop
# mỗi tick. Xem khối SBPLDetourPlanner ở cuối file.
planner_name: SBPLDetourPlanner # namespace param riêng (khối bên dưới)
planner_symbol: SBPLLatticePlanner # alias Boost.DLL CÓ THẬT trong .so — libsbpl_lattice_planner
# chỉ export đúng một symbol này. Khác bộ local planner, nơi
# mỗi biến thể là một class riêng nên tên instance trùng alias.
# "combined" (2026-08-05, vòng 3 — LƯỚI GỘP, xem DETOUR_MERGED_GRID_PLAN.md): snapshot global
# làm nền (tường/kệ tĩnh toàn bản đồ) + đè cửa sổ local lên (vật cản sensor tươi), SBPL lập
# đường TRÊN BẢN CHỤP GỘP — không giữ mutex costmap nào trong lúc search, map update không bị
# bỏ đói dù attempt thua. Một bên báo chắn là chắn; local FREE không xoá tường global.
# Cần planner export capability "<planner_symbol>ExternalGrid" (sbpl_lattice_planner có sẵn).
# "local" / "global" đơn lẻ vẫn dùng được cho hệ thiếu một trong hai nguồn.
costmap_source: combined
planning_distance: 1.0 # [m] điểm nối cách robot tối thiểu chừng này dọc plan.
# [m] TRẦN quét dọc plan, thuần tuỳ chọn. 0 = quét tới CUỐI PLAN (lưới gộp phủ toàn bản đồ nên
# không còn ràng buộc cửa sổ local). Quét luôn tự dừng ở rìa lưới; hai lần sim 2026-08-05 cap
# hữu hạn (3.8 rồi 8.0) đều cắt quét ngay sau pose bẩn cuối và báo "không có chỗ sạch" oan.
max_rejoin_distance: 0.0
sample_step: 0.5 # [m] khoảng cách dọc plan giữa hai ứng viên điểm nối
# [m] Điểm nối phải cách pose bẩn gần nhất CẢ HAI PHÍA dọc plan chừng này, và nằm SAU CỤM CHẮN
# ĐẦU TIÊN (không phải pose bẩn cuối toàn tầm — vết bẩn xa trên đuôi plan cũ không được giết
# ứng viên hợp lệ giữa hai vật cản; sim 11:00: `last blocked at 7.96 m` -> refuse oan -> ABORT).
rejoin_clearance: 0.5
attempts_per_run: 3 # số ứng viên thử tối đa; mỗi control cycle thử MỘT ứng viên
# Tuyến để NỐI LẠI (2026-08-05e). "reference" = tuyến GỐC của chặng, tức plan do planner sinh và
# recovery không thay được: đường vòng luôn quay về tuyến order, và `max_deviation` đo đúng hành
# lang fleet đã duyệt. "current" = plan đang bám (hành vi cũ) — sim đo được tuyến bị ăn mòn dần
# qua từng lượt detour (rejoin 106/305 -> 130/289 -> 132/273) và robot không bao giờ về tuyến.
rejoin_on: reference
# [m] Hành lang lệch tuyến cho phép quanh tuyến gốc của order. 0 = KHÔNG giới hạn — giữ 0 cho
# tới khi biết dung sai thật của fleet master; đặt số mò còn tệ hơn tắt.
max_deviation: 0.0
timeout: 10.0 # [s] trần lượt
# Bảng symbol -> thư viện cho Boost.DLL. Thiếu khoá library_path là nguyên nhân phổ biến nhất của
# lỗi "plugin build xong nhưng runtime báo không tìm thấy".
WaitRecovery:
library_path: librecovery_core_wait_recovery
ClearCostmapRecovery:
library_path: librecovery_core_clear_costmap_recovery
RotateRecovery:
library_path: librecovery_core_rotate_recovery
BackUpRecovery:
library_path: librecovery_core_back_up_recovery
DetourPathRecovery:
library_path: libsbpl_recovery
# Instance SBPL RIÊNG cho detour — cùng .so với SBPLLatticePlanner, khác tuning.
#
# Vì sao không dùng chung: `DetourPathRecovery::onUpdate` gọi makePlan ĐỒNG BỘ trên control thread,
# nên `allocated_time` ở đây là trần thời gian control loop bị treo mỗi tick — `cancel`/`pause` từ
# host không được xử lý trong khoảng đó. Bản backup để 10.0 s là hợp lý cho vai trò của nó, nhưng
# treo loop 10 s thì không.
#
# `initial_epsilon` lớn = ARA* trả nghiệm đầu rất nhanh, đường dài hơn tối ưu nhưng đây là đường
# vòng tạm — nhanh quan trọng hơn ngắn.
SBPLDetourPlanner:
# KHÔNG có `library_path` ở đây: `SBPLDetourPlanner` không phải symbol, nó chỉ là namespace param.
# Việc nạp .so đi theo `planner_symbol: SBPLLatticePlanner` ở trên.
environment_type: XYThetaLattice
planner_type: ARAPlanner
allocated_time: 0.4 # [s] TRẦN CỨNG cho một tick recovery
initial_epsilon: 2.0
force_scratch_limit: 10000
forward_search: true
# false cho DETOUR, khác bản backup: free heading làm mỗi lượt hỏng chạy HAI lần search (log sim:
# "no solution with free start heading, retrying..." = 0.77 s giữ mutex costmap, map update miss
# nhịp). Detour chạy khi robot ĐANG BÁM plan nên heading thật đã xuôi theo tuyến — không cần nới.
free_start_heading: true
nominalvel_mpersecs: 0.3
timetoturn45degsinplace_secs: 1.31
# Cùng file .mprim với SBPLLatticePlanner: resolution 0.05 m khớp resolution costmap local.
primitive_filename: /home/duongtd/rl_ws/mir_robot/mir_navigation/mprim/unicycle_highcost_5cm.mprim

6
config/sbpl_global_params.yaml
View File

@@ -6,6 +6,10 @@ SBPLLatticePlanner:
initial_epsilon: 1.0 initial_epsilon: 1.0
force_scratch_limit: 10000 force_scratch_limit: 10000
forward_search: true forward_search: true
# Bỏ ràng buộc heading xuất phát: local planner đã có bước quay tại chỗ đầu path
# (turn_around_priority) nên không cần SBPL vẽ cung quay đầu khi goal ở phía sau.
# Nếu không ra nghiệm, planner tự retry một lần với heading thật của robot.
free_start_heading: true
nominalvel_mpersecs: 0.3 nominalvel_mpersecs: 0.3
timetoturn45degsinplace_secs: 1.31 # = 0.6 rad/s timetoturn45degsinplace_secs: 1.31 # = 0.6 rad/s
primitive_filename: /home/duongtd/T800_ws/src/AMR_T800/Test/sbpl_lattice_planner/matlab/mprim/unicycle_highcost_5cm.mprim primitive_filename: /home/duongtd/rl_ws/mir_robot/mir_navigation/mprim/unicycle_highcost_5cm.mprim

107
config/stanley_local_planner_params.yaml
View File

@@ -5,10 +5,111 @@ LocalPlannerAdapter:
min_approach_linear_velocity: 0.06 min_approach_linear_velocity: 0.06
StanleyLocalPlanner: StanleyLocalPlanner:
# Algorithm
library_path: libstanley_local_planner library_path: libstanley_local_planner
# ================= Robot limits =================
max_vel_x: 0.40 # [m/s] forward, > 0
min_vel_x: -0.25 # [m/s] reverse, < 0
wheel_base: 0.0 # [m] 0 = derive from TF front_axle_frame -> rear_axle_frame
front_axle_frame: "steer_link"
rear_axle_frame: "base_link"
# Two SEPARATE steering limits.
# Path tracking must not use a large angle: forward_vel_ scales with cos(delta) so it
# collapses to speed_base, and vel_steer = v/cos(steer) is guarded to 0 as cos -> 0.
# Pivoting in place uses the full 90 deg the hardware has.
steer_limit_standstill: 1.5708 # [rad] TO BE MEASURED - ceiling at |v| ~ 0
steer_limit_at_max_vel: 0.5000 # [rad] TO BE MEASURED - ceiling at |v| = max_vel_x
align_steer_angle: 1.5708 # [rad] steering while pivoting. Literal number, NOT M_PI/2
min_steer_angle: 0.20 # [rad] threshold for "steering is engaged"
max_steer_rate: 0.90 # [rad/s] TO BE MEASURED - steering servo slew
# ================= Goal tolerance =================
xy_goal_tolerance: 0.030 # [m] should match LocalPlannerAdapter
yaw_goal_tolerance: 0.020 # [rad]
goal_sq_dist_tol: 0.05 # [m^2] early exit of the closest-point scan
# ================= Control law (Stanley) =================
path_yaw_mode: "tangent" # tangent | bearing - "bearing" is the old behaviour, kept for rollback
control_gain_e: 2.0 # heading gain (path tangent - robot yaw)
control_gain_d: 1.0 # base cross-track gain
gain_cte_weight: 0.8 # added to k per |cross-track error| [1/m]
gain_heading_weight: 0.5 # added to k per |heading error| [1/rad]
gain_min: 0.5 # lower clamp of k
gain_max: 1.8 # upper clamp of k
soft_min_vel: 0.20 # [m/s] FLOOR of the atan2(k*e, v) denominator. NO ceiling
alpha_gain: 0.0 # steering low-pass [0..1]; 0 = off, max_steer_rate is used instead
curve_gain_e_bonus: 2.0 # added to control_gain_e in curve mode
tangent_window_pts: 3 # points averaged for the tangent when the plan carries no yaw
# ================= Steering bias (kills the steady-state offset) =================
# Enable when the robot tracks parallel to the global path because of a mechanical
# zero offset or an IMU bias. Start at 0 and raise by 0.005 per trial.
steer_bias_gain: 0.0 # [rad/(m*s)] 0 = OFF. Suggested when enabled: 0.02
steer_bias_clamp: 0.05 # [rad] anti-windup ceiling
steer_bias_decay: 0.995 # per-cycle bleed when the integrate conditions do not hold
steer_bias_min_vel: 0.30 # [m/s] below this, do not integrate
steer_bias_max_heading_err: 0.10 # [rad] above this, do not integrate
# ================= Plan window / lookahead =================
max_global_plan_lookahead_dist: 3.0 # [m] window ceiling
min_lookahead: 0.50 # [m] window floor
min_lookahead_near_goal: 0.40 # [m] separate floor near the goal
dyn_lookahead_gain: 2.50 # [m/(m/s)] lookahead = min + gain*|v|
near_goal_freeze_dist: 0.50 # [m] below this: freeze yaw_path, run theta_d only
# Index continuity thresholds, given in METRES and converted to indices with the
# point spacing measured at runtime - the plan resolution is the global planner's
# choice, not a constant.
# The old code used fixed indices (back=3, fwd=15 ~ 0.75 m on a 0.05 m/point plan) and
# therefore never fired: at 1.2 m/s the robot covers 0.04 m = 0.8 index per 33 ms cycle.
# Applied to BOTH closest (window anchor) and target_idx_ (inside the core).
max_advance_dist: 0.20 # [m] forward travel allowed along the plan per cycle
max_back_dist: 0.10 # [m] backward travel allowed along the plan per cycle
# ================= Curvature =================
high_curvature: 0.30 # [1/m] above: shrink lookahead
mid_curvature: 0.20 # [1/m] above: cut the transform window
low_curvature: 0.10 # [1/m] above: enter curve mode
straight_confirm_steps: 110 # consecutive straight cycles before leaving curve mode
# ================= Speed profile =================
speed_base: 0.30 # [m/s] base of the cosine steering profile
curve_max_speed: 0.25 # [m/s] ceiling while curving
min_drive_speed: 0.07 # [m/s] floor while driving
accel_distance: 2.00 # [m] ramp length after leaving a curve
decel_min: 0.05 # [m/s per cycle] deceleration on predicted collision
decel_max: 0.10 # [m/s per cycle]
collision_horizon_factor: 2.0 # multiplies max_vel_x to get the collision check distance
# ================= Align / phase =================
heading_eps: 0.020 # [rad] heading error accepted as aligned
steer_eps: 0.020 # [rad] steering error accepted as "servo arrived"
heading_align_tol: 0.050 # [rad] gate for the final straight-line approach (was 0.01 = too tight)
near_goal_dist: 1.00 # [m] below this: enable the final straight mode
steer_realign_off: 0.503 # [rad] above this: CUT drive (0.16*pi)
steer_realign_on: 0.150 # [rad] below this: RESUME drive - must be smaller than _off
steering_active: 0.094 # [rad] above this the steering counts as active (0.03*pi)
segment_transition_threshold: 0.010 # [m]
segment_end_ratio: 0.05 # fraction of the segment left before advancing
xy_reached_gate_factor: 3.0 # multiplies xy_goal_tolerance; a 1-point window may only latch inside this
# ================= TF guard =================
# Measured on the robot: 225 map->odom jumps, max 12.381 m, coinciding with the
# localisation FinishTrajectory(1) 10:41:18 / AddTrajectory(2) 10:41:21. The source is
# outside this package; this is containment, not a cure.
tf_lookup_at_pose_stamp: true # true = look up at pose.header.stamp instead of "latest".
# "latest" returns a different entry each call when the
# buffer holds two map->odom transforms
transform_tolerance: 0.20 # [s] accepted age when looking up by timestamp
max_plan_tf_jump_rate: 0.60 # [m/s] threshold as a RATE, multiplied by the real elapsed time
max_plan_tf_yaw_jump: 0.20 # [rad] rotation threshold - the old code checked translation only
tf_reject_window_cycles: 30 # sliding window; exceeding it means a real relocalisation
tf_reject_ratio: 0.5 # reject fraction inside the window that triggers the reset
# ================= Debug =================
debug_level: 1 # 0=off 1=on state change 2=throttled 1 Hz 3=every cycle
# 3 produces >2000 lines/min at 30 Hz - field debugging only
StanleyDockingLocalPlanner: StanleyDockingLocalPlanner:
# Algorithm
library_path: libstanley_local_planner library_path: libstanley_local_planner

859
src/APIs/c_api/NAV_C_API_2_GUIDE.md Normal file
View File

@@ -0,0 +1,859 @@
# `nav_c_api_2` — hướng dẫn cho khung C#
Tài liệu cho phía host .NET/C# khi chuyển nền navigation từ `libmove_base.so` sang
`libmove_base2.so`.
Bám sát mã nguồn tại thời điểm viết:
`pnkx_nav_core/src/APIs/c_api`, `Test/move_base2`, `pnkx_nav_core/src/Navigations/Packages/move_base`
(bản cũ, dùng để đối chiếu).
---
## 0. Đọc nhanh — 8 việc phải làm ở phía C#
| # | Việc | Vì sao |
|---|---|---|
| 1 | Gọi `navigation_add_static_map` **trước** `navigation_initialize` | Planner lattice chặn tới khi costmap có kích thước khác 0, và tự `exit(1)` khi chờ quá lâu — giết cả tiến trình |
| 2 | Gọi `navigation_set_robot_footprint` **trước** `navigation_initialize` | Bản cũ gọi trước là rơi mất; bản mới cache lại và áp đúng lúc dựng costmap |
| 3 | Thay `navigation_initialize` bằng `navigation_initialize_checked` | Hàm cũ luôn trả `true`, kể cả khi lõi dựng hỏng |
| 4 | Thêm `navigation_shutdown` vào đường tắt, **trước** `navigation_destroy` | Bản mới có control thread + thread costmap + thread mission đang chạy |
| 5 | Bỏ mọi suy đoán từ giá trị trả về của `navigation_set_twist_linear/_angular` | Giờ luôn trả `true` nếu số hợp lệ; tín hiệu "đang cancel" của bản cũ không còn |
| 6 | Bỏ mọi so sánh chuỗi `feed_back_str`, chuyển sang so enum | Nội dung chuỗi do lõi mới đặt, khác hoàn toàn bản cũ |
| 7 | Không coi `navigation_pause` trả về là robot đã dừng | Lệnh chỉ được ghi nhận, có hiệu lực ở cycle sau (≤ 33 ms @ 30 Hz) |
| 8 | Gọi các hàm `navigation_free_*` mới sau mỗi getter cấp bộ nhớ | Trước đây không có hàm giải phóng nào cho grid/scan/cloud/plan — mỗi lời gọi rò một lần |
Ba việc **nên** làm thêm: dùng `navigation_get_state` thay cho `navigation_get_feedback` ở vòng poll
(không cấp phát chuỗi); dùng `navigation_add_depth_camera_data` để bật clear theo frustum; dùng
`navigation_move_to_order_v2` nếu fleet master có gửi order update.
---
## 1. Bố cục — và các hàm cũ nằm ở đâu
**Một thư viện `.so` duy nhất.** `nav_c_api_2` không phải một thư viện khác, không phải một handle
khác, không phải một tầng bọc. Nó là thêm file nguồn vào **cùng** thư viện đó.
```
libnav_c_api.so
├── nav_c_api.cpp → 50 hàm cũ (không sửa, không dịch lại, không bọc)
├── convertor.cpp → order_free
├── nav_c_api_2.cpp → 13 hàm mới
└── convertor_2.cpp → converter cho depth camera
```
64 symbol, cùng một `.so`, không hàm nào trùng tên hàm nào.
**Vậy các hàm không đổi thì C# gọi thế nào? Y hệt hôm nay, không sửa một dòng.** Cùng
`DllImport("nav_c_api")`, cùng tên symbol, cùng chữ ký, cùng struct. C# không dùng file header —
P/Invoke nối theo **tên symbol** trong `.so`, mà tên đó không đổi. Lớp binding hiện có tiếp tục chạy
nguyên trạng.
Cái C# phải thêm chỉ là **13 `DllImport` mới + 4 struct mới** (phụ lục B). Cái C# phải *sửa* là hành
vi, không phải khai báo — đúng 8 việc ở mục 0.
Hai header chỉ dành cho bên gọi C/C++:
| File | Nội dung |
|---|---|
| `include/nav_c_api.h` | Bề mặt cũ. Không đổi chữ ký hàm nào. |
| `include/nav_c_api_2.h` | Phần thêm. `#include` luôn `nav_c_api.h`, nên bên gọi C/C++ chỉ cần include file này. |
Tên thư viện sinh ra là `libnav_c_api.so` (target `nav_c_api`), nên phía C# là
`[DllImport("nav_c_api")]`. Tài liệu cũ có chỗ ghi `libnavigation_c_api.so` — kiểm tra lại chuỗi
khung hiện tại đang dùng.
Mọi hàm dùng `CallingConvention.Cdecl`. `bool` của C++ là 1 byte, phải khai
`[return: MarshalAs(UnmanagedType.I1)]`. `size_t` trên Linux x64 là 8 byte — dùng `UIntPtr`.
---
## 2. Điều kiện vận hành trước khi chạy
| Việc | Giá trị |
|---|---|
| `PNKX_NAV_CORE_CONFIG_DIR` | Trỏ vào `Test/move_base2/config/runtime` — **không** phải `pnkx_nav_core/config` |
| YAML chọn plugin | `MoveBase: library_path: libmove_base2` (đã có sẵn trong `move_base_common_params.yaml`) |
| Đường tìm `.so` | `libmove_base2.so` và toàn bộ plugin planner/recovery/costmap phải nằm trong `PNKX_NAV_CORE_LIBRARY_PATH`, `devel/lib` hoặc `LD_LIBRARY_PATH` |
| Nhịp control loop | `controller_frequency: 30.0` — mọi độ trễ "một cycle" nói trong tài liệu này là ~33 ms |
`navigation_create()` tra symbol `"MoveBase"`. `move_base2` export cả `MoveBase` lẫn `MoveBase2`, nên
đổi runtime chỉ tốn một dòng YAML, không phải sửa C#.
---
## 3. Trình tự khởi tạo
Thứ tự này **bắt buộc**, không phải khuyến nghị. Sai thứ tự ở bước 3 làm chết cả tiến trình.
```
1. tf3_buffer_create() → có buffer TF
2. navigation_create() → nạp libmove_base2.so, chưa chạy gì
3. bơm TF liên tục vào buffer → bắt đầu NGAY, chạy suốt vòng đời
4. navigation_set_robot_footprint() → trước initialize
5. navigation_add_static_map("/map", ...) → BẮT BUỘC trước initialize
6. navigation_initialize_checked() → nặng, chặn, tự start control thread
7. kiểm tra kết quả trả về → false thì đọc navigation_get_status_text()
8. bắt đầu vòng bơm sensor định kỳ
9. từ đây mới gửi goal
```
```csharp
// ---- 1..2 -------------------------------------------------------------------------------
tfHandle = tf3_buffer_create();
navHandle = navigation_create();
if (navHandle == IntPtr.Zero)
throw new InvalidOperationException("navigation_create failed — kiểm tra library_path của MoveBase");
// ---- 3 ----------------------------------------------------------------------------------
// TF phải chảy TRƯỚC initialize: costmap lấy pose robot ngay trong chu kỳ cập nhật đầu tiên.
StartTfPump();
// ---- 4 ----------------------------------------------------------------------------------
// Footprint theo chiều ngược kim đồng hồ, toạ độ trong frame base_link, đơn vị mét.
var footprint = new[]
{
new Point { x = 0.3, y = -0.2, z = 0.0 },
new Point { x = 0.3, y = 0.2, z = 0.0 },
new Point { x = -0.3, y = 0.2, z = 0.0 },
new Point { x = -0.3, y = -0.2, z = 0.0 },
};
navigation_set_robot_footprint(navHandle, footprint, (UIntPtr)footprint.Length);
// ---- 5 ----------------------------------------------------------------------------------
// Tên "/map" phải khớp map_topic trong costmap_common_params.yaml.
navigation_add_static_map(navHandle, "/map", occupancyGrid);
// ---- 6..7 -------------------------------------------------------------------------------
// Chặn hàng giây: dựng 2 costmap, nạp planner/controller/recovery, dựng mission layer,
// start control thread. Không gọi từ UI thread.
if (!navigation_initialize_checked(navHandle, tfHandle))
{
IntPtr reason = navigation_get_status_text(navHandle);
string text = Marshal.PtrToStringAnsi(reason) ?? "unknown";
nav_c_api_free_string(reason);
throw new InvalidOperationException($"navigation core not ready: {text}");
}
// ---- 8..9 -------------------------------------------------------------------------------
StartSensorPumps();
```
**Vì sao static map phải đi trước.** `initialize()` nạp planner ở pha hai.
`SBPLLatticePlanner::initialize` chặn tới khi costmap báo kích thước khác 0, mà kích thước đó chỉ có
khi một static map đã tới `StaticLayer`. Nạp planner trước khi map vào được là khoá chết: planner chờ
map, map chờ planner xong. SBPL `exit(1)` sau 2 giây. `move_base2` xử lý bằng cách cất static map đã
nhận rồi **phát lại** ngay trước khi nạp planner — nhưng nó chỉ phát lại được cái host đã gửi.
**Về `map_save_` / `map_name_save_`.** Hai biến public đó là cách bản cũ bù cho đúng vấn đề trên. Với
`move_base2` host **không cần** đụng tới chúng.
---
## 4. Bơm dữ liệu: cái gì, tên gì, tần số nào
Tên truyền vào các hàm `navigation_add_*` phải khớp **giá trị `topic`** khai trong
`Test/move_base2/config/runtime/costmap_common_params.yaml`, không phải khoá của observation source.
Sai tên thì dữ liệu được nhận rồi bị bỏ ở tầng layer, im lặng.
| Dữ liệu | Hàm | Tên (`topic` trong YAML) | Tần số | Bắt buộc |
|---|---|---|---|---|
| Bản đồ tĩnh | `navigation_add_static_map` | `/map` | Khi bản đồ đổi | **Có** — và trước `initialize` |
| Laser sau | `navigation_add_laser_scan` | `/b_scan` | Theo nguồn (~10–40 Hz) | Có, nếu dùng laser |
| Point cloud depth | `navigation_add_point_cloud2` | `/camera/depth/points_proc` | Theo camera | Tuỳ cấu hình |
| Point cloud depth phải | `navigation_add_point_cloud2` | `/camera_right/depth/points_proc` | Theo camera | Tuỳ cấu hình |
| Depth camera (clear frustum) | `navigation_add_depth_camera_data` | `/camera/depth/data` | Theo camera (~10–15 Hz) | Cần nếu muốn clear ghost |
| Depth camera phải | `navigation_add_depth_camera_data` | `/camera_right/depth/data` | Theo camera | Cần nếu muốn clear ghost |
| Odometry | `navigation_add_odometry` | tên tự do | Theo nguồn odometry | **Có** |
| TF | qua `tf3` buffer, không qua API này | — | Liên tục | **Có** |
Ghi chú quan trọng:
- **Odometry giờ là dữ liệu điều khiển, không phải dữ liệu hiển thị.** `move_base2` lấy vận tốc đo
được từ đó và đẩy xuống controller mỗi cycle. Bơm thưa hoặc bỏ qua là controller mất feedback vận
tốc.
- **Nguồn `/camera/depth/data` khai `data_type: DepthCameraData` và `frustum_clearing_enabled: true`.**
Chỉ `navigation_add_depth_camera_data` đẩy vào được nguồn này. Point cloud không thay thế được —
không có intrinsics thì không dựng được frustum, không clear được.
- **Không bơm nhanh hơn nguồn thật.** Đường depth tốn CPU nhất trong các đường sensor.
- **Toàn bộ đường sensor không thread-safe.** Xem mục 8.
---
## 5. Trình tự tắt
```csharp
public void Dispose()
{
// 1. Dừng MỌI thread C# đang gọi navigation_add_* và đợi chúng thoát hẳn.
sensorCts.Cancel();
Task.WaitAll(sensorTasks, TimeSpan.FromSeconds(2));
// 2. Quiesce lõi khi tiến trình còn sống đầy đủ. Trả về trong khoảng 1 giây.
// Sau bước này lõi không phát lệnh vận tốc nữa, nhưng getter vẫn gọi được.
navigation_shutdown(navHandle);
// 3. Thả instance.
navigation_destroy(navHandle);
navHandle = IntPtr.Zero;
// 4. Buffer TF sau cùng — lõi còn tham chiếu nó cho tới hết bước 3.
tf3_buffer_destroy(tfHandle);
tfHandle = IntPtr.Zero;
}
```
Không được đảo bước 1 và 2, và không được để `navigation_destroy` chạy trong finalizer/GC: thời điểm
đó do runtime .NET quyết định, có thể rơi vào lúc nó đã tháo dỡ một phần trong khi control thread vẫn
đang chạy.
---
## 6. Tham chiếu từng hàm
Ký hiệu cột "Đổi so với bản cũ": **=** giữ nguyên · **~** cùng chữ ký, khác hành vi · **+** hàm mới.
### 6.1. Vòng đời
| | Hàm | Đổi |
|---|---|---|
| | `NavigationHandle navigation_create(void)` | = |
Nạp `libmove_base2.so` qua symbol `MoveBase` và tạo một instance. Trả `NULL` khi không tìm được
library (sai `library_path`, sai `PNKX_NAV_CORE_CONFIG_DIR`, hoặc `.so` không nằm trong đường tìm).
Chưa chạy thread nào.
| | Hàm | Đổi |
|---|---|---|
| | `bool navigation_initialize(NavigationHandle, TFListenerHandle)` | ~ |
Dựng costmap, nạp planner/controller/recovery, dựng mission layer, start control thread. **Chặn**,
tốn hàng giây. **Luôn trả `true`** — contract bên dưới trả `void` nên không có gì để trả về. Dùng
`navigation_initialize_checked` thay thế.
| | Hàm | Đổi |
|---|---|---|
| | `bool navigation_initialize_checked(NavigationHandle, TFListenerHandle)` | + |
Như trên, nhưng đọc lại cờ sẵn sàng của lõi và trả về đúng kết quả thật. Trả `false` khi bất kỳ pha
nào hỏng: dựng costmap, cấu hình sensor, nạp plugin, cấu hình control loop, start control thread.
Lý do cụ thể lấy bằng `navigation_get_status_text`.
| | Hàm | Đổi |
|---|---|---|
| | `bool navigation_is_ready(NavigationHandle)` | + |
Lõi đã sẵn sàng nhận goal chưa. Rẻ, poll được. Không cấp phát gì.
| | Hàm | Đổi |
|---|---|---|
| | `void navigation_shutdown(NavigationHandle)` | + |
Dừng control thread, thread cập nhật costmap và thread mission — **không huỷ instance**. Sau lời gọi
này mọi getter vẫn an toàn. Idempotent. Không ném exception ra ngoài biên C.
| | Hàm | Đổi |
|---|---|---|
| | `void navigation_destroy(NavigationHandle)` | ~ |
Thả instance. Bản cũ gọi thẳng là chấp nhận được vì nó không override `shutdown()`. Bản mới **phải**
gọi `navigation_shutdown` trước.
### 6.2. Trạng thái và phản hồi
| | Hàm | Đổi |
|---|---|---|
| | `bool navigation_get_feedback(NavigationHandle, NavFeedback &out)` | ~ |
Trả trạng thái, chuỗi mô tả, pose 2D hiện tại, cờ `goal_checked`, cờ `is_ready`.
- `feed_back_str` được `strdup`, **phải** giải phóng bằng `nav_c_api_free_string`.
- **Nội dung `feed_back_str` khác bản cũ** — là chuỗi lý do của control loop mới. Chỉ dùng để log.
- `navigation_state` là nguồn duy nhất nên rẽ nhánh.
- Giữa hai chặng của một order nhiều node, lõi ở `SUCCEEDED`/`PENDING` nhưng API **cố ý báo `ACTIVE`**.
Nhờ vậy suy "order xong" từ `SUCCEEDED` vẫn đúng — đừng tìm cách đi vòng qua lớp che này.
| | Hàm | Đổi |
|---|---|---|
| | `bool navigation_get_state(NavigationHandle, NavigationState *out)` | + |
Chỉ lấy enum trạng thái, không cấp phát chuỗi. Dùng cho vòng poll. `navigation_get_feedback` gọi ở
10 Hz nghĩa là 10 lần `strdup` + 10 lần `free` mỗi giây, và một lần quên `free` là một lần rò.
| | Hàm | Đổi |
|---|---|---|
| | `char *navigation_get_status_text(NavigationHandle)` | + |
Chuỗi mô tả của lõi, hoặc `NULL`. Giải phóng bằng `nav_c_api_free_string`. Chỉ để log — nội dung
không phải hợp đồng.
| | Hàm | Đổi |
|---|---|---|
| | `char *navigation_state_to_string(NavigationState)` | = |
Tên trạng thái dạng chuỗi. Giải phóng bằng `nav_c_api_free_string`. (Hàm này có export nhưng chưa
được khai báo trong `nav_c_api.h` — khai báo thủ công phía C# nếu cần.)
### 6.3. Footprint
| | Hàm | Đổi |
|---|---|---|
| | `bool navigation_set_robot_footprint(NavigationHandle, const Point *pts, size_t n)` | ~ |
Đặt hình robot, toạ độ trong frame `base_link`, đơn vị mét, thứ tự ngược kim đồng hồ.
- **Bản cũ:** áp ngay vào hai costmap; gọi trước `initialize` thì **rơi mất im lặng**.
- **Bản mới:** ghi nhận rồi áp trong `initialize` và ở cycle sau nếu đổi. **Nên gọi trước
`initialize`** để planner đầu tiên thấy đúng hình robot.
| | Hàm | Đổi |
|---|---|---|
| | `bool navigation_get_robot_footprint(NavigationHandle, Point *out, size_t &out_count)` | ~ |
- **Bản cũ:** trả footprint của costmap — có giá trị (từ YAML) kể cả khi host chưa từng set.
- **Bản mới:** trả đúng cái host đã set. **Rỗng nếu host chưa gọi `set_robot_footprint`.**
Cảnh báo về chữ ký: mảng do bên gọi cấp mà **không có tham số dung lượng** — hàm không biết mảng có
đủ chỗ không. Cấp dư (ví dụ 64 điểm) khi gọi.
### 6.4. Lệnh di chuyển
Tất cả trả `true` khi lệnh được nhận, không phải khi robot tới nơi. Theo dõi kết quả bằng
`navigation_get_state`.
| | Hàm | Đổi |
|---|---|---|
| | `bool navigation_move_to(NavigationHandle, PoseStamped goal)` | ~ |
Đi tới một pose. Quaternion không hợp lệ bị từ chối ngay tại API. Trong `move_base2` goal này đi qua
mission layer như một mission một chặng, để dùng chung đường huỷ và vòng đời với order VDA5050; mission
layer tắt thì rơi xuống đường trực tiếp.
Dung sai `xy`/`yaw` đến từ profile trong YAML, không đặt được theo từng goal (bản cũ có tham số dung
sai trong contract nhưng `c_api` chưa bao giờ truyền).
| | Hàm | Đổi |
|---|---|---|
| | `bool navigation_move_to_order(NavigationHandle, Order order, PoseStamped goal)` | ~ |
Gửi order VDA5050 đầy đủ. Trong `move_base2` order được **cắt thành từng chặng** tại các node có
action, lọc theo cờ `released`, và nối tiếp được khi fleet master release thêm horizon. `orderId` và
`orderUpdateId` là định danh order ở tầng mission — điền đúng.
Nếu `Order` lấy từ `convert2COrder()` thì gọi `order_free()` khi xong.
| | Hàm | Đổi |
|---|---|---|
| | `bool navigation_move_to_nodes_edges(NavigationHandle, const Node*, size_t, const Edge*, size_t, PoseStamped)` | ~ |
Tiện ích: gửi order chỉ từ mảng node/edge. **Không đặt được `orderId`/`orderUpdateId`** — cả hai để
rỗng/0, nên mission layer không phân biệt được order update với order mới. Dùng
`navigation_move_to_order_v2` nếu cần order update.
> Bản trước của hàm này dựng `Order` **không khởi tạo**, khiến bộ chuyển đổi đọc con trỏ rác trên
> stack. Đã sửa. Nếu khung C# đang chạy với `.so` cũ và thấy crash ngẫu nhiên hoặc chuỗi vô nghĩa ở
> đường order, đây là nguyên nhân.
| | Hàm | Đổi |
|---|---|---|
| | `bool navigation_move_to_order_v2(NavigationHandle, const char *order_id, uint32_t order_update_id, const Node*, size_t, const Edge*, size_t, PoseStamped)` | + |
Như trên nhưng có định danh thật. Quy ước: cùng `order_id` + `order_update_id` tăng dần = cập nhật
tuyến đang chạy; đổi `order_id` = order mới thay thế hoàn toàn. `order_id` rỗng bị từ chối.
| | Hàm | Đổi |
|---|---|---|
| | `bool navigation_dock_to(NavigationHandle, const char *marker, PoseStamped goal)` | = |
| | `bool navigation_dock_to_order(NavigationHandle, Order, const char *marker, PoseStamped)` | = |
| | `bool navigation_dock_to_nodes_edges(...)` | ~ (cùng vấn đề `Order`, đã sửa) |
| | `bool navigation_dock_to_order_v2(NavigationHandle, const char *marker, const char *order_id, uint32_t, const Node*, size_t, const Edge*, size_t, PoseStamped)` | + |
Docking dùng profile riêng (`docking_planner_name`) và tên marker. Docking **không** đi qua mission
layer — nó xuống thẳng lõi.
| | Hàm | Đổi |
|---|---|---|
| | `bool navigation_move_straight_to(NavigationHandle, double distance)` | = |
Đi thẳng theo hướng hiện tại. Tham số là **khoảng cách [m]**, không phải pose: hàm tự đọc pose robot
rồi tính goal. Âm là lùi. Vì nó đọc pose ngay lúc gọi nên TF phải sẵn sàng.
| | Hàm | Đổi |
|---|---|---|
| | `bool navigation_rotate_to(NavigationHandle, PoseStamped goal)` | = |
Quay tại chỗ tới hướng của `goal` (chỉ dùng phần orientation).
### 6.5. Vòng đời nhiệm vụ
| | Hàm | Đổi |
|---|---|---|
| | `void navigation_pause(NavigationHandle)` | ~ |
| | `void navigation_resume(NavigationHandle)` | ~ |
| | `void navigation_cancel(NavigationHandle)` | ~ |
- **Bản cũ:** đồng bộ, có hiệu lực ngay trong lời gọi; ném `std::runtime_error` nếu lõi chưa dựng
xong (C API nuốt, C# thấy im lặng).
- **Bản mới:** chỉ ghi nhận yêu cầu; control thread áp ở cycle kế tiếp (≤ 33 ms @ 30 Hz). Không bao
giờ ném.
Hệ quả: **trả về ≠ robot đã dừng.** Muốn biết đã dừng, chờ `navigation_get_state` báo `PAUSED`, hoặc
theo dõi `navigation_get_twist`. Nếu C# có tầng an toàn giả định dừng tức thì thì phải sửa ở phía C#.
`cancel` từ host huỷ **cả order** (gồm hàng đợi mission), khác với huỷ nội bộ do chính mission layer
phát ra chỉ dừng chặng đang chạy.
### 6.6. Trần vận tốc
| | Hàm | Đổi |
|---|---|---|
| | `bool navigation_set_twist_linear(NavigationHandle, double x, double y, double z)` | ~ |
| | `bool navigation_set_twist_angular(NavigationHandle, double x, double y, double z)` | ~ |
**Đây là trần vận tốc, không phải lệnh jog.** Đường này để tầng an toàn hạ tốc độ robot.
- **Dấu của `x` tuyến tính chọn chiều**: `x < 0` đặt trần lùi, còn lại đặt trần tiến. Hai trần được
giữ riêng.
- Đơn vị: tuyến tính `[m/s]`, góc `[rad/s]`.
- `NaN`/`Inf` bị từ chối, trả `false`.
- Có hiệu lực ở cycle kế tiếp.
- **Bản cũ** đẩy thẳng xuống controller và trả `false` khi đang có cancel (kèm `unlock()` nội bộ).
**Bản mới** chỉ kiểm tra số: giá trị hợp lệ luôn trả `true`. Tín hiệu "đang cancel" **không còn tồn
tại** — C# nào dựa vào nó phải chuyển sang đọc trạng thái.
### 6.7. Truy vấn pose và vận tốc
| | Hàm | Đổi |
|---|---|---|
| | `bool navigation_get_robot_pose_stamped(NavigationHandle, PoseStamped &out)` | = |
| | `bool navigation_get_robot_pose_2d(NavigationHandle, Pose2D &out)` | = |
Pose robot trong global frame. Trả `false` khi TF chưa đủ. Bản `PoseStamped` cấp `frame_id` bằng
`strdup` — giải phóng bằng `nav_c_api_free_string`.
| | Hàm | Đổi |
|---|---|---|
| | `bool navigation_get_twist(NavigationHandle, Twist2DStamped &out)` | ~ |
**Lệnh vận tốc đang phát**, không phải vận tốc đo được. Đây là thứ host publish ra `cmd_vel`.
Bản mới có cửa ân hạn: khi không có yêu cầu nào chạy, **stamp đứng yên**; stamp bằng 0 nghĩa là lõi
chưa từng điều khiển. C# nên kiểm tra stamp trước khi publish, tránh đè lên teleop.
`out.header.frame_id` được `strdup` — giải phóng bằng `nav_c_api_free_string`.
### 6.8. Bơm dữ liệu sensor vào
| | Hàm | Đổi |
|---|---|---|
| | `bool navigation_add_static_map(NavigationHandle, const char *name, OccupancyGrid)` | ~ |
| | `bool navigation_add_laser_scan(NavigationHandle, const char *name, LaserScan)` | = |
| | `bool navigation_add_point_cloud(NavigationHandle, const char *name, PointCloud)` | = |
| | `bool navigation_add_point_cloud2(NavigationHandle, const char *name, PointCloud2)` | = |
| | `bool navigation_add_odometry(NavigationHandle, const char *name, Odometry)` | ~ |
| | `bool navigation_add_depth_camera_data(NavigationHandle, const char *topic, DepthCameraData)` | + |
Chung cho cả nhóm: dữ liệu được **copy** ngay trong lời gọi, buffer C# giải phóng được ngay sau khi
hàm trả về. `name` phải khớp `topic` trong YAML costmap.
- `navigation_add_static_map`: gọi được **trước** `initialize`, và bắt buộc phải thế (mục 3).
- `navigation_add_odometry`: giờ là nguồn vận tốc đo được của controller, không còn chỉ để hiển thị.
- `navigation_add_depth_camera_data` (mới): đường duy nhất tới `VoxelLayer` để **clear theo frustum**
— xoá vật cản ma nằm trong tầm nhìn camera. Yêu cầu:
- `header.frame_id` là **optical frame** của camera, và TF từ frame đó tới base frame phải có trong
buffer tf3 tại đúng `header.stamp`;
- `depth.encoding` đúng thật (`"16UC1"` hoặc `"32FC1"`), `step` là số byte mỗi hàng;
- `camera_info` là intrinsics của **đúng độ phân giải đang gửi**, không phải của stream màu;
- ảnh depth và camera info **cùng một thời điểm** — ghép lệch hai luồng là clear sai vùng.
### 6.9. Đọc lại và xoá dữ liệu sensor
| | Hàm | Đổi |
|---|---|---|
| | `bool navigation_get_static_map(NavigationHandle, const char *name, OccupancyGrid &out)` | = |
| | `bool navigation_get_laser_scan(NavigationHandle, const char *name, LaserScan &out)` | = |
| | `bool navigation_get_point_cloud(NavigationHandle, const char *name, PointCloud &out)` | = |
| | `bool navigation_get_point_cloud2(NavigationHandle, const char *name, PointCloud2 &out)` | = |
| | `bool navigation_get_all_static_maps(NavigationHandle, NamedOccupancyGrid *out, size_t &n)` | = |
| | `bool navigation_get_all_laser_scans(...)`, `..._point_clouds(...)`, `..._point_cloud2s(...)` | = |
| | `bool navigation_remove_static_map / _laser_scan / _point_cloud / _point_cloud2` | = |
| | `bool navigation_remove_all_static_maps / _laser_scans / _point_clouds / _point_cloud2s` | = |
| | `bool navigation_remove_all_data(NavigationHandle)` | = |
Các getter **cấp bộ nhớ** — dùng `navigation_free_*` (mục 6.11) sau khi xong.
Bản mới cất **bản đã lọc** của laser scan, đúng bản mà costmap nhìn thấy, nên getter và costmap không
còn lệch nguồn.
Cảnh báo về chữ ký `get_all_*`: mảng do bên gọi cấp, **không có tham số dung lượng**. Cấp dư.
### 6.10. Dữ liệu hiển thị
| | Hàm | Đổi |
|---|---|---|
| | `bool navigation_get_global_data(NavigationHandle, PlannerDataOutput *out)` | ~ |
| | `bool navigation_get_local_data(NavigationHandle, PlannerDataOutput *out)` | ~ |
Trả plan, lưới costmap (hoặc bản update), cờ `is_costmap_updated`, và footprint. Hàm **tự giải phóng
nội dung cũ** của struct được truyền vào, nên tái sử dụng một struct qua nhiều lần poll là đúng cách
và không rò. Lần điền cuối cùng thì giải phóng bằng `navigation_free_planner_data`.
Ba khác biệt so với bản cũ:
1. **`plan` của `get_local_data` giờ có dữ liệu.** Bản cũ không bao giờ điền trường này cho local.
2. **`costmap` vẫn được điền đầy đủ** qua bộ xuất costmap — không rỗng.
3. **`footprint` đổi ý nghĩa.** Bản cũ: polygon **đã pad và đã transform tới pose robot**, `frame_id`
là global frame — vẽ thẳng lên bản đồ là đúng chỗ. Bản mới: polygon **thô** host đã set, chưa
transform, `frame_id` là `base_link`, và **rỗng nếu host chưa set footprint**.
> Điểm 3 làm vỡ hiển thị footprint. Chỗ sửa đúng nằm ở `move_base2` (lấy footprint từ costmap thay vì
> từ bản thô) chứ không phải bắt C# tự transform. Xem `C_API_MOVE_BASE2_PLAN.md` mục 8.4. Cho tới khi
> sửa, C# muốn vẽ đúng thì phải tự xoay/tịnh tiến footprint theo pose lấy từ
> `navigation_get_robot_pose_2d` — chấp nhận lệch thời gian giữa hai lời gọi.
### 6.11. Bộ nhớ
| | Hàm | Đổi |
|---|---|---|
| | `void nav_c_api_free_string(char *)` | = |
| | `void order_free(Order *)` | = |
| | `void navigation_free_occupancy_grid(OccupancyGrid *)` | + |
| | `void navigation_free_laser_scan(LaserScan *)` | + |
| | `void navigation_free_point_cloud(PointCloud *)` | + |
| | `void navigation_free_point_cloud2(PointCloud2 *)` | + |
| | `void navigation_free_planner_data(PlannerDataOutput *)` | + |
Trước đây chỉ có hai hàm đầu, nên grid/scan/cloud/plan **không có hàm giải phóng nào** — mỗi lời gọi
getter rò một lần. Năm hàm mới đóng lỗ đó. Tất cả đều an toàn với `NULL`, xoá luôn con trỏ và bộ đếm,
gọi hai lần không sao.
Nguyên tắc: cấp phát ở trong thư viện thì **phải** giải phóng bằng hàm của thư viện. Đừng gọi `free()`
của libc từ C#.
### 6.12. Tiện ích hình học
| | Hàm | Đổi |
|---|---|---|
| | `bool navigation_offset_goal_2d(const Pose2D&, const char *frame_id, double d, PoseStamped &out)` | = |
| | `bool navigation_offset_goal_stamped(const PoseStamped&, double d, PoseStamped &out)` | = |
Dịch một pose đi `d` mét dọc theo hướng của chính nó (âm là lùi). Thuần tính toán, không đụng lõi.
(Hai hàm này có export nhưng chưa được khai báo trong `nav_c_api.h`.)
---
## 7. Quy tắc bộ nhớ — ai giải phóng cái gì
| Nguồn | Giải phóng bằng |
|---|---|
| `NavFeedback.feed_back_str` | `nav_c_api_free_string` |
| `navigation_get_status_text` | `nav_c_api_free_string` |
| `navigation_state_to_string` | `nav_c_api_free_string` |
| `PoseStamped.header.frame_id`, `Twist2DStamped.header.frame_id` từ getter | `nav_c_api_free_string` |
| `OccupancyGrid` từ `navigation_get_static_map` | `navigation_free_occupancy_grid` |
| `LaserScan` từ `navigation_get_laser_scan` | `navigation_free_laser_scan` |
| `PointCloud` / `PointCloud2` từ getter | `navigation_free_point_cloud` / `_point_cloud2` |
| `PlannerDataOutput` từ `get_global_data` / `get_local_data` | `navigation_free_planner_data` |
| `Order` từ `convert2COrder` | `order_free` |
| Mảng truyền **vào** các hàm `add_*` / `move_to_*` | C# tự giữ; thư viện đã copy xong khi hàm trả về |
---
## 8. Thread safety
| Nhóm | Gọi được từ thread nào |
|---|---|
| `navigation_add_*` (mọi đường sensor) | **Một thread duy nhất.** Đường sensor không thread-safe. Nếu C# có nhiều nguồn chạy song song, dồn qua một hàng đợi rồi bơm từ một thread. |
| `navigation_set_robot_footprint` | Thread bất kỳ (ghi dưới mutex, áp ở cycle sau) |
| `pause` / `resume` / `cancel` / `set_twist_*` | Thread bất kỳ (ghi dưới mutex, áp ở cycle sau) |
| `move_to*` / `dock_to*` / `rotate_to` / `move_straight_to` | Thread bất kỳ |
| `get_feedback` / `get_state` / `get_twist` / `get_robot_pose_*` | Thread bất kỳ |
| `get_global_data` / `get_local_data` | Thread bất kỳ; mỗi consumer nên có **struct riêng**, không dùng chung một `PlannerDataOutput` giữa nhiều timer |
| `navigation_initialize*` / `navigation_shutdown` / `navigation_destroy` | Thread khởi tạo/thread tắt, **không** song song với nhóm sensor |
---
## 9. Chẩn đoán nhanh
| Hiện tượng | Nguyên nhân thường gặp |
|---|---|
| `navigation_create` trả `NULL` | Sai `PNKX_NAV_CORE_CONFIG_DIR`, thiếu khoá `MoveBase: library_path`, hoặc `libmove_base2.so` không nằm trong đường tìm |
| Tiến trình chết ngay trong `initialize` | Chưa bơm static map trước `initialize` — SBPL chờ costmap rồi `exit(1)` |
| `initialize_checked` trả `false` | Đọc `navigation_get_status_text` — chuỗi nói rõ pha nào hỏng |
| Gửi goal xong không có gì xảy ra, không log | Control thread chưa chạy: `initialize` đã hỏng nhưng code cũ không kiểm tra kết quả |
| Robot không tránh vật cản từ camera | Sai tên `topic` khi bơm, hoặc thiếu TF của optical frame, hoặc chưa dùng `navigation_add_depth_camera_data` |
| Vật cản ma không bao giờ biến mất | Chưa bơm `DepthCameraData` — point cloud không clear được theo frustum |
| Footprint vẽ sai chỗ trên bản đồ | Mục 6.10 điểm 3 |
| Bộ nhớ tăng dần | Thiếu `navigation_free_*` ở đường getter, hoặc thiếu `nav_c_api_free_string` cho `feed_back_str` |
| Tắt chương trình bị treo hoặc crash | Thiếu `navigation_shutdown`, hoặc còn thread bơm sensor chạy khi tắt |
| Đổi tham số planner mãi không có tác dụng | Sửa nhầm cây config — `move_base2` đọc `Test/move_base2/config/runtime`, không đọc `pnkx_nav_core/config` |
---
## 10. Chưa xác nhận
- Chưa đọc được mã C# hiện tại, nên chưa biết chỗ nào đang thật sự dựa vào các hành vi cũ ở mục 6.5,
6.6 và 6.10.
- Trường `footprint` trong dữ liệu hiển thị: đã đối chiếu cả hai bản và khác biệt là chắc chắn, nhưng
chưa xác nhận phía C# có đang vẽ nó lên bản đồ hay không.
- Các hàm mới trong `nav_c_api_2` mới chỉ được kiểm tra biên dịch, **chưa chạy trên robot hay
Gazebo**. Đường depth camera cần kiểm tra thực tế bằng bộ đếm chẩn đoán của cổng sensor.
- `navigation_move_straight_to` với khoảng cách âm (lùi) trong `move_base2` chưa được kiểm tra.
---
## Phụ lục A — bảng symbol đầy đủ trong `libnav_c_api.so`
Lấy bằng `nm --defined-only` trên các object file. Cột "Nguồn" cho biết symbol nằm trong file nguồn
nào — chỉ để tra cứu, C# không cần biết: mọi symbol đều nằm chung một `.so`.
### 50 symbol cũ — `nav_c_api.cpp`, C# **không đổi gì**
```
nav_c_api_free_string navigation_get_point_cloud2
navigation_add_laser_scan navigation_get_robot_footprint
navigation_add_odometry navigation_get_robot_pose_2d
navigation_add_point_cloud navigation_get_robot_pose_stamped
navigation_add_point_cloud2 navigation_get_static_map
navigation_add_static_map navigation_get_twist
navigation_cancel navigation_initialize
navigation_create navigation_move_straight_to
navigation_destroy navigation_move_to
navigation_dock_to navigation_move_to_nodes_edges
navigation_dock_to_nodes_edges navigation_move_to_order
navigation_dock_to_order navigation_offset_goal_2d
navigation_get_all_laser_scans navigation_offset_goal_stamped
navigation_get_all_point_cloud2s navigation_pause
navigation_get_all_point_clouds navigation_remove_all_data
navigation_get_all_static_maps navigation_remove_all_laser_scans
navigation_get_feedback navigation_remove_all_point_cloud2s
navigation_get_global_data navigation_remove_all_point_clouds
navigation_get_laser_scan navigation_remove_all_static_maps
navigation_get_local_data navigation_remove_laser_scan
navigation_get_point_cloud navigation_remove_point_cloud
navigation_resume navigation_remove_point_cloud2
navigation_rotate_to navigation_remove_static_map
navigation_set_robot_footprint navigation_state_to_string
navigation_set_twist_angular navigation_set_twist_linear
```
Cộng `order_free` từ `convertor.cpp`.
### 13 symbol mới — `nav_c_api_2.cpp`
```
navigation_initialize_checked navigation_add_depth_camera_data
navigation_is_ready navigation_move_to_order_v2
navigation_shutdown navigation_dock_to_order_v2
navigation_get_state navigation_free_occupancy_grid
navigation_get_status_text navigation_free_laser_scan
navigation_free_point_cloud navigation_free_point_cloud2
navigation_free_planner_data
```
---
## Phụ lục B — phần C# phải thêm
Đây là **toàn bộ** phần khai báo mới. Mọi thứ khác trong lớp binding hiện tại giữ nguyên.
### B.1. Bốn struct mới
Ba struct đầu chưa từng xuất hiện trong bề mặt cũ vì đường depth camera chưa bao giờ được phơi ra.
```csharp
[StructLayout(LayoutKind.Sequential)]
public struct RegionOfInterest
{
public uint x_offset;
public uint y_offset;
public uint height;
public uint width;
[MarshalAs(UnmanagedType.I1)] public bool do_rectify;
}
[StructLayout(LayoutKind.Sequential)]
public struct Image
{
public Header header;
public uint height;
public uint width;
public IntPtr encoding; // char* — "16UC1" hoặc "32FC1"
public byte is_bigendian;
public uint step; // số byte mỗi hàng
public IntPtr data; // uint8_t*
public UIntPtr data_count;
}
[StructLayout(LayoutKind.Sequential)]
public struct CameraInfo
{
public Header header;
public uint height;
public uint width;
public IntPtr distortion_model; // char*
public IntPtr D; // double*
public UIntPtr D_count;
[MarshalAs(UnmanagedType.ByValArray, SizeConst = 9)] public double[] K;
[MarshalAs(UnmanagedType.ByValArray, SizeConst = 9)] public double[] R;
[MarshalAs(UnmanagedType.ByValArray, SizeConst = 12)] public double[] P;
public uint binning_x;
public uint binning_y;
public RegionOfInterest roi;
}
[StructLayout(LayoutKind.Sequential)]
public struct DepthCameraData
{
public Header header; // stamp + optical frame của mẫu
public Image depth;
public CameraInfo camera_info; // intrinsics của ĐÚNG ảnh trên
}
```
### B.2. Mười ba `DllImport` mới
```csharp
private const string Lib = "nav_c_api";
private const CallingConvention Cdecl = CallingConvention.Cdecl;
// ---- vòng đời ---------------------------------------------------------------------------
[DllImport(Lib, CallingConvention = Cdecl)] [return: MarshalAs(UnmanagedType.I1)]
public static extern bool navigation_initialize_checked(IntPtr handle, IntPtr tf3Buffer);
[DllImport(Lib, CallingConvention = Cdecl)] [return: MarshalAs(UnmanagedType.I1)]
public static extern bool navigation_is_ready(IntPtr handle);
[DllImport(Lib, CallingConvention = Cdecl)]
public static extern void navigation_shutdown(IntPtr handle);
// ---- trạng thái -------------------------------------------------------------------------
[DllImport(Lib, CallingConvention = Cdecl)] [return: MarshalAs(UnmanagedType.I1)]
public static extern bool navigation_get_state(IntPtr handle, out NavigationState state);
[DllImport(Lib, CallingConvention = Cdecl)]
public static extern IntPtr navigation_get_status_text(IntPtr handle); // free: nav_c_api_free_string
// ---- sensor -----------------------------------------------------------------------------
[DllImport(Lib, CallingConvention = Cdecl)] [return: MarshalAs(UnmanagedType.I1)]
public static extern bool navigation_add_depth_camera_data(
IntPtr handle, [MarshalAs(UnmanagedType.LPStr)] string topic, DepthCameraData data);
// ---- order ------------------------------------------------------------------------------
[DllImport(Lib, CallingConvention = Cdecl)] [return: MarshalAs(UnmanagedType.I1)]
public static extern bool navigation_move_to_order_v2(
IntPtr handle,
[MarshalAs(UnmanagedType.LPStr)] string orderId, uint orderUpdateId,
[In] Node[] nodes, UIntPtr nodeCount,
[In] Edge[] edges, UIntPtr edgeCount,
PoseStamped goal);
[DllImport(Lib, CallingConvention = Cdecl)] [return: MarshalAs(UnmanagedType.I1)]
public static extern bool navigation_dock_to_order_v2(
IntPtr handle, [MarshalAs(UnmanagedType.LPStr)] string marker,
[MarshalAs(UnmanagedType.LPStr)] string orderId, uint orderUpdateId,
[In] Node[] nodes, UIntPtr nodeCount,
[In] Edge[] edges, UIntPtr edgeCount,
PoseStamped goal);
// ---- giải phóng bộ nhớ ------------------------------------------------------------------
[DllImport(Lib, CallingConvention = Cdecl)]
public static extern void navigation_free_occupancy_grid(ref OccupancyGrid grid);
[DllImport(Lib, CallingConvention = Cdecl)]
public static extern void navigation_free_laser_scan(ref LaserScan scan);
[DllImport(Lib, CallingConvention = Cdecl)]
public static extern void navigation_free_point_cloud(ref PointCloud cloud);
[DllImport(Lib, CallingConvention = Cdecl)]
public static extern void navigation_free_point_cloud2(ref PointCloud2 cloud);
[DllImport(Lib, CallingConvention = Cdecl)]
public static extern void navigation_free_planner_data(ref PlannerDataOutput data);
```
### B.3. Ví dụ bơm một mẫu depth camera
Ảnh depth thường vài MB. Ghim mảng lại thay vì để bộ marshaller sao chép thêm một lần — thư viện đã
copy sang phía C++ ngay trong lời gọi, nên gỡ ghim ngay sau khi trả về là an toàn.
```csharp
public bool PushDepthSample(string topic, string opticalFrame,
uint sec, uint nsec,
byte[] depthBytes, uint height, uint width, uint step,
double[] k, double[] p)
{
var frameId = Marshal.StringToHGlobalAnsi(opticalFrame);
var encoding = Marshal.StringToHGlobalAnsi("16UC1");
var distModel = Marshal.StringToHGlobalAnsi("plumb_bob");
var pinned = GCHandle.Alloc(depthBytes, GCHandleType.Pinned);
try
{
var header = new Header { seq = 0, sec = sec, nsec = nsec, frame_id = frameId };
var data = new DepthCameraData
{
header = header,
depth = new Image
{
header = header,
height = height,
width = width,
encoding = encoding,
is_bigendian = 0,
step = step,
data = pinned.AddrOfPinnedObject(),
data_count = (UIntPtr)depthBytes.Length,
},
camera_info = new CameraInfo
{
header = header,
height = height,
width = width,
distortion_model = distModel,
D = IntPtr.Zero,
D_count = UIntPtr.Zero,
K = k, // 9 phần tử
R = new double[9],
P = p, // 12 phần tử
binning_x = 0,
binning_y = 0,
roi = default,
},
};
return navigation_add_depth_camera_data(navHandle, topic, data);
}
finally
{
pinned.Free();
Marshal.FreeHGlobal(distModel);
Marshal.FreeHGlobal(encoding);
Marshal.FreeHGlobal(frameId);
}
}
```
### B.4. Ví dụ đọc dữ liệu hiển thị mà không rò bộ nhớ
```csharp
// MỘT struct cho MỖI consumer, giữ lại giữa các lần poll: hàm tự giải phóng nội dung cũ.
// Đừng dùng chung một struct giữa nhiều timer.
private PlannerDataOutput globalData;
private void OnGlobalTimer()
{
if (!navigation_get_global_data(navHandle, ref globalData))
return;
DrawPlan(globalData.plan);
DrawCostmap(globalData.costmap, globalData.is_costmap_updated, globalData.costmap_update);
}
// Khi dừng consumer — nếu không, lần điền cuối cùng sẽ rò.
private void StopGlobalTimer()
{
timer.Stop();
navigation_free_planner_data(ref globalData);
}
```

51
src/APIs/c_api/include/convertor_2.h Normal file
View File

@@ -0,0 +1,51 @@
#ifndef C_API_CONVERTOR_2_H
#define C_API_CONVERTOR_2_H
/**
* @file convertor_2.h
* @brief Converters that only the move_base2 entry points need.
*
* Kept apart from convertor.h so the move_base2 surface can be added, reviewed and reverted without
* touching the converters the existing binding already depends on.
*/
// C
#include "std_msgs/Header.h"
#include "sensor_msgs/Image.h"
#include "sensor_msgs/CameraInfo.h"
#include "sensor_msgs/DepthCameraData.h"
// C++
#include <robot_std_msgs/Header.h>
#include <robot_sensor_msgs/Image.h>
#include <robot_sensor_msgs/CameraInfo.h>
#include <robot_sensor_msgs/DepthCameraData.h>
/**
* @brief Convert C Header to C++ Header.
* @param header C header; `frame_id` may be NULL (becomes an empty string).
*/
robot_std_msgs::Header convert2CppHeader(const Header& header);
/**
* @brief Convert C Image to C++ Image.
*
* The pixel buffer is copied, so the caller keeps ownership of `image.data` and may free it as
* soon as this call returns.
*
* @param image C image; `data` and `encoding` may be NULL.
*/
robot_sensor_msgs::Image convert2CppImage(const Image& image);
/**
* @brief Convert C CameraInfo to C++ CameraInfo.
* @param info C camera info; `D` and `distortion_model` may be NULL. K/R/P are fixed size (9/9/12).
*/
robot_sensor_msgs::CameraInfo convert2CppCameraInfo(const CameraInfo& info);
/**
* @brief Convert C DepthCameraData to C++ DepthCameraData. Everything is deep-copied.
*/
robot_sensor_msgs::DepthCameraData convert2CppDepthCameraData(const DepthCameraData& data);
#endif // C_API_CONVERTOR_2_H

301
src/APIs/c_api/include/nav_c_api_2.h Normal file
View File

@@ -0,0 +1,301 @@
#ifndef NAVIGATION_C_API_2_H
#define NAVIGATION_C_API_2_H
/**
* @file nav_c_api_2.h
* @brief Additional C entry points required by the move_base2 navigation runtime.
*
* ## What this file is
*
* `nav_c_api.h` covers the navigation contract as the previous runtime exposed it. Every one of
* those functions keeps working against move_base2 — move_base2 implements
* `robot::move_base_core::BaseNavigation` with the signatures unchanged, so no existing binding
* breaks. This file adds what that contract gained but the C surface never exposed, plus the two
* entry points move_base2 makes newly necessary:
*
* - navigation_shutdown() — quiesce the core before destroying it.
* - navigation_is_ready() — did initialize() actually succeed?
* - navigation_initialize_checked() — initialize + verify, in one call.
* - navigation_get_state() — poll the state without allocating a string.
* - navigation_get_status_text() — the core's own explanation, for logging.
* - navigation_add_depth_camera_data() — the depth-camera input path, never exposed before.
* - navigation_move_to_order_v2() — an order carrying a real orderId / orderUpdateId.
* - navigation_dock_to_order_v2() — same, for docking.
*
* Include this header **in addition to** nav_c_api.h. It adds symbols; it replaces none.
*
* ## What changed underneath the functions you already call
*
* These keep their signatures and their meaning, but behave differently enough to matter:
*
* 1. navigation_initialize() is now heavy and blocking — it builds both costmaps, loads the
* planners, the controller, the recovery behaviours and the mission layer, then starts the
* control thread. It still returns `true` unconditionally because the underlying contract
* returns void. Use navigation_initialize_checked(), or call navigation_is_ready() after it.
*
* 2. A static map must be pushed **before** initialize(). The lattice planner blocks until the
* costmap has a non-zero size, and that size only exists once a static map has reached the
* static layer. It aborts the whole process if it waits too long.
*
* 3. navigation_set_robot_footprint() may now be called before initialize() — the footprint is
* cached and applied while the costmaps are built. Under the old runtime the same call was
* dropped silently because the costmaps did not exist yet.
*
* 4. navigation_pause() / _resume() / _cancel() no longer take effect inside the call. They record
* the request; the control thread applies it on its next cycle. Returning from the call does
* not mean the robot stopped — watch the state, or watch the twist.
*
* 5. navigation_set_twist_linear() / _angular() set a velocity **ceiling**, and the sign of the
* linear x component selects which direction the ceiling applies to. They now only reject
* NaN/Inf; the old runtime also returned false while a cancel was in flight, and that signal no
* longer exists. The new ceiling takes effect on the next control cycle.
*
* 6. navigation_get_twist() returns the command being issued, not a measurement. While no request
* is running its stamp stops advancing; a zero stamp means the core has never commanded
* anything. Check the stamp before republishing it as a velocity command.
*
* 7. NavFeedback.feed_back_str now carries the core's own reason strings — different text from the
* old runtime. Branch on NavigationState, never on the string.
*
* 8. NavFeedback.navigation_state deliberately reports ACTIVE between the legs of a multi-leg
* order, where the core itself is momentarily SUCCEEDED or PENDING. Treating SUCCEEDED as "the
* order finished" stays correct only because of that; do not bypass it.
*
* 9. navigation_add_odometry() feeds the controller's measured velocity every cycle, not just a
* stored value. Push it at the rate the odometry source produces it.
*
* 10. The `footprint` field of navigation_get_global_data() / _local_data() is the raw footprint the
* host set, in the robot base frame, and it is empty if the host never set one. The old runtime
* returned the padded footprint already transformed to the robot pose in the global frame.
*
* 11. navigation_destroy() alone is no longer enough — call navigation_shutdown() first, from a
* point in your own shutdown sequence where the process is still fully alive.
*
* See NAV_C_API_2_GUIDE.md in this directory for the per-function walkthrough and the startup and
* shutdown sequences.
*/
#ifdef __cplusplus
extern "C"
{
#endif
#include <stdbool.h>
#include <stdint.h>
#include <stddef.h>
#include "nav_c_api.h"
#include "sensor_msgs/DepthCameraData.h"
#include "sensor_msgs/LaserScan.h"
#include "sensor_msgs/PointCloud.h"
#include "sensor_msgs/PointCloud2.h"
#include "nav_msgs/OccupancyGrid.h"
#include "protocol_msgs/Node.h"
#include "protocol_msgs/Edge.h"
// ============================================================================================
// Lifecycle
// ============================================================================================
/**
* @brief Initialize the navigation core and report whether it actually came up.
*
* Wraps navigation_initialize(). The underlying contract method returns void, so every failure
* path inside the runtime — costmap construction, sensor configuration, plugin loading, control
* loop configuration, control thread startup — returns normally and only records the reason in
* the feedback. This function reads that back and turns it into a return value.
*
* Blocking and expensive: it builds two costmaps, loads every plugin named in the YAML, starts
* the map-update threads, the mission threads and the control thread. Do not call it from a UI
* thread.
*
* @param handle Navigation handle from navigation_create().
* @param tf3_buffer tf3 BufferCore the host owns; must outlive the navigation instance.
* @return true only when the core is ready to accept goals.
*
* @pre At least one static map has been pushed with navigation_add_static_map().
* @pre The robot footprint has been set with navigation_set_robot_footprint(), if the host wants
* its own footprint rather than the one in the YAML.
* @see navigation_get_status_text() for the reason behind a false return.
*/
bool navigation_initialize_checked(NavigationHandle handle, TFListenerHandle tf3_buffer);
/**
* @brief Is the core initialized and accepting goals?
*
* Cheap enough to poll. Reads the same flag navigation_initialize_checked() checks, without
* allocating the feedback string.
*
* @return false if the handle is null, the core never initialized, or initialization failed.
*/
bool navigation_is_ready(NavigationHandle handle);
/**
* @brief Stop the core's threads without destroying the instance.
*
* Gives the host a point inside its own shutdown sequence — while the process is still fully
* alive — to stop the control thread, the costmap update threads and the mission threads.
* Destroying the instance from a finalizer, after the runtime has already begun tearing down, is
* the race this exists to avoid.
*
* After this call the core issues no further velocity commands, and every getter
* (navigation_get_feedback, navigation_get_twist, navigation_get_robot_pose_*) remains safe to
* call. Safe to call more than once.
*
* @warning No thread may be calling navigation_add_* while this runs, and none may call them
* afterwards. The sensor path is not thread-safe and there is no longer a control thread
* consuming what it produces.
*/
void navigation_shutdown(NavigationHandle handle);
// ============================================================================================
// Status
// ============================================================================================
/**
* @brief Current navigation state, without allocating anything.
*
* navigation_get_feedback() strdup()s its message on every call, so polling it at control rate
* means one allocation and one free per poll — and one leak per missed free. Use this when only
* the state matters, which is the common case.
*
* @param handle Navigation handle.
* @param out_state Receives the current state; untouched on failure.
* @return false if the handle is null or the core has no feedback yet.
*/
bool navigation_get_state(NavigationHandle handle, NavigationState *out_state);
/**
* @brief The core's own description of what it is doing, or why it refused.
*
* For logging and diagnostics only. The wording comes from the runtime and changes between
* versions — never branch on it. Branch on navigation_get_state().
*
* @return Newly allocated string, or NULL. Free with nav_c_api_free_string().
*/
char *navigation_get_status_text(NavigationHandle handle);
// ============================================================================================
// Sensor input
// ============================================================================================
/**
* @brief Push one coherent depth-camera sample into the navigation core.
*
* This is the only way to reach the depth-camera path from C. The voxel layer projects the depth
* image through the intrinsics to build the camera frustum, which is what lets it *clear* stale
* obstacles inside the field of view. A point cloud cannot substitute: it carries no intrinsics,
* so no frustum can be reconstructed and nothing gets cleared.
*
* @param handle Navigation handle.
* @param topic Logical name of the stream. Must match an `observation_source` declared for the
* voxel layer in the costmap YAML — a name no layer subscribes to is accepted here
* and then dropped, silently, further down.
* @param data Depth image and its matching intrinsics. Deep-copied; the caller may release its
* buffers as soon as this returns.
* @return true if the sample was accepted.
*
* @pre `data.header.frame_id` is the camera's optical frame, and a transform from it to the robot
* base frame exists in the tf3 buffer at `data.header.stamp`.
* @pre `data.depth.encoding` matches the real encoding ("16UC1" or "32FC1") and `step` is the
* real row stride in bytes.
* @pre `data.camera_info` describes the resolution actually being sent, not the colour stream's.
* @pre The depth image and the camera info come from the same instant.
*
* @note Push at the camera's real rate. This is the most expensive sensor path in the core;
* pushing faster than the source only burns CPU.
*/
bool navigation_add_depth_camera_data(NavigationHandle handle, const char *topic,
const DepthCameraData data);
// ============================================================================================
// Orders
// ============================================================================================
/**
* @brief Send a multi-node order with a real identity.
*
* Under move_base2 an order goes through the mission layer, which splits it into legs at the
* nodes that carry actions, filters by `released`, and can extend the route when the fleet master
* releases more of the horizon. `orderId` plus `orderUpdateId` is what identifies an order there:
* the same `orderId` with a higher `orderUpdateId` is an update of the route in progress, while a
* different `orderId` is a new order that replaces it.
*
* navigation_move_to_nodes_edges() cannot express that — it has nowhere to put either field — so
* every call looks like a different order and no update can ever be sent. Use this instead
* whenever the fleet master sends order updates or releases the horizon progressively.
*
* @param handle Navigation handle.
* @param order_id VDA5050 orderId. Must not be NULL or empty.
* @param order_update_id VDA5050 orderUpdateId. Must increase within one orderId.
* @param nodes Node array; the host keeps ownership.
* @param node_count Number of nodes.
* @param edges Edge array; the host keeps ownership.
* @param edge_count Number of edges.
* @param goal Final pose, in the global frame.
* @return true if the order was accepted.
*/
bool navigation_move_to_order_v2(NavigationHandle handle,
const char *order_id, uint32_t order_update_id,
const Node *nodes, size_t node_count,
const Edge *edges, size_t edge_count,
const PoseStamped goal);
/**
* @brief Docking counterpart of navigation_move_to_order_v2().
*
* @param handle Navigation handle.
* @param marker Docking marker name or id. Must not be NULL.
* @param order_id VDA5050 orderId. Must not be NULL or empty.
* @param order_update_id VDA5050 orderUpdateId.
* @param nodes Node array; the host keeps ownership.
* @param node_count Number of nodes.
* @param edges Edge array; the host keeps ownership.
* @param edge_count Number of edges.
* @param goal Docking pose.
* @return true if the docking order was accepted.
*/
bool navigation_dock_to_order_v2(NavigationHandle handle, const char *marker,
const char *order_id, uint32_t order_update_id,
const Node *nodes, size_t node_count,
const Edge *edges, size_t edge_count,
const PoseStamped goal);
// ============================================================================================
// Releasing what the getters allocate
// ============================================================================================
//
// Every navigation_get_* function that returns a grid, a scan, a cloud or a plan fills it with
// buffers allocated inside the library. Only two release helpers ever shipped —
// nav_c_api_free_string() and order_free() — so the rest had no counterpart and leaked one
// allocation per call. At four display timers polling at 10 Hz that is a steady leak, not a
// rounding error. These close it.
//
// All are null-safe, clear the pointers and counts they release, and are safe to call twice.
/** @brief Release the buffers inside an OccupancyGrid filled by navigation_get_static_map(). */
void navigation_free_occupancy_grid(OccupancyGrid *grid);
/** @brief Release the buffers inside a LaserScan filled by navigation_get_laser_scan(). */
void navigation_free_laser_scan(LaserScan *scan);
/** @brief Release the buffers inside a PointCloud filled by navigation_get_point_cloud(). */
void navigation_free_point_cloud(PointCloud *cloud);
/** @brief Release the buffers inside a PointCloud2 filled by navigation_get_point_cloud2(). */
void navigation_free_point_cloud2(PointCloud2 *cloud);
/**
* @brief Release the buffers inside a PlannerDataOutput.
*
* navigation_get_global_data() / _local_data() already release the previous contents of the
* struct they are handed, so reusing one struct across polls does not leak. What does leak is the
* last fill, once the host stops polling — free it here.
*/
void navigation_free_planner_data(PlannerDataOutput *data);
#ifdef __cplusplus
}
#endif
#endif // NAVIGATION_C_API_2_H

0
src/APIs/c_api/include/nav_c_api_io_structure.md
View File

36
src/APIs/c_api/include/sensor_msgs/DepthCameraData.h Normal file
View File

@@ -0,0 +1,36 @@
#ifndef C_API_SENSOR_MSGS_DEPTHCAMERADATA_H
#define C_API_SENSOR_MSGS_DEPTHCAMERADATA_H
#ifdef __cplusplus
extern "C" {
#endif
#include <stdint.h>
#include <stddef.h>
#include "std_msgs/Header.h"
#include "sensor_msgs/Image.h"
#include "sensor_msgs/CameraInfo.h"
/**
* @brief One coherent depth-camera sample: a depth image plus the intrinsics that describe it.
*
* Mirrors robot_sensor_msgs::DepthCameraData. "Coherent" is the whole point of the struct: the
* voxel layer projects every depth pixel through the intrinsics to build a frustum, so a depth
* image paired with intrinsics from a different resolution or a different instant clears the wrong
* volume of space. Pair them at the source, not here.
*/
typedef struct
{
/** Canonical timestamp and optical frame of this sample. */
Header header;
/** Depth image. `encoding` must be the true encoding ("16UC1" or "32FC1"). */
Image depth;
/** Intrinsics of the exact image above — same resolution, same instant. */
CameraInfo camera_info;
} DepthCameraData;
#ifdef __cplusplus
}
#endif
#endif // C_API_SENSOR_MSGS_DEPTHCAMERADATA_H

78
src/APIs/c_api/src/convertor_2.cpp Normal file
View File

@@ -0,0 +1,78 @@
#include "convertor_2.h"
#include <algorithm>
#include <cstddef>
#include <string>
namespace
{
/// Empty string for a NULL C string — every char* coming from the host is optional.
inline std::string safe_string(const char* s)
{
return s != nullptr ? std::string(s) : std::string();
}
} // namespace
robot_std_msgs::Header convert2CppHeader(const Header& header)
{
robot_std_msgs::Header cpp;
cpp.seq = header.seq;
cpp.stamp.sec = header.sec;
cpp.stamp.nsec = header.nsec;
cpp.frame_id = safe_string(header.frame_id);
return cpp;
}
robot_sensor_msgs::Image convert2CppImage(const Image& image)
{
robot_sensor_msgs::Image cpp;
cpp.header = convert2CppHeader(image.header);
cpp.height = image.height;
cpp.width = image.width;
cpp.encoding = safe_string(image.encoding);
cpp.is_bigendian = image.is_bigendian;
cpp.step = image.step;
// Copy, never alias: the host buffer may be a pinned .NET array released right after this call,
// while the voxel layer reads the sample later on the map-update thread.
const size_t count = image.data != nullptr ? image.data_count : 0;
cpp.data.assign(image.data, image.data + count);
return cpp;
}
robot_sensor_msgs::CameraInfo convert2CppCameraInfo(const CameraInfo& info)
{
robot_sensor_msgs::CameraInfo cpp;
cpp.header = convert2CppHeader(info.header);
cpp.height = info.height;
cpp.width = info.width;
cpp.distortion_model = safe_string(info.distortion_model);
const size_t d_count = info.D != nullptr ? info.D_count : 0;
cpp.D.assign(info.D, info.D + d_count);
std::copy(info.K, info.K + 9, cpp.K.begin());
std::copy(info.R, info.R + 9, cpp.R.begin());
std::copy(info.P, info.P + 12, cpp.P.begin());
cpp.binning_x = info.binning_x;
cpp.binning_y = info.binning_y;
cpp.roi.x_offset = info.roi.x_offset;
cpp.roi.y_offset = info.roi.y_offset;
cpp.roi.height = info.roi.height;
cpp.roi.width = info.roi.width;
cpp.roi.do_rectify = info.roi.do_rectify;
return cpp;
}
robot_sensor_msgs::DepthCameraData convert2CppDepthCameraData(const DepthCameraData& data)
{
robot_sensor_msgs::DepthCameraData cpp;
cpp.header = convert2CppHeader(data.header);
cpp.depth = convert2CppImage(data.depth);
cpp.camera_info = convert2CppCameraInfo(data.camera_info);
return cpp;
}

7
src/APIs/c_api/src/nav_c_api.cpp
View File

@@ -317,7 +317,9 @@ extern "C" bool navigation_move_to_nodes_edges(NavigationHandle handle, const No
if (!nav_ptr) if (!nav_ptr)
return false; return false;
robot::log_error("navigation_move_to_nodes_edges goal %f %f", goal.pose.position.x, goal.pose.position.y); robot::log_error("navigation_move_to_nodes_edges goal %f %f", goal.pose.position.x, goal.pose.position.y);
Order order; // Zero-init: the fields left unset below are all char*, and convert2CppOrder treats any
// non-null pointer as a valid string — an uninitialized struct hands it stack garbage.
Order order{};
order.nodes = const_cast<Node *>(nodes); order.nodes = const_cast<Node *>(nodes);
order.nodes_count = node_count; order.nodes_count = node_count;
order.edges = const_cast<Edge *>(edges); order.edges = const_cast<Edge *>(edges);
@@ -416,7 +418,8 @@ extern "C" bool navigation_dock_to_nodes_edges(NavigationHandle handle, const ch
reinterpret_cast<robot::move_base_core::BaseNavigation *>(handle), [](::robot::move_base_core::BaseNavigation *) {}); reinterpret_cast<robot::move_base_core::BaseNavigation *>(handle), [](::robot::move_base_core::BaseNavigation *) {});
if (!nav_ptr) if (!nav_ptr)
return false; return false;
Order order; // Zero-init: see navigation_move_to_nodes_edges.
Order order{};
order.nodes = const_cast<Node *>(nodes); order.nodes = const_cast<Node *>(nodes);
order.nodes_count = node_count; order.nodes_count = node_count;
order.edges = const_cast<Edge *>(edges); order.edges = const_cast<Edge *>(edges);

422
src/APIs/c_api/src/nav_c_api_2.cpp Normal file
View File

@@ -0,0 +1,422 @@
/**
* @file nav_c_api_2.cpp
* @brief Implementation of the move_base2 additions to the C navigation API.
*
* Deliberately separate from nav_c_api.cpp: the surface the existing binding depends on stays byte
* for byte where it is, and everything move_base2 needs on top of it lands here, where it can be
* reviewed — or dropped — on its own.
*
* The handle is the same opaque `robot::move_base_core::BaseNavigation*` navigation_create()
* returns; ownership still lives in nav_c_api.cpp, so nothing here creates or frees an instance.
*/
#include "nav_c_api_2.h"
#include <cstdlib>
#include <cstring>
#include <string>
#include <boost/make_shared.hpp>
#include <robot/robot.h>
#include <move_base_core/common.h>
#include <move_base_core/navigation.h>
#include <robot_sensor_msgs/DepthCameraData.h>
#include "convertor.h"
#include "convertor_2.h"
namespace
{
/// Non-owning view of the handle. The instance is owned by the table in nav_c_api.cpp.
inline robot::move_base_core::BaseNavigation *as_navigation(NavigationHandle handle)
{
return static_cast<robot::move_base_core::BaseNavigation *>(handle);
}
/**
* @brief Build a C Order from an id plus the node/edge arrays the host owns.
*
* Zero-initialized on purpose. Every optional field of Order is a char*, and the converter treats a
* non-null pointer as a valid string — an uninitialized struct therefore hands it stack garbage to
* build std::string from. The pointers below stay non-owning: the converter copies before this
* returns, and the arrays are never freed here.
*/
Order make_order(const char *order_id, uint32_t order_update_id,
const Node *nodes, size_t node_count,
const Edge *edges, size_t edge_count)
{
Order order{};
order.orderId = const_cast<char *>(order_id);
order.orderUpdateId = order_update_id;
order.nodes = const_cast<Node *>(nodes);
order.nodes_count = nodes != nullptr ? node_count : 0;
order.edges = const_cast<Edge *>(edges);
order.edges_count = edges != nullptr ? edge_count : 0;
return order;
}
} // namespace
// ================================================================================================
// Lifecycle
// ================================================================================================
extern "C" bool navigation_initialize_checked(NavigationHandle handle, TFListenerHandle tf3_buffer)
{
if (!handle || !tf3_buffer)
{
robot::log_error("navigation_initialize_checked: null handle or tf3 buffer");
return false;
}
try
{
auto *nav = as_navigation(handle);
// The host owns the buffer and must outlive the core; hand over a non-owning alias.
robot::TFListenerPtr tf(static_cast<tf3::BufferCore *>(tf3_buffer),
[](tf3::BufferCore *) {});
nav->initialize(tf);
// initialize() returns void, so the runtime reports failure the only way it can: it leaves
// is_ready false and writes the reason into the feedback. Not reading it back is how a host
// ends up pushing goals into a core that has no control thread.
robot::move_base_core::NavFeedback *feedback = nav->getFeedback();
if (feedback == nullptr)
{
robot::log_error("navigation_initialize_checked: core exposes no feedback");
return false;
}
if (!feedback->is_ready)
{
robot::log_error("navigation_initialize_checked: core not ready: %s",
feedback->feed_back_str.c_str());
return false;
}
return true;
}
catch (const std::exception &e)
{
robot::log_error("navigation_initialize_checked failed: %s", e.what());
return false;
}
catch (...)
{
robot::log_error("navigation_initialize_checked failed: unknown exception");
return false;
}
}
extern "C" bool navigation_is_ready(NavigationHandle handle)
{
if (!handle)
{
return false;
}
try
{
robot::move_base_core::NavFeedback *feedback = as_navigation(handle)->getFeedback();
return feedback != nullptr && feedback->is_ready;
}
catch (...)
{
return false;
}
}
extern "C" void navigation_shutdown(NavigationHandle handle)
{
if (!handle)
{
return;
}
try
{
as_navigation(handle)->shutdown();
}
catch (const std::exception &e)
{
// Swallowed on purpose: this runs inside the host's shutdown sequence, where throwing across
// the C boundary would abort a process that is already on its way down.
robot::log_error("navigation_shutdown failed: %s", e.what());
}
catch (...)
{
robot::log_error("navigation_shutdown failed: unknown exception");
}
}
// ================================================================================================
// Status
// ================================================================================================
extern "C" bool navigation_get_state(NavigationHandle handle, NavigationState *out_state)
{
if (!handle || !out_state)
{
return false;
}
try
{
robot::move_base_core::NavFeedback *feedback = as_navigation(handle)->getFeedback();
if (feedback == nullptr)
{
return false;
}
*out_state = static_cast<NavigationState>(static_cast<int>(feedback->navigation_state));
return true;
}
catch (...)
{
return false;
}
}
extern "C" char *navigation_get_status_text(NavigationHandle handle)
{
if (!handle)
{
return nullptr;
}
try
{
robot::move_base_core::NavFeedback *feedback = as_navigation(handle)->getFeedback();
if (feedback == nullptr || feedback->feed_back_str.empty())
{
return nullptr;
}
return strdup(feedback->feed_back_str.c_str());
}
catch (...)
{
return nullptr;
}
}
// ================================================================================================
// Sensor input
// ================================================================================================
extern "C" bool navigation_add_depth_camera_data(NavigationHandle handle, const char *topic,
const DepthCameraData data)
{
if (!handle || !topic)
{
return false;
}
try
{
// ConstPtr is a boost::shared_ptr and the core keeps the sample for as long as it needs it, so
// hand it a copy rather than a view of host memory.
auto sample = boost::make_shared<robot_sensor_msgs::DepthCameraData>(
convert2CppDepthCameraData(data));
as_navigation(handle)->addDepthCameraData(std::string(topic), sample);
return true;
}
catch (const std::exception &e)
{
robot::log_error("navigation_add_depth_camera_data failed: %s", e.what());
return false;
}
catch (...)
{
return false;
}
}
// ================================================================================================
// Orders
// ================================================================================================
extern "C" bool navigation_move_to_order_v2(NavigationHandle handle,
const char *order_id, uint32_t order_update_id,
const Node *nodes, size_t node_count,
const Edge *edges, size_t edge_count,
const PoseStamped goal)
{
if (!handle || order_id == nullptr || order_id[0] == '\0')
{
robot::log_error("navigation_move_to_order_v2: null handle or empty orderId");
return false;
}
if (!isQuaternionValid(goal.pose.orientation))
{
robot::log_error("navigation_move_to_order_v2: goal quaternion is invalid");
return false;
}
try
{
const Order order = make_order(order_id, order_update_id, nodes, node_count, edges, edge_count);
return as_navigation(handle)->moveTo(convert2CppOrder(order), convert2CppPoseStamped(goal));
}
catch (const std::exception &e)
{
robot::log_error("navigation_move_to_order_v2 failed: %s", e.what());
return false;
}
catch (...)
{
return false;
}
}
extern "C" bool navigation_dock_to_order_v2(NavigationHandle handle, const char *marker,
const char *order_id, uint32_t order_update_id,
const Node *nodes, size_t node_count,
const Edge *edges, size_t edge_count,
const PoseStamped goal)
{
if (!handle || !marker || order_id == nullptr || order_id[0] == '\0')
{
robot::log_error("navigation_dock_to_order_v2: null handle, marker or empty orderId");
return false;
}
if (!isQuaternionValid(goal.pose.orientation))
{
robot::log_error("navigation_dock_to_order_v2: goal quaternion is invalid");
return false;
}
try
{
const Order order = make_order(order_id, order_update_id, nodes, node_count, edges, edge_count);
return as_navigation(handle)->dockTo(convert2CppOrder(order), std::string(marker),
convert2CppPoseStamped(goal));
}
catch (const std::exception &e)
{
robot::log_error("navigation_dock_to_order_v2 failed: %s", e.what());
return false;
}
catch (...)
{
return false;
}
}
// ================================================================================================
// Releasing what the getters allocate
// ================================================================================================
//
// The converters allocate with malloc()/strdup(), so these release with free(). Mixing in delete[]
// here would be undefined behaviour, and the host cannot free these itself on Windows-style
// runtimes anyway — allocation and release must happen inside the same C runtime.
namespace
{
/// free() a buffer and clear both the pointer and its element count in one step.
template <typename T>
inline void free_array(T *&ptr, size_t &count)
{
free(ptr);
ptr = nullptr;
count = 0;
}
inline void free_string_field(char *&str)
{
free(str);
str = nullptr;
}
} // namespace
extern "C" void navigation_free_occupancy_grid(OccupancyGrid *grid)
{
if (!grid)
{
return;
}
free_string_field(grid->header.frame_id);
free_array(grid->data, grid->data_count);
}
extern "C" void navigation_free_laser_scan(LaserScan *scan)
{
if (!scan)
{
return;
}
free_string_field(scan->header.frame_id);
free_array(scan->ranges, scan->ranges_count);
free_array(scan->intensities, scan->intensities_count);
}
extern "C" void navigation_free_point_cloud(PointCloud *cloud)
{
if (!cloud)
{
return;
}
free_string_field(cloud->header.frame_id);
free_array(cloud->points, cloud->points_count);
// Each channel owns a name and a value buffer of its own; freeing the array alone leaks both.
if (cloud->channels != nullptr)
{
for (size_t i = 0; i < cloud->channels_count; ++i)
{
free_string_field(cloud->channels[i].name);
free_array(cloud->channels[i].values, cloud->channels[i].values_count);
}
}
free_array(cloud->channels, cloud->channels_count);
}
extern "C" void navigation_free_point_cloud2(PointCloud2 *cloud)
{
if (!cloud)
{
return;
}
free_string_field(cloud->header.frame_id);
if (cloud->fields != nullptr)
{
for (size_t i = 0; i < cloud->fields_count; ++i)
{
free_string_field(cloud->fields[i].name);
}
}
free_array(cloud->fields, cloud->fields_count);
free_array(cloud->data, cloud->data_count);
}
extern "C" void navigation_free_planner_data(PlannerDataOutput *data)
{
if (!data)
{
return;
}
if (data->plan.poses != nullptr)
{
for (size_t i = 0; i < data->plan.poses_count; ++i)
{
free_string_field(data->plan.poses[i].header.frame_id);
}
}
free_array(data->plan.poses, data->plan.poses_count);
free_string_field(data->plan.header.frame_id);
navigation_free_occupancy_grid(&data->costmap);
free_string_field(data->costmap_update.header.frame_id);
free_array(data->costmap_update.data, data->costmap_update.data_count);
free_string_field(data->footprint.header.frame_id);
free_array(data->footprint.polygon.points, data->footprint.polygon.points_count);
data->is_costmap_updated = false;
}

2
src/Libraries/common_msgs

Submodule src/Libraries/common_msgs updated: b6c387dd0f...0313fe6aa2

2
src/Libraries/costmap_2d

Submodule src/Libraries/costmap_2d updated: 2fcd211ccf...9f7e2f82f1

BIN
src/Libraries/costmap_2d_dev_1_cam.zip Normal file
View File

Binary file not shown.

BIN
src/Libraries/costmap_2d_dev_multi_cam_test.zip Normal file
View File

Binary file not shown.

2
src/Libraries/data_convert

Submodule src/Libraries/data_convert updated: 1cf62c0856...5570c30deb

BIN
src/Libraries/data_convert.zip Normal file
View File

Binary file not shown.

22
src/Navigations/Cores/move_base_core/include/move_base_core/navigation.h
View File

@@ -14,6 +14,7 @@
#include <robot_nav_msgs/Odometry.h> #include <robot_nav_msgs/Odometry.h>
#include <robot_nav_msgs/OccupancyGrid.h> #include <robot_nav_msgs/OccupancyGrid.h>
#include <robot_map_msgs/OccupancyGridUpdate.h> #include <robot_map_msgs/OccupancyGridUpdate.h>
#include <robot_sensor_msgs/DepthCameraData.h>
#include <robot_sensor_msgs/LaserScan.h> #include <robot_sensor_msgs/LaserScan.h>
#include <robot_sensor_msgs/PointCloud.h> #include <robot_sensor_msgs/PointCloud.h>
#include <robot_sensor_msgs/PointCloud2.h> #include <robot_sensor_msgs/PointCloud2.h>
@@ -184,6 +185,19 @@ namespace robot
*/ */
virtual void initialize(TFListenerPtr tf) = 0; virtual void initialize(TFListenerPtr tf) = 0;
/**
* @brief Stop internal activity (control loop, planner...) before destruction.
*
* The host MUST be able to quiesce the navigation core at a chosen point in its
* own shutdown sequence, while the process is still fully alive — destroying the
* object from a static-destruction context after main() has returned is exactly
* the race that produced boost::lock_error crashes. Default is a no-op so legacy
* implementations remain source-compatible; after this call the object must stop
* producing velocity commands, and queries (getFeedback, getTwist...) must remain
* safe to call. Safe to call multiple times.
*/
virtual void shutdown() {}
/** /**
* @brief Set the robot's footprint (outline shape) in the global frame. * @brief Set the robot's footprint (outline shape) in the global frame.
* This can be used for planning or collision checking. * This can be used for planning or collision checking.
@@ -245,6 +259,14 @@ namespace robot
*/ */
virtual void addPointCloud2(const std::string &point_cloud2_name, robot_sensor_msgs::PointCloud2 point_cloud2) = 0; virtual void addPointCloud2(const std::string &point_cloud2_name, robot_sensor_msgs::PointCloud2 point_cloud2) = 0;
/**
* @brief Add one coherent depth-camera sample to the navigation system.
* @param topic The logical topic of the combined depth-camera stream.
* @param data Depth image and matching camera intrinsics.
*/
virtual void addDepthCameraData(const std::string &topic,
robot_sensor_msgs::DepthCameraData::ConstPtr data) = 0;
/** /**
* @brief Get a static map from the navigation system. * @brief Get a static map from the navigation system.
* @param map_name The name of the map. * @param map_name The name of the map.

1
src/Navigations/Cores/robot_nav_core2/include/robot_nav_core2/global_planner.h
View File

@@ -36,6 +36,7 @@
#include <robot_nav_core2/common.h> #include <robot_nav_core2/common.h>
#include <robot_nav_core2/costmap.h> #include <robot_nav_core2/costmap.h>
#include <robot_nav_core2/exceptions.h>
#include <robot_nav_2d_msgs/Path2D.h> #include <robot_nav_2d_msgs/Path2D.h>
#include <robot_nav_2d_msgs/Pose2DStamped.h> #include <robot_nav_2d_msgs/Pose2DStamped.h>
#include <string> #include <string>

1
src/Navigations/Cores/robot_nav_core2/include/robot_nav_core2/local_planner.h
View File

@@ -36,6 +36,7 @@
#include <robot_nav_core2/common.h> #include <robot_nav_core2/common.h>
#include <robot_nav_core2/costmap.h> #include <robot_nav_core2/costmap.h>
#include <robot_nav_core2/exceptions.h>
#include <robot_nav_2d_msgs/Path2D.h> #include <robot_nav_2d_msgs/Path2D.h>
#include <robot_nav_2d_msgs/Pose2DStamped.h> #include <robot_nav_2d_msgs/Pose2DStamped.h>
#include <robot_nav_2d_msgs/Twist2D.h> #include <robot_nav_2d_msgs/Twist2D.h>

4
src/Navigations/Packages/move_base/CMakeLists.txt
View File

@@ -46,6 +46,7 @@ if (NOT BUILDING_WITH_CATKIN)
robot_cpp robot_cpp
robot_move_base_msgs robot_move_base_msgs
laser_filter laser_filter
mission_adapters
) )
find_library(TF3_LIBRARY NAMES tf3 PATHS /usr/lib /usr/local/lib /usr/lib/x86_64-linux-gnu) find_library(TF3_LIBRARY NAMES tf3 PATHS /usr/lib /usr/local/lib /usr/lib/x86_64-linux-gnu)
else() else()
@@ -65,12 +66,13 @@ else()
robot_nav_2d_utils robot_nav_2d_utils
robot_cpp robot_cpp
robot_move_base_msgs robot_move_base_msgs
mission_adapters
) )
find_library(TF3_LIBRARY NAMES tf3 PATHS /usr/lib /usr/local/lib /usr/lib/x86_64-linux-gnu) find_library(TF3_LIBRARY NAMES tf3 PATHS /usr/lib /usr/local/lib /usr/lib/x86_64-linux-gnu)
catkin_package( catkin_package(
INCLUDE_DIRS include INCLUDE_DIRS include
LIBRARIES ${PROJECT_NAME} LIBRARIES ${PROJECT_NAME}
CATKIN_DEPENDS geometry_msgs robot_std_msgs move_base_core robot_nav_core robot_costmap_2d robot_tf3_sensor_msgs robot_tf3_geometry_msgs data_convert robot_nav_2d_utils robot_cpp robot_move_base_msgs CATKIN_DEPENDS mission_adapters geometry_msgs robot_std_msgs move_base_core robot_nav_core robot_costmap_2d robot_tf3_sensor_msgs robot_tf3_geometry_msgs data_convert robot_nav_2d_utils robot_cpp robot_move_base_msgs
DEPENDS Boost yaml-cpp DEPENDS Boost yaml-cpp
) )

9
src/Navigations/Packages/move_base/include/move_base/move_base.h
View File

@@ -18,6 +18,7 @@
#include <robot_nav_core/base_global_planner.h> #include <robot_nav_core/base_global_planner.h>
#include <robot_nav_core/base_local_planner.h> #include <robot_nav_core/base_local_planner.h>
#include <robot_nav_core/recovery_behavior.h> #include <robot_nav_core/recovery_behavior.h>
#include <mission_adapters/mission_adapters.h>
// boost headers // boost headers
#include <boost/thread.hpp> #include <boost/thread.hpp>
@@ -141,6 +142,14 @@ namespace move_base
*/ */
virtual void addPointCloud2(const std::string &point_cloud2_name, robot_sensor_msgs::PointCloud2 point_cloud2) override; virtual void addPointCloud2(const std::string &point_cloud2_name, robot_sensor_msgs::PointCloud2 point_cloud2) override;
/**
* @brief Add one coherent depth-camera sample to the navigation system.
* @param topic The logical topic of the combined depth-camera stream.
* @param data Depth image and matching camera intrinsics.
*/
virtual void addDepthCameraData(const std::string &topic,
robot_sensor_msgs::DepthCameraData::ConstPtr data) override;
/** /**
* @brief Get a static map from the navigation system. * @brief Get a static map from the navigation system.
* @param map_name The name of the map. * @param map_name The name of the map.

3
src/Navigations/Packages/move_base/package.xml
View File

@@ -49,6 +49,9 @@
<build_depend>robot_cpp</build_depend> <build_depend>robot_cpp</build_depend>
<run_depend>robot_cpp</run_depend> <run_depend>robot_cpp</run_depend>
<build_depend>mission_adapters</build_depend>
<run_depend>mission_adapters</run_depend>
<build_depend>robot_move_base_msgs</build_depend> <build_depend>robot_move_base_msgs</build_depend>
<run_depend>robot_move_base_msgs</run_depend> <run_depend>robot_move_base_msgs</run_depend>

14
src/Navigations/Packages/move_base/src/move_base.cpp
View File

@@ -613,6 +613,20 @@ void move_base::MoveBase::addPointCloud2(const std::string &point_cloud2_name, r
updateGlobalCostmap<robot_sensor_msgs::PointCloud2>(point_cloud2, robot_costmap_2d::LayerType::VOXEL_LAYER, point_cloud2_name); updateGlobalCostmap<robot_sensor_msgs::PointCloud2>(point_cloud2, robot_costmap_2d::LayerType::VOXEL_LAYER, point_cloud2_name);
} }
void move_base::MoveBase::addDepthCameraData(const std::string &topic,
robot_sensor_msgs::DepthCameraData::ConstPtr data)
{
if (!data)
return;
// robot::log_error("DEBUG");
updateLocalCostmap<robot_sensor_msgs::DepthCameraData::ConstPtr>(
data, robot_costmap_2d::LayerType::VOXEL_LAYER, topic);
updateGlobalCostmap<robot_sensor_msgs::DepthCameraData::ConstPtr>(
data, robot_costmap_2d::LayerType::VOXEL_LAYER, topic);
}
robot_sensor_msgs::PointCloud2 move_base::MoveBase::getPointCloud2(const std::string &point_cloud2_name) robot_sensor_msgs::PointCloud2 move_base::MoveBase::getPointCloud2(const std::string &point_cloud2_name)
{ {
auto it = point_cloud2s_.find(point_cloud2_name); auto it = point_cloud2s_.find(point_cloud2_name);