diff --git a/CMAKE_BUILD_README.md b/CMAKE_BUILD_README.md index a6f434c..4361fd5 100644 --- a/CMAKE_BUILD_README.md +++ b/CMAKE_BUILD_README.md @@ -356,7 +356,8 @@ export PNKX_NAV_CORE_CONFIG_DIR=/path/to/config # Chỉ định workspace directory 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 ``` diff --git a/CMakeLists.txt b/CMakeLists.txt index f949b57..e5c34ec 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -150,6 +150,83 @@ if (NOT TARGET robot_clear_costmap_recovery) add_subdirectory(${CMAKE_SOURCE_DIR}/src/Navigations/Libraries/robot_clear_costmap_recovery) 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) # message(STATUS "[move_base] Shared library configured") if (NOT TARGET move_base) @@ -164,5 +241,3 @@ endif() message(STATUS "========================================") message(STATUS "All packages configured successfully") message(STATUS "========================================") - - diff --git a/config/config/voxel_layer_params.yaml b/config/config/voxel_layer_params.yaml index 3dc96dd..b168fdd 100644 --- a/config/config/voxel_layer_params.yaml +++ b/config/config/voxel_layer_params.yaml @@ -8,4 +8,8 @@ voxel_layer: unknown_threshold: 15.0 mark_threshold: 0 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 diff --git a/config/costmap_common_params.yaml b/config/costmap_common_params.yaml index efb6335..be43c2a 100755 --- a/config/costmap_common_params.yaml +++ b/config/costmap_common_params.yaml @@ -1,5 +1,7 @@ robot_base_frame: base_link transform_tolerance: 1.0 +performance_metrics_enabled: true +performance_metrics_period: 5.0 obstacle_range: 3.0 #mark_threshold: 1 publish_voxel_map: true @@ -19,7 +21,7 @@ virtual_walls_map: lethal_cost_threshold: 100 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 # topic: /f_scan # data_type: LaserScan @@ -36,20 +38,13 @@ obstacles: # inf_is_valid: true # min_obstacle_height: 0.0 # max_obstacle_height: 0.25 - b_scan_marking: - 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: + b_scan: topic: /b_scan data_type: LaserScan clearing: true - marking: false + marking: true inf_is_valid: true + frustum_clearing_enabled: false min_obstacle_height: 0.0 max_obstacle_height: 0.25 @@ -59,6 +54,7 @@ obstacles: clearing: false marking: true inf_is_valid: false + frustum_clearing_enabled: false observation_persistence: 0.0 expected_update_rate: 0.5 obstacle_range: 2.5 @@ -67,14 +63,38 @@ obstacles: max_obstacle_height: 1.0 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 - clearing: true - marking: false + clearing: false + marking: true inf_is_valid: false + frustum_clearing_enabled: false observation_persistence: 0.0 expected_update_rate: 0.5 obstacle_range: 2.5 raytrace_range: 3.0 - min_obstacle_height: -0.5 - max_obstacle_height: 5.0 + min_obstacle_height: 0.1 + 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 diff --git a/config/hybrid_local_planner_params.yaml b/config/hybrid_local_planner_params.yaml index 213875a..321000b 100644 --- a/config/hybrid_local_planner_params.yaml +++ b/config/hybrid_local_planner_params.yaml @@ -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: # base_local_planner: "hybrid_local_planner/HybridLocalPlanner" # HybridLocalPlanner: library_path: libhybrid_local_planner # Robot - robot_max_v_ac: 0.4 - robot_max_w_ac: 0.6 - robot_max_v_pt: 1.0 - robot_max_w_pt: 0.6 - robot_min_v: -0.5 - robot_min_w: 0.05 - robot_max_v_backwards_pt: -0.25 - acc_lim_x: 1.0 + max_forward_velocity: 1.0 + max_reverse_velocity: 0.25 # [m/s], positive magnitude + avoidance_max_velocity: 0.4 + max_angular_velocity: 0.6 + min_in_place_angular_velocity: 0.3 + acc_lim_x: 0.1 + decel_lim_x: 0.5 acc_lim_theta: 2.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 footprint_padding: 0.08 use_footprint: true @@ -39,26 +24,32 @@ HybridLocalPlanner: max_global_plan_lookahead_dist: 3.0 global_plan_viapoint_sep: 0.5 global_plan_goal_sep: 0.05 - global_plan_prune_distance: 0.0 predict_time: 3.0 sim_period: 0.1 sim_time_samples: 10 - sim_direction: M_PI / 2.0 # Optimization - w_vel: 0.8 w_omega: 2.5 obs_cost_gain: 1.3 - path_cost_gain: 2.0 + path_cost_gain: 0.5 to_goal_cost_gain: 0.8 - speed_cost_gain: 0.8 + speed_cost_gain: 0.5 # Obstacles obs_range: 2.5 #GeneralSetting - segment_transition_threshold: 0.01 - calibration_factor: 1.5 + segment_transition_threshold: 0.03 # [m], also used as DWA stop distance for intermediate segments + 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 slow_velocity_th: 0.1 - turn_direction_th: 0.1 \ No newline at end of file + 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] diff --git a/config/mission_adapters_params.yaml b/config/mission_adapters_params.yaml new file mode 100644 index 0000000..902a7db --- /dev/null +++ b/config/mission_adapters_params.yaml @@ -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 diff --git a/config/move_base_common_params.yaml b/config/move_base_common_params.yaml index b1c056b..3b83e09 100755 --- a/config/move_base_common_params.yaml +++ b/config/move_base_common_params.yaml @@ -1,43 +1,18 @@ -# position_planner_name: PNKXLocalPlanner -# docking_planner_name: PNKXDockingLocalPlanner -# position_planner_name: StanleyLocalPlanner -# docking_planner_name: StanleyDockingLocalPlanner -position_planner_name: MPPILocalPlanner #HybridLocalPlanner MPPILocalPlanner -docking_planner_name: PNKXDockingLocalPlanner +position_planner_name: PriestLocalPlanner #HybridLocalPlanner MPPILocalPlanner PriestLocalPlanner PNKXLocalPlanner +docking_planner_name: PNKXDockingLocalPlanner #StanleyDockingLocalPlanner PNKXDockingLocalPlanner go_straight_planner_name: PNKXGoStraightLocalPlanner rotate_planner_name: PNKXRotateLocalPlanner base_local_planner: LocalPlannerAdapter base_global_planner: CustomPlanner -# PNKXLocalPlanner: -# 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: +PriestLocalPlanner: base_local_planner: LocalPlannerAdapter - base_global_planner: SBPLLatticePlanner + base_global_planner: CustomPlanner #CustomPlanner SBPLLatticePlanner PNKXDockingLocalPlanner: base_local_planner: LocalPlannerAdapter base_global_planner: TwoPointsPlanner - PNKXGoStraightLocalPlanner: base_local_planner: LocalPlannerAdapter base_global_planner: TwoPointsPlanner @@ -47,18 +22,28 @@ PNKXRotateLocalPlanner: base_global_planner: TwoPointsPlanner ### 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 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... 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_distance: 0.5 -### recovery behaviors -recovery_behavior_enabled: true +## recovery behaviors +# +# 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: [ {name: aggressive_reset, type: ClearCostmapRecovery}, - {name: conservative_reset, type: ClearCostmapRecovery} + {name: conservative_reset, type: ClearCostmapRecovery}, ] conservative_reset: diff --git a/config/mppi_local_planner_params.yaml b/config/mppi_local_planner_params.yaml index 28f8e15..bc90f9d 100644 --- a/config/mppi_local_planner_params.yaml +++ b/config/mppi_local_planner_params.yaml @@ -7,120 +7,92 @@ LocalPlannerAdapter: MPPILocalPlanner: library_path: libmppi_local_planner - # Robot limits [m/s, rad/s, m/s^2, rad/s^2] - max_velocity: 0.5 + # Robot limits [m/s, rad/s] min_velocity: 0.0 - max_y_velocity: 0.0 - min_command_velocity: 0.05 - max_angular_velocity: 1.9 - min_angular_velocity: 0.05 - max_acceleration: 0.5 - max_angular_acceleration: 1.0 - max_vel_trans: 0.5 + # Cao hon reference_velocity mot khoang dem (headroom). Neu de sat ref thi + # nua tren cua nhieu sampling bi clamp o max, keo van toc trung binh xuong + # (do duoc: ref0.45/max0.50 -> chi dat 0.33; max0.65 -> dat 0.41). Robot chay + # duoc >0.6 m/s nen dat 0.65. + max_velocity: 0.65 + max_angular_velocity: 1.0 + 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 - vy_std: 0.2 - wz_std: 0.4 - motion_model: 0 - min_turning_radius: 0.2 + wz_std: 0.25 + reference_velocity: 0.48 - # Goal tolerance [m, rad] - xy_goal_tolerance: 0.1 - yaw_goal_tolerance: 0.1 - 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 + # Path and goal + # Phai lon hon horizon (1.8 m), neu khong terminal goal_weight se keo robot ve + # diem cat cua reference path va lam no giam toc moi chu ky. max_global_plan_lookahead_dist: 3.0 - global_plan_prune_distance: 0.2 - sim_period: 0.1 + xy_goal_tolerance: 0.10 + yaw_goal_tolerance: 0.10 - # Costmap safety - use_costmap_collision_check: true - allow_unknown: true - lethal_cost: 253 - inflation_score_distance: 0.3 - consider_footprint: true + # Costmap and cost weights + obstacle_range: 2.5 + obstacle_cost_threshold: 150 + obstacle_cell_step: 2 + max_obstacle_points: 180 + 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. - constraint_enabled: true - constraint_power: 1.0 - constraint_weight: 4.0 + # Phat hien ket: MPPI co the tra lenh hop le nhung ~0 mai o cuc tieu dia phuong. + # Qua nguong nay controller bao that bai -> move_base PLANNING roi CLEARING. + stuck_velocity_threshold: 0.03 + stuck_omega_threshold: 0.05 + stuck_patience_cycles: 40 - cost_enabled: true - cost_power: 1.0 - cost_weight: 3.81 - cost_critical_cost: 300.0 - cost_near_collision_cost: 253 - cost_collision_cost: 1000000.0 - cost_near_goal_distance: 1.0 - cost_trajectory_point_step: 2 - - goal_enabled: true - goal_power: 1.0 - goal_weight: 5.0 - goal_threshold_to_consider: 1.4 - - goal_angle_enabled: true - goal_angle_power: 1.0 - goal_angle_weight: 3.0 - goal_angle_threshold_to_consider: 0.5 - - 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 + # --- Covariance thich nghi (giai doan 2) --- + # Moi chu ky fit lai do rong nhieu cho TUNG buoc thoi gian tu chinh batch. + # vx_std/wz_std o tren chi con la gia tri khoi tao va gia tri sau reset. + adaptive_covariance: true + gaussian_fitting_lambda: 5.0 # phai < param_lambda de trong so du nhon + # San chon bang thuc nghiem: thap hon -> muot hon nhung cham va sat vat can hon. + min_vx_std: 0.08 + max_vx_std: 0.35 + min_wz_std: 0.10 + max_wz_std: 0.60 + # Khi bi ket, no rong sigma dan qua tung chu ky de thoat cuc tieu dia phuong. + stuck_sigma_inflation: 1.6 + hard_obstacle_cost_threshold: 254 + stage_xy_weight: 60.0 + stage_yaw_weight: 1.0 + stage_speed_weight: 12.0 + terminal_xy_weight: 90.0 + terminal_yaw_weight: 1.0 + terminal_speed_weight: 12.0 \ No newline at end of file diff --git a/config/mprim/mqtt_general.yaml b/config/mprim/mqtt_general.yaml index 1230673..bc22191 100644 --- a/config/mprim/mqtt_general.yaml +++ b/config/mprim/mqtt_general.yaml @@ -1,8 +1,8 @@ -MQTT: - Name: T800 - Host: 172.20.235.170 - Port: 1885 - Client_ID: T800 - Username: robotics - Password: robotics - Keep_Alive: 60 \ No newline at end of file +# MQTT: +# Name: T800 +# Host: 172.20.235.170 +# Port: 1885 +# Client_ID: T800 +# Username: robotics +# Password: robotics +# Keep_Alive: 60 \ No newline at end of file diff --git a/config/priest_local_planner_params.yaml b/config/priest_local_planner_params.yaml new file mode 100644 index 0000000..d2c579c --- /dev/null +++ b/config/priest_local_planner_params.yaml @@ -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 diff --git a/config/recovery_behaviors_params.yaml b/config/recovery_behaviors_params.yaml new file mode 100644 index 0000000..102dd36 --- /dev/null +++ b/config/recovery_behaviors_params.yaml @@ -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 "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 diff --git a/config/sbpl_global_params.yaml b/config/sbpl_global_params.yaml index c619680..dc4d1c0 100644 --- a/config/sbpl_global_params.yaml +++ b/config/sbpl_global_params.yaml @@ -6,6 +6,10 @@ SBPLLatticePlanner: initial_epsilon: 1.0 force_scratch_limit: 10000 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 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 \ No newline at end of file + primitive_filename: /home/duongtd/rl_ws/mir_robot/mir_navigation/mprim/unicycle_highcost_5cm.mprim \ No newline at end of file diff --git a/config/stanley_local_planner_params.yaml b/config/stanley_local_planner_params.yaml index 494379c..5cc1a76 100644 --- a/config/stanley_local_planner_params.yaml +++ b/config/stanley_local_planner_params.yaml @@ -1,14 +1,115 @@ LocalPlannerAdapter: library_path: liblocal_planner_adapter - yaw_goal_tolerance: 0.017 - xy_goal_tolerance: 0.03 + yaw_goal_tolerance: 0.017 + xy_goal_tolerance: 0.03 min_approach_linear_velocity: 0.06 StanleyLocalPlanner: - # Algorithm library_path: libstanley_local_planner -StanleyDockingLocalPlanner: - # Algorithm + # ================= 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: library_path: libstanley_local_planner - \ No newline at end of file diff --git a/src/APIs/c_api/NAV_C_API_2_GUIDE.md b/src/APIs/c_api/NAV_C_API_2_GUIDE.md new file mode 100644 index 0000000..71fbf3d --- /dev/null +++ b/src/APIs/c_api/NAV_C_API_2_GUIDE.md @@ -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); +} +``` diff --git a/src/APIs/c_api/include/convertor_2.h b/src/APIs/c_api/include/convertor_2.h new file mode 100644 index 0000000..47ad8fe --- /dev/null +++ b/src/APIs/c_api/include/convertor_2.h @@ -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 +#include +#include +#include + +/** + * @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 diff --git a/src/APIs/c_api/include/nav_c_api_2.h b/src/APIs/c_api/include/nav_c_api_2.h new file mode 100644 index 0000000..c1f6715 --- /dev/null +++ b/src/APIs/c_api/include/nav_c_api_2.h @@ -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 +#include +#include + +#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 diff --git a/src/APIs/c_api/include/nav_c_api_io_structure.md b/src/APIs/c_api/include/nav_c_api_io_structure.md deleted file mode 100644 index e69de29..0000000 diff --git a/src/APIs/c_api/include/sensor_msgs/DepthCameraData.h b/src/APIs/c_api/include/sensor_msgs/DepthCameraData.h new file mode 100644 index 0000000..dcd8be6 --- /dev/null +++ b/src/APIs/c_api/include/sensor_msgs/DepthCameraData.h @@ -0,0 +1,36 @@ +#ifndef C_API_SENSOR_MSGS_DEPTHCAMERADATA_H +#define C_API_SENSOR_MSGS_DEPTHCAMERADATA_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#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 diff --git a/src/APIs/c_api/src/convertor_2.cpp b/src/APIs/c_api/src/convertor_2.cpp new file mode 100644 index 0000000..85a6637 --- /dev/null +++ b/src/APIs/c_api/src/convertor_2.cpp @@ -0,0 +1,78 @@ +#include "convertor_2.h" + +#include +#include +#include + +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; +} diff --git a/src/APIs/c_api/src/nav_c_api.cpp b/src/APIs/c_api/src/nav_c_api.cpp index 0292a09..a269992 100644 --- a/src/APIs/c_api/src/nav_c_api.cpp +++ b/src/APIs/c_api/src/nav_c_api.cpp @@ -317,7 +317,9 @@ extern "C" bool navigation_move_to_nodes_edges(NavigationHandle handle, const No if (!nav_ptr) return false; 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(nodes); order.nodes_count = node_count; order.edges = const_cast(edges); @@ -416,7 +418,8 @@ extern "C" bool navigation_dock_to_nodes_edges(NavigationHandle handle, const ch reinterpret_cast(handle), [](::robot::move_base_core::BaseNavigation *) {}); if (!nav_ptr) return false; - Order order; + // Zero-init: see navigation_move_to_nodes_edges. + Order order{}; order.nodes = const_cast(nodes); order.nodes_count = node_count; order.edges = const_cast(edges); diff --git a/src/APIs/c_api/src/nav_c_api_2.cpp b/src/APIs/c_api/src/nav_c_api_2.cpp new file mode 100644 index 0000000..1d38efc --- /dev/null +++ b/src/APIs/c_api/src/nav_c_api_2.cpp @@ -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 +#include +#include + +#include + +#include +#include +#include +#include + +#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(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(order_id); + order.orderUpdateId = order_update_id; + order.nodes = const_cast(nodes); + order.nodes_count = nodes != nullptr ? node_count : 0; + order.edges = const_cast(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_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(static_cast(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( + 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 +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; +} diff --git a/src/Libraries/common_msgs b/src/Libraries/common_msgs index b6c387d..0313fe6 160000 --- a/src/Libraries/common_msgs +++ b/src/Libraries/common_msgs @@ -1 +1 @@ -Subproject commit b6c387dd0fcd4350edf2841767356ad6ee5f2c9e +Subproject commit 0313fe6aa2c2fb87b5e792d9d804f0b0f4b60cb8 diff --git a/src/Libraries/costmap_2d b/src/Libraries/costmap_2d index 2fcd211..9f7e2f8 160000 --- a/src/Libraries/costmap_2d +++ b/src/Libraries/costmap_2d @@ -1 +1 @@ -Subproject commit 2fcd211ccf121e28845f365550e7c21c68b26fc9 +Subproject commit 9f7e2f82f11ac1401cc94ff831d9d3e18921a8a7 diff --git a/src/Libraries/costmap_2d_dev_1_cam.zip b/src/Libraries/costmap_2d_dev_1_cam.zip new file mode 100644 index 0000000..d287e80 Binary files /dev/null and b/src/Libraries/costmap_2d_dev_1_cam.zip differ diff --git a/src/Libraries/costmap_2d_dev_multi_cam_test.zip b/src/Libraries/costmap_2d_dev_multi_cam_test.zip new file mode 100644 index 0000000..7436002 Binary files /dev/null and b/src/Libraries/costmap_2d_dev_multi_cam_test.zip differ diff --git a/src/Libraries/data_convert b/src/Libraries/data_convert index 1cf62c0..5570c30 160000 --- a/src/Libraries/data_convert +++ b/src/Libraries/data_convert @@ -1 +1 @@ -Subproject commit 1cf62c085687dc167e28cded970371b329a83479 +Subproject commit 5570c30deb061fd5b94e405017692ba5c50af4bc diff --git a/src/Libraries/data_convert.zip b/src/Libraries/data_convert.zip new file mode 100644 index 0000000..df05a40 Binary files /dev/null and b/src/Libraries/data_convert.zip differ diff --git a/src/Navigations/Cores/move_base_core/include/move_base_core/navigation.h b/src/Navigations/Cores/move_base_core/include/move_base_core/navigation.h index aef4d41..4b0fdfe 100755 --- a/src/Navigations/Cores/move_base_core/include/move_base_core/navigation.h +++ b/src/Navigations/Cores/move_base_core/include/move_base_core/navigation.h @@ -14,6 +14,7 @@ #include #include #include +#include #include #include #include @@ -184,6 +185,19 @@ namespace robot */ 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. * 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; + /** + * @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. * @param map_name The name of the map. diff --git a/src/Navigations/Cores/robot_nav_core2/include/robot_nav_core2/global_planner.h b/src/Navigations/Cores/robot_nav_core2/include/robot_nav_core2/global_planner.h index b437c3e..90465f6 100755 --- a/src/Navigations/Cores/robot_nav_core2/include/robot_nav_core2/global_planner.h +++ b/src/Navigations/Cores/robot_nav_core2/include/robot_nav_core2/global_planner.h @@ -36,6 +36,7 @@ #include #include +#include #include #include #include diff --git a/src/Navigations/Cores/robot_nav_core2/include/robot_nav_core2/local_planner.h b/src/Navigations/Cores/robot_nav_core2/include/robot_nav_core2/local_planner.h index 55802a8..24f8120 100755 --- a/src/Navigations/Cores/robot_nav_core2/include/robot_nav_core2/local_planner.h +++ b/src/Navigations/Cores/robot_nav_core2/include/robot_nav_core2/local_planner.h @@ -36,6 +36,7 @@ #include #include +#include #include #include #include diff --git a/src/Navigations/Packages/move_base/CMakeLists.txt b/src/Navigations/Packages/move_base/CMakeLists.txt index f1af110..2f761e0 100644 --- a/src/Navigations/Packages/move_base/CMakeLists.txt +++ b/src/Navigations/Packages/move_base/CMakeLists.txt @@ -46,6 +46,7 @@ if (NOT BUILDING_WITH_CATKIN) robot_cpp robot_move_base_msgs laser_filter + mission_adapters ) find_library(TF3_LIBRARY NAMES tf3 PATHS /usr/lib /usr/local/lib /usr/lib/x86_64-linux-gnu) else() @@ -65,12 +66,13 @@ else() robot_nav_2d_utils robot_cpp robot_move_base_msgs + mission_adapters ) find_library(TF3_LIBRARY NAMES tf3 PATHS /usr/lib /usr/local/lib /usr/lib/x86_64-linux-gnu) catkin_package( INCLUDE_DIRS include 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 ) diff --git a/src/Navigations/Packages/move_base/include/move_base/move_base.h b/src/Navigations/Packages/move_base/include/move_base/move_base.h index 5628a8d..bb8b4fc 100644 --- a/src/Navigations/Packages/move_base/include/move_base/move_base.h +++ b/src/Navigations/Packages/move_base/include/move_base/move_base.h @@ -18,6 +18,7 @@ #include #include #include +#include // boost headers #include @@ -140,6 +141,14 @@ namespace move_base * @param point_cloud2 The point cloud2 to add. */ 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. @@ -705,4 +714,4 @@ namespace move_base } // namespace move_base -#endif // NAV_MOVE_BASE_ACTION_H_ \ No newline at end of file +#endif // NAV_MOVE_BASE_ACTION_H_ diff --git a/src/Navigations/Packages/move_base/package.xml b/src/Navigations/Packages/move_base/package.xml index 3a8f0bc..9746e12 100644 --- a/src/Navigations/Packages/move_base/package.xml +++ b/src/Navigations/Packages/move_base/package.xml @@ -49,6 +49,9 @@ robot_cpp robot_cpp + mission_adapters + mission_adapters + robot_move_base_msgs robot_move_base_msgs diff --git a/src/Navigations/Packages/move_base/src/move_base.cpp b/src/Navigations/Packages/move_base/src/move_base.cpp index 08d49ab..c63bb2d 100644 --- a/src/Navigations/Packages/move_base/src/move_base.cpp +++ b/src/Navigations/Packages/move_base/src/move_base.cpp @@ -613,6 +613,20 @@ void move_base::MoveBase::addPointCloud2(const std::string &point_cloud2_name, r updateGlobalCostmap(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( + data, robot_costmap_2d::LayerType::VOXEL_LAYER, topic); + updateGlobalCostmap( + data, robot_costmap_2d::LayerType::VOXEL_LAYER, topic); +} + robot_sensor_msgs::PointCloud2 move_base::MoveBase::getPointCloud2(const std::string &point_cloud2_name) { auto it = point_cloud2s_.find(point_cloud2_name); @@ -3249,4 +3263,4 @@ robot::move_base_core::BaseNavigation::Ptr move_base::MoveBase::create() return std::make_shared(); } -BOOST_DLL_ALIAS(move_base::MoveBase::create, MoveBase) \ No newline at end of file +BOOST_DLL_ALIAS(move_base::MoveBase::create, MoveBase)