From bb461b263d993b1bd13bb5f817fd2945d91b3a1a Mon Sep 17 00:00:00 2001 From: duongtd Date: Tue, 21 Jul 2026 14:38:45 +0700 Subject: [PATCH] test costmap + cam depth 14:38 --- README.md | 19 ++++---- .../costmap_common_params.yaml} | 16 +++---- .../costmap_local_params.yaml} | 43 +++++++++---------- ...local_params_plugins_no_virtual_walls.yaml | 12 ++++++ config/costmap_common_params.yaml | 41 ++++++++++++++++++ config/costmap_local_params.yaml | 32 ++++++++++++++ ...local_params_plugins_no_virtual_walls.yaml | 12 ++++++ config/inflation_layer_params.yaml | 5 --- config/voxel_layer_params.yaml | 16 ------- 9 files changed, 137 insertions(+), 59 deletions(-) rename config/{obstacle_layer_params.yaml => config/costmap_common_params.yaml} (76%) rename config/{costmap_params.yaml => config/costmap_local_params.yaml} (61%) create mode 100644 config/config/costmap_local_params_plugins_no_virtual_walls.yaml create mode 100644 config/costmap_common_params.yaml create mode 100644 config/costmap_local_params.yaml create mode 100644 config/costmap_local_params_plugins_no_virtual_walls.yaml delete mode 100644 config/inflation_layer_params.yaml delete mode 100644 config/voxel_layer_params.yaml diff --git a/README.md b/README.md index 395afc5..0fba676 100644 --- a/README.md +++ b/README.md @@ -80,24 +80,27 @@ Các topic RViz giống hướng `amr_control::AmrPublisher`: - `/local_costmap/costmap` kiểu `nav_msgs/OccupancyGrid` - `/local_costmap/costmap/footprint` kiểu `geometry_msgs/PolygonStamped` -Config local cho `robot_costmap_2d` nằm trong thư mục `config/config/` của package này khi export: +Config local chính của package này dùng cùng kiểu tách file với +`pnkx_nav_core/config`: + +- `config/costmap_common_params.yaml` +- `config/costmap_local_params_plugins_no_virtual_walls.yaml` +- `config/costmap_local_params.yaml` + +Khi test trên NUC, export: ```bash export PNKX_NAV_CORE_CONFIG_DIR=/home/robotics/DuongTD/dev_ws/src/test/depth_local_costmap_noetic_test/config ``` -Lý do là `robot_costmap_2d::getSourceFile()` tự nối thêm `/config`, nên export tới package `config/` sẽ làm runtime đọc: +`robot_cpp::NodeHandle` sẽ auto-load 3 file trên. Hiện `robot_costmap_2d` +vẫn còn một số loader cũ dùng `getSourceFile()` và tự nối thêm `/config`, nên +package cũng giữ các file shim tương thích trong: ```text /home/robotics/DuongTD/dev_ws/src/test/depth_local_costmap_noetic_test/config/config/*.yaml ``` -File test đang bám theo: - -- `costmap_common_params.yaml`: source `pc_marking` và `pc_clearing` -- `costmap_local_params_plugins_no_virtual_walls.yaml`: plugin `obstacles` kiểu `VoxelLayer`, `inflation` kiểu `InflationLayer` -- `costmap_local_params.yaml`: `global_frame=odom`, `rolling_window=true`, `width/height=8.0`, `resolution=0.05` - Với `robot_costmap_2d` chưa patch resolver plugin path, `library_path` trong config được đặt thẳng tới NUC: ```yaml diff --git a/config/obstacle_layer_params.yaml b/config/config/costmap_common_params.yaml similarity index 76% rename from config/obstacle_layer_params.yaml rename to config/config/costmap_common_params.yaml index 0fcde92..46466b8 100644 --- a/config/obstacle_layer_params.yaml +++ b/config/config/costmap_common_params.yaml @@ -1,12 +1,12 @@ -obstacle_layer: - enabled: true - track_unknown_space: false - transform_tolerance: 1.0 - footprint_clearing_enabled: true - combination_method: 1 +robot_base_frame: base_link +transform_tolerance: 1.0 +performance_metrics_enabled: true +performance_metrics_period: 5.0 +obstacle_range: 3.0 +publish_voxel_map: true +footprint_padding: 0.0 - # Subset of costmap_common_params.yaml for the topics available in this - # local depth-camera test. +obstacles: observation_sources: pc_marking pc_clearing pc_marking: diff --git a/config/costmap_params.yaml b/config/config/costmap_local_params.yaml similarity index 61% rename from config/costmap_params.yaml rename to config/config/costmap_local_params.yaml index 21abc8e..778a0f5 100644 --- a/config/costmap_params.yaml +++ b/config/config/costmap_local_params.yaml @@ -1,33 +1,32 @@ -robot_costmap_2d: - # Derived from pnkx_nav_core/config/costmap_local_params.yaml. - # NUC path is intentional for the user's local test workspace. +local_costmap: + # Absolute NUC path is intentional for this local test package. It avoids + # boost::dll resolving a relative value such as "libplugins" to ./libplugins. library_path: /home/robotics/DuongTD/dev_ws/devel/lib/libplugins.so global_frame: odom robot_base_frame: base_link + update_frequency: 6.0 + publish_frequency: 6.0 rolling_window: true track_unknown_space: false - - plugins: - # Derived from costmap_local_params_plugins_no_virtual_walls.yaml. - - name: obstacles - type: VoxelLayer - - name: inflation - type: InflationLayer - + raytrace_range: 3.0 + resolution: 0.05 + z_resolution: 0.15 + z_voxels: 8 + width: 8.0 + height: 8.0 + origin_x: 0.0 + origin_y: 0.0 + transform_tolerance: 1.0 + performance_metrics_enabled: true + performance_metrics_period: 5.0 + footprint_padding: 0.0 + robot_radius: 0.0 footprint: - [0.30, 0.30] - [0.30, -0.30] - [-0.30, -0.30] - [-0.30, 0.30] - transform_tolerance: 1.0 - performance_metrics_enabled: true - performance_metrics_period: 5.0 - update_frequency: 6.0 - width: 8.0 - height: 8.0 - resolution: 0.05 - origin_x: 0.0 - origin_y: 0.0 - footprint_padding: 0.0 - robot_radius: 0.0 + inflation: + cost_scaling_factor: 10.0 + inflation_radius: 0.3 diff --git a/config/config/costmap_local_params_plugins_no_virtual_walls.yaml b/config/config/costmap_local_params_plugins_no_virtual_walls.yaml new file mode 100644 index 0000000..2e3b4ab --- /dev/null +++ b/config/config/costmap_local_params_plugins_no_virtual_walls.yaml @@ -0,0 +1,12 @@ +local_costmap: + frame_id: odom + plugins: + - {name: obstacles, type: "VoxelLayer"} + - {name: inflation, type: "InflationLayer"} + + obstacles: + enabled: true + footprint_clearing_enabled: true + + inflation: + enabled: true diff --git a/config/costmap_common_params.yaml b/config/costmap_common_params.yaml new file mode 100644 index 0000000..46466b8 --- /dev/null +++ b/config/costmap_common_params.yaml @@ -0,0 +1,41 @@ +robot_base_frame: base_link +transform_tolerance: 1.0 +performance_metrics_enabled: true +performance_metrics_period: 5.0 +obstacle_range: 3.0 +publish_voxel_map: true +footprint_padding: 0.0 + +obstacles: + observation_sources: pc_marking pc_clearing + + pc_marking: + topic: /camera/depth/points_proc + data_type: PointCloud2 + 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.1 + max_obstacle_height: 1.0 + + pc_clearing: + topic: /camera/depth/data + data_type: DepthCameraData + clearing: false + marking: true + inf_is_valid: false + frustum_clearing_enabled: true + frustum_clearing_pixel_step: 8 + frustum_min_range: 0.20 + frustum_max_range: 3.5 + observation_persistence: 0.0 + expected_update_rate: 0.5 + obstacle_range: 2.5 + raytrace_range: 3.5 + min_obstacle_height: 0.1 + max_obstacle_height: 1.0 diff --git a/config/costmap_local_params.yaml b/config/costmap_local_params.yaml new file mode 100644 index 0000000..778a0f5 --- /dev/null +++ b/config/costmap_local_params.yaml @@ -0,0 +1,32 @@ +local_costmap: + # Absolute NUC path is intentional for this local test package. It avoids + # boost::dll resolving a relative value such as "libplugins" to ./libplugins. + library_path: /home/robotics/DuongTD/dev_ws/devel/lib/libplugins.so + global_frame: odom + robot_base_frame: base_link + update_frequency: 6.0 + publish_frequency: 6.0 + rolling_window: true + track_unknown_space: false + raytrace_range: 3.0 + resolution: 0.05 + z_resolution: 0.15 + z_voxels: 8 + width: 8.0 + height: 8.0 + origin_x: 0.0 + origin_y: 0.0 + transform_tolerance: 1.0 + performance_metrics_enabled: true + performance_metrics_period: 5.0 + footprint_padding: 0.0 + robot_radius: 0.0 + footprint: + - [0.30, 0.30] + - [0.30, -0.30] + - [-0.30, -0.30] + - [-0.30, 0.30] + + inflation: + cost_scaling_factor: 10.0 + inflation_radius: 0.3 diff --git a/config/costmap_local_params_plugins_no_virtual_walls.yaml b/config/costmap_local_params_plugins_no_virtual_walls.yaml new file mode 100644 index 0000000..2e3b4ab --- /dev/null +++ b/config/costmap_local_params_plugins_no_virtual_walls.yaml @@ -0,0 +1,12 @@ +local_costmap: + frame_id: odom + plugins: + - {name: obstacles, type: "VoxelLayer"} + - {name: inflation, type: "InflationLayer"} + + obstacles: + enabled: true + footprint_clearing_enabled: true + + inflation: + enabled: true diff --git a/config/inflation_layer_params.yaml b/config/inflation_layer_params.yaml deleted file mode 100644 index 1221237..0000000 --- a/config/inflation_layer_params.yaml +++ /dev/null @@ -1,5 +0,0 @@ -inflation_layer: - enabled: true - inflate_unknown: false - cost_scaling_factor: 10.0 - inflation_radius: 0.30 diff --git a/config/voxel_layer_params.yaml b/config/voxel_layer_params.yaml deleted file mode 100644 index bf3b622..0000000 --- a/config/voxel_layer_params.yaml +++ /dev/null @@ -1,16 +0,0 @@ -voxel_layer: - enabled: true - footprint_clearing_enabled: true - max_obstacle_height: 1.0 - origin_z: 0.0 - z_resolution: 0.15 - z_voxels: 8 - unknown_threshold: 15.0 - mark_threshold: 0 - combination_method: 1 - publish_voxel_map: true - frustum_clearing_enabled: false - frustum_clearing_pixel_step: 8 - frustum_min_range: 0.20 - frustum_max_range: 3.5 - frustum_depth_camera_topic: /camera/depth/data