diff --git a/README.md b/README.md index 78333a0..c9edd32 100644 --- a/README.md +++ b/README.md @@ -66,8 +66,26 @@ Log runtime cần thấy: - `Fed depth cloud to robot_costmap_2d` - `Fed DepthCameraData to robot_costmap_2d` -Config local cho `robot_costmap_2d` nằm trong thư mục `config/` của package này. Launch file set: +Config local cho `robot_costmap_2d` nằm trong thư mục `config/config/` của package này khi export: -```xml - +```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: + +```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 +library_path: /home/robotics/DuongTD/dev_ws/devel/lib/libplugins.so ``` diff --git a/config/config/costmap_params.yaml b/config/config/costmap_params.yaml new file mode 100644 index 0000000..21abc8e --- /dev/null +++ b/config/config/costmap_params.yaml @@ -0,0 +1,33 @@ +robot_costmap_2d: + # Derived from pnkx_nav_core/config/costmap_local_params.yaml. + # NUC path is intentional for the user's local test workspace. + library_path: /home/robotics/DuongTD/dev_ws/devel/lib/libplugins.so + global_frame: odom + robot_base_frame: base_link + 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 + + 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 diff --git a/config/config/inflation_layer_params.yaml b/config/config/inflation_layer_params.yaml new file mode 100644 index 0000000..1221237 --- /dev/null +++ b/config/config/inflation_layer_params.yaml @@ -0,0 +1,5 @@ +inflation_layer: + enabled: true + inflate_unknown: false + cost_scaling_factor: 10.0 + inflation_radius: 0.30 diff --git a/config/config/obstacle_layer_params.yaml b/config/config/obstacle_layer_params.yaml new file mode 100644 index 0000000..0fcde92 --- /dev/null +++ b/config/config/obstacle_layer_params.yaml @@ -0,0 +1,41 @@ +obstacle_layer: + enabled: true + track_unknown_space: false + transform_tolerance: 1.0 + footprint_clearing_enabled: true + combination_method: 1 + + # Subset of costmap_common_params.yaml for the topics available in this + # local depth-camera test. + 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/config/static_layer_params.yaml b/config/config/static_layer_params.yaml new file mode 100644 index 0000000..e8e6acd --- /dev/null +++ b/config/config/static_layer_params.yaml @@ -0,0 +1,11 @@ +static_layer: + enabled: true + map_topic: "map" + first_map_only: false + subscribe_to_updates: false + track_unknown_space: true + use_maximum: false + lethal_cost_threshold: 100 + unknown_cost_value: -1 + trinary_costmap: true + base_frame_id: "map" diff --git a/config/config/voxel_layer_params.yaml b/config/config/voxel_layer_params.yaml new file mode 100644 index 0000000..bf3b622 --- /dev/null +++ b/config/config/voxel_layer_params.yaml @@ -0,0 +1,16 @@ +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 diff --git a/config/costmap_params.yaml b/config/costmap_params.yaml index 8343c20..21abc8e 100644 --- a/config/costmap_params.yaml +++ b/config/costmap_params.yaml @@ -1,29 +1,31 @@ robot_costmap_2d: + # Derived from pnkx_nav_core/config/costmap_local_params.yaml. + # NUC path is intentional for the user's local test workspace. + library_path: /home/robotics/DuongTD/dev_ws/devel/lib/libplugins.so global_frame: odom robot_base_frame: base_link rolling_window: true track_unknown_space: false plugins: - - name: voxel_layer + # Derived from costmap_local_params_plugins_no_virtual_walls.yaml. + - name: obstacles type: VoxelLayer - - name: inflation_layer + - name: inflation type: InflationLayer - library_path: /home/duongtd/T800_ws/devel/lib/libplugins.so - footprint: - [0.30, 0.30] - [0.30, -0.30] - [-0.30, -0.30] - [-0.30, 0.30] - transform_tolerance: 0.5 - performance_metrics_enabled: false + transform_tolerance: 1.0 + performance_metrics_enabled: true performance_metrics_period: 5.0 - update_frequency: 5.0 - width: 6.0 - height: 6.0 + update_frequency: 6.0 + width: 8.0 + height: 8.0 resolution: 0.05 origin_x: 0.0 origin_y: 0.0 diff --git a/config/inflation_layer_params.yaml b/config/inflation_layer_params.yaml index 931d5ea..1221237 100644 --- a/config/inflation_layer_params.yaml +++ b/config/inflation_layer_params.yaml @@ -1,5 +1,5 @@ inflation_layer: enabled: true inflate_unknown: false - cost_scaling_factor: 3.0 - inflation_radius: 0.45 + cost_scaling_factor: 10.0 + inflation_radius: 0.30 diff --git a/config/obstacle_layer_params.yaml b/config/obstacle_layer_params.yaml index 7dc672b..0fcde92 100644 --- a/config/obstacle_layer_params.yaml +++ b/config/obstacle_layer_params.yaml @@ -1,10 +1,12 @@ obstacle_layer: + enabled: true track_unknown_space: false - transform_tolerance: 0.5 + transform_tolerance: 1.0 footprint_clearing_enabled: true - max_obstacle_height: 1.5 combination_method: 1 + # Subset of costmap_common_params.yaml for the topics available in this + # local depth-camera test. observation_sources: pc_marking pc_clearing pc_marking: @@ -13,13 +15,13 @@ obstacle_layer: 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.10 + min_obstacle_height: 0.1 max_obstacle_height: 1.0 - frustum_clearing_enabled: false pc_clearing: topic: /camera/depth/data @@ -27,13 +29,13 @@ obstacle_layer: clearing: false marking: true inf_is_valid: false - observation_persistence: 0.0 - expected_update_rate: 0.5 - obstacle_range: 2.5 - raytrace_range: 3.5 - min_obstacle_height: 0.10 - max_obstacle_height: 1.0 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/voxel_layer_params.yaml b/config/voxel_layer_params.yaml index c94275d..bf3b622 100644 --- a/config/voxel_layer_params.yaml +++ b/config/voxel_layer_params.yaml @@ -1,13 +1,14 @@ voxel_layer: enabled: true footprint_clearing_enabled: true - max_obstacle_height: 1.5 + 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 diff --git a/launch/depth_local_costmap_test.launch b/launch/depth_local_costmap_test.launch index 27bb7be..c978f06 100644 --- a/launch/depth_local_costmap_test.launch +++ b/launch/depth_local_costmap_test.launch @@ -13,8 +13,6 @@ type="depth_local_costmap_noetic_test_node" name="depth_local_costmap_noetic_test" output="screen"> - - @@ -26,22 +24,36 @@ - + - - - - - + + + + + + + + + + + + - - - - - - - - - + + + + + + + + + + + + + + + +