Compare commits
5 Commits
main
...
dev_multi_
| Author | SHA1 | Date | |
|---|---|---|---|
| f0390fcc67 | |||
| 28a32ee67c | |||
| 03b13f6936 | |||
| bdbb03aa51 | |||
| 6a9834d3a8 |
@@ -299,6 +299,7 @@ if(BUILD_COSTMAP_TESTS)
|
|||||||
if(EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/test/coordinates_test.cpp)
|
if(EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/test/coordinates_test.cpp)
|
||||||
add_executable(test_costmap test/coordinates_test.cpp)
|
add_executable(test_costmap test/coordinates_test.cpp)
|
||||||
target_link_libraries(test_costmap PRIVATE
|
target_link_libraries(test_costmap PRIVATE
|
||||||
|
plugins
|
||||||
robot_costmap_2d
|
robot_costmap_2d
|
||||||
GTest::GTest
|
GTest::GTest
|
||||||
GTest::Main
|
GTest::Main
|
||||||
|
|||||||
636
README.md
Normal file
636
README.md
Normal file
@@ -0,0 +1,636 @@
|
|||||||
|
# robot_costmap_2d
|
||||||
|
|
||||||
|
`robot_costmap_2d` là thư viện costmap dạng nhiều lớp của T800. Package duy trì
|
||||||
|
lưới chi phí 2D dùng cho global/local planner, nhận bản đồ tĩnh và dữ liệu cảm
|
||||||
|
biến, xóa vùng trống, đánh dấu vật cản, sau đó tạo vùng chi phí an toàn quanh
|
||||||
|
vật cản.
|
||||||
|
|
||||||
|
Package hỗ trợ C++17, catkin và standalone CMake. Các plugin được nạp bằng
|
||||||
|
`boost::dll` từ thư viện `libplugins`.
|
||||||
|
|
||||||
|
## 1. Luồng dữ liệu và kiến trúc
|
||||||
|
|
||||||
|
Luồng cập nhật chính:
|
||||||
|
|
||||||
|
```text
|
||||||
|
OccupancyGrid -------------------------> StaticLayer ---------+
|
||||||
|
LaserScan / PointCloud / PointCloud2 --> ObstacleLayer -------+--> master costmap
|
||||||
|
PointCloud2 + DepthCameraData ---------> VoxelLayer ----------+
|
||||||
|
master costmap ------------------------> InflationLayer -------+
|
||||||
|
```
|
||||||
|
|
||||||
|
Mỗi chu kỳ, `LayeredCostmap` gọi lần lượt:
|
||||||
|
|
||||||
|
1. `updateBounds()` để từng layer mở rộng vùng cần cập nhật.
|
||||||
|
2. Reset vùng tương ứng trên master costmap.
|
||||||
|
3. `updateCosts()` theo đúng thứ tự trong danh sách `plugins`.
|
||||||
|
|
||||||
|
Vì vậy thứ tự plugin có ảnh hưởng trực tiếp tới kết quả. Trong cấu hình chạy
|
||||||
|
thực tế, nên đặt layer bản đồ trước, layer vật cản sau và `InflationLayer` cuối
|
||||||
|
cùng để cả vật cản tĩnh lẫn vật cản động đều được inflation.
|
||||||
|
|
||||||
|
Các plugin được build trong package:
|
||||||
|
|
||||||
|
| Plugin | Vai trò |
|
||||||
|
| --- | --- |
|
||||||
|
| `StaticLayer` | Đưa `OccupancyGrid` tĩnh vào costmap. |
|
||||||
|
| `ObstacleLayer` | Marking/clearing 2D từ `LaserScan`, `PointCloud`, `PointCloud2`. |
|
||||||
|
| `VoxelLayer` | Lưu vật cản theo voxel 3D, chiếu kết quả xuống costmap 2D và hỗ trợ clearing theo frustum depth camera. |
|
||||||
|
| `InflationLayer` | Tạo vùng chi phí giảm dần quanh ô vật cản. |
|
||||||
|
| `CriticalLayer` | Gộp vùng critical do hệ thống T800 cung cấp. |
|
||||||
|
| `DirectionalLayer` | Gộp thông tin vùng có hướng di chuyển. |
|
||||||
|
| `PreferredLayer` | Gộp vùng ưu tiên. |
|
||||||
|
| `UnPreferredLayer` | Gộp vùng không ưu tiên. |
|
||||||
|
|
||||||
|
## 2. Cách package nạp cấu hình
|
||||||
|
|
||||||
|
Tham số có ba tầng ưu tiên, tầng sau ghi đè tầng trước:
|
||||||
|
|
||||||
|
1. Giá trị fallback trong code khi một key không tồn tại trong YAML.
|
||||||
|
2. Các file YAML tên cố định nằm dưới thư mục `config/`.
|
||||||
|
3. Tham số trong `robot::NodeHandle`, thường được launch nạp cho
|
||||||
|
`global_costmap` hoặc `local_costmap`.
|
||||||
|
|
||||||
|
Biến môi trường `PNKX_NAV_CORE_CONFIG_DIR` phải trỏ tới thư mục cha có thư mục
|
||||||
|
con `config/`. Hàm nạp sẽ tìm đệ quy các file sau:
|
||||||
|
|
||||||
|
- `costmap_params.yaml`
|
||||||
|
- `static_layer_params.yaml`
|
||||||
|
- `obstacle_layer_params.yaml`
|
||||||
|
- `voxel_layer_params.yaml`
|
||||||
|
- `inflation_layer_params.yaml`
|
||||||
|
|
||||||
|
Ví dụ dùng các file mặc định nằm ngay trong package:
|
||||||
|
|
||||||
|
```bash
|
||||||
|
export PNKX_NAV_CORE_CONFIG_DIR=/home/duongtd/T800_ws/src/AMR_T800/pnkx_nav_core/src/Libraries/costmap_2d
|
||||||
|
```
|
||||||
|
|
||||||
|
Ở hệ thống chạy thật, cấu hình launch-facing nằm trong
|
||||||
|
`Controllers/Packages/amr_startup/config/`; launch nạp
|
||||||
|
`costmap_common_params.yaml` riêng vào namespace `global_costmap` và
|
||||||
|
`local_costmap`, sau đó nạp file global/local tương ứng.
|
||||||
|
|
||||||
|
> Không đặt nhiều file trùng tên trong các nhánh con của cùng một thư mục
|
||||||
|
> `config/`. Hàm tìm kiếm dừng ở file đầu tiên tìm thấy, nên nguồn cấu hình sẽ
|
||||||
|
> khó xác định.
|
||||||
|
|
||||||
|
## 3. Cấu hình mặc định của package
|
||||||
|
|
||||||
|
Các file trong `config/` là fallback và cũng được test của package sử dụng.
|
||||||
|
Chúng mô tả giá trị mặc định, không phải cấu hình hoàn chỉnh để chạy robot.
|
||||||
|
|
||||||
|
### 3.1. Costmap chính
|
||||||
|
|
||||||
|
File `config/costmap_params.yaml`:
|
||||||
|
|
||||||
|
```yaml
|
||||||
|
robot_costmap_2d:
|
||||||
|
global_frame: map
|
||||||
|
robot_base_frame: base_link
|
||||||
|
rolling_window: false
|
||||||
|
track_unknown_space: false
|
||||||
|
|
||||||
|
plugins:
|
||||||
|
- name: static_layer
|
||||||
|
type: StaticLayer
|
||||||
|
- name: inflation_layer
|
||||||
|
type: InflationLayer
|
||||||
|
- name: obstacle_layer
|
||||||
|
type: ObstacleLayer
|
||||||
|
- name: voxel_layer
|
||||||
|
type: VoxelLayer
|
||||||
|
|
||||||
|
library_path: ./libplugins.so
|
||||||
|
|
||||||
|
footprint:
|
||||||
|
- [0.3, 0.3]
|
||||||
|
- [0.3, -0.3]
|
||||||
|
- [-0.3, -0.3]
|
||||||
|
- [-0.3, 0.3]
|
||||||
|
|
||||||
|
transform_tolerance: 0.0
|
||||||
|
performance_metrics_enabled: false
|
||||||
|
performance_metrics_period: 5.0
|
||||||
|
update_frequency: 1.0
|
||||||
|
width: 0.0
|
||||||
|
height: 0.0
|
||||||
|
resolution: 0.0
|
||||||
|
origin_x: 0.0
|
||||||
|
origin_y: 0.0
|
||||||
|
footprint_padding: 0.0
|
||||||
|
robot_radius: 0.0
|
||||||
|
```
|
||||||
|
|
||||||
|
Các giá trị `width`, `height` và `resolution` bằng `0.0` chỉ là placeholder.
|
||||||
|
Khi không có `StaticLayer` resize costmap từ bản đồ, bắt buộc ghi đè cả ba giá
|
||||||
|
trị bằng số dương trước khi chạy.
|
||||||
|
|
||||||
|
Danh sách plugin fallback ở trên phản ánh file hiện tại. Cấu hình deployment
|
||||||
|
nên khai báo lại plugin và đặt `InflationLayer` cuối danh sách.
|
||||||
|
|
||||||
|
### 3.2. Static layer
|
||||||
|
|
||||||
|
File `config/static_layer_params.yaml`:
|
||||||
|
|
||||||
|
```yaml
|
||||||
|
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
|
||||||
|
```
|
||||||
|
|
||||||
|
### 3.3. Obstacle layer
|
||||||
|
|
||||||
|
File `config/obstacle_layer_params.yaml` hiện chứa các giá trị cơ sở:
|
||||||
|
|
||||||
|
```yaml
|
||||||
|
obstacle_layer:
|
||||||
|
track_unknown_space: true
|
||||||
|
transform_tolerance: 0.2
|
||||||
|
topic: map
|
||||||
|
sensor_frame: laser_frame
|
||||||
|
observation_persistence: 0.0
|
||||||
|
expected_update_rate: 0.0
|
||||||
|
data_type: PointCloud
|
||||||
|
min_obstacle_height: 0.0
|
||||||
|
max_obstacle_height: 2.0
|
||||||
|
inf_is_valid: false
|
||||||
|
clearing: false
|
||||||
|
marking: true
|
||||||
|
obstacle_range: 2.5
|
||||||
|
raytrace_range: 3.0
|
||||||
|
footprint_clearing_enabled: true
|
||||||
|
combination_method: 1
|
||||||
|
```
|
||||||
|
|
||||||
|
`ObstacleLayer` chỉ tạo buffer khi có `observation_sources`. Các tham số
|
||||||
|
`topic`, `data_type`, `marking`, `clearing`, range và height phải được đặt dưới
|
||||||
|
từng source. Do file fallback trên chưa khai báo `observation_sources`, nó không
|
||||||
|
tự đăng ký nguồn cảm biến nào.
|
||||||
|
|
||||||
|
### 3.4. Voxel layer
|
||||||
|
|
||||||
|
File `config/voxel_layer_params.yaml`:
|
||||||
|
|
||||||
|
```yaml
|
||||||
|
voxel_layer:
|
||||||
|
enabled: true
|
||||||
|
footprint_clearing_enabled: true
|
||||||
|
max_obstacle_height: 3.0
|
||||||
|
origin_z: 0.0
|
||||||
|
z_resolution: 0.2
|
||||||
|
z_voxels: 16
|
||||||
|
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
|
||||||
|
```
|
||||||
|
|
||||||
|
Trong implementation hiện tại, các tham số `frustum_*` được đọc theo từng
|
||||||
|
observation source bởi `ObstacleLayer`, là lớp cha của `VoxelLayer`. Vì vậy,
|
||||||
|
đừng chỉ chỉnh các key `frustum_*` trong `voxel_layer_params.yaml`; hãy đặt
|
||||||
|
chúng dưới source depth camera trong `costmap_common_params.yaml`.
|
||||||
|
|
||||||
|
Topic dùng để dispatch dữ liệu depth là `topic` của source. Key
|
||||||
|
`frustum_depth_camera_topic` vẫn có trong cấu hình fallback nhưng không thay thế
|
||||||
|
cho `pc_clearing.topic` trong contract hiện tại.
|
||||||
|
|
||||||
|
### 3.5. Inflation layer
|
||||||
|
|
||||||
|
File `config/inflation_layer_params.yaml`:
|
||||||
|
|
||||||
|
```yaml
|
||||||
|
inflation_layer:
|
||||||
|
enabled: true
|
||||||
|
inflate_unknown: false
|
||||||
|
cost_scaling_factor: 15.0
|
||||||
|
inflation_radius: 0.55
|
||||||
|
```
|
||||||
|
|
||||||
|
## 4. Cấu hình tham khảo cho T800
|
||||||
|
|
||||||
|
Ví dụ sau dùng laser để marking/clearing 2D, point cloud đã xử lý để marking
|
||||||
|
vật cản 3D và message gộp `DepthCameraData` để clearing theo frustum.
|
||||||
|
|
||||||
|
### 4.1. Tham số dùng chung cho global và local costmap
|
||||||
|
|
||||||
|
```yaml
|
||||||
|
robot_base_frame: base_link
|
||||||
|
transform_tolerance: 1.0
|
||||||
|
footprint_padding: 0.0
|
||||||
|
|
||||||
|
# Polygon phải đo theo robot thật, đơn vị mét, trong robot_base_frame.
|
||||||
|
footprint:
|
||||||
|
- [0.583, -0.48]
|
||||||
|
- [0.583, 0.48]
|
||||||
|
- [-0.583, 0.48]
|
||||||
|
- [-0.583, -0.48]
|
||||||
|
|
||||||
|
obstacles:
|
||||||
|
observation_sources: b_scan pc_marking pc_clearing
|
||||||
|
|
||||||
|
b_scan:
|
||||||
|
topic: /b_scan
|
||||||
|
data_type: LaserScan
|
||||||
|
sensor_frame: ""
|
||||||
|
marking: true
|
||||||
|
clearing: true
|
||||||
|
inf_is_valid: true
|
||||||
|
observation_persistence: 0.0
|
||||||
|
expected_update_rate: 0.0
|
||||||
|
obstacle_range: 2.5
|
||||||
|
raytrace_range: 3.0
|
||||||
|
min_obstacle_height: 0.0
|
||||||
|
max_obstacle_height: 0.25
|
||||||
|
frustum_clearing_enabled: false
|
||||||
|
|
||||||
|
# Giữ PointCloud2 cho marking và persistence nếu cần.
|
||||||
|
pc_marking:
|
||||||
|
topic: /camera/depth/points_proc
|
||||||
|
data_type: PointCloud2
|
||||||
|
sensor_frame: ""
|
||||||
|
marking: true
|
||||||
|
clearing: false
|
||||||
|
inf_is_valid: false
|
||||||
|
observation_persistence: 0.0
|
||||||
|
expected_update_rate: 0.5
|
||||||
|
obstacle_range: 2.5
|
||||||
|
raytrace_range: 3.0
|
||||||
|
min_obstacle_height: 0.10
|
||||||
|
max_obstacle_height: 1.00
|
||||||
|
frustum_clearing_enabled: false
|
||||||
|
|
||||||
|
# Clearing dùng raw depth + CameraInfo trong cùng một message.
|
||||||
|
pc_clearing:
|
||||||
|
topic: /camera/depth/data
|
||||||
|
data_type: DepthCameraData
|
||||||
|
sensor_frame: ""
|
||||||
|
marking: false
|
||||||
|
clearing: false
|
||||||
|
observation_persistence: 0.0
|
||||||
|
expected_update_rate: 0.5
|
||||||
|
frustum_clearing_enabled: true
|
||||||
|
frustum_clearing_pixel_step: 8
|
||||||
|
frustum_min_range: 0.20
|
||||||
|
frustum_max_range: 3.5
|
||||||
|
```
|
||||||
|
|
||||||
|
Với `DepthCameraData`, cờ `frustum_clearing_enabled: true` chọn buffer depth
|
||||||
|
riêng. Clearing được thực hiện trực tiếp theo tia camera trong `VoxelLayer`, vì
|
||||||
|
vậy không cần đặt `clearing: true` cho source này.
|
||||||
|
|
||||||
|
Message `DepthCameraData` phải thỏa các điều kiện:
|
||||||
|
|
||||||
|
- Depth encoding là `16UC1`, `mono16` hoặc `32FC1`.
|
||||||
|
- `width`, `height`, `step` và kích thước `data` hợp lệ.
|
||||||
|
- `CameraInfo.K[0]` (`fx`) và `K[4]` (`fy`) lớn hơn `0`.
|
||||||
|
- Kích thước depth và camera info khớp nhau nếu camera info khai báo kích thước.
|
||||||
|
- Frame của depth và camera info không mâu thuẫn.
|
||||||
|
- Có TF từ optical frame của camera tới `global_frame` costmap.
|
||||||
|
|
||||||
|
### 4.2. Global costmap
|
||||||
|
|
||||||
|
```yaml
|
||||||
|
global_costmap:
|
||||||
|
library_path: libplugins
|
||||||
|
global_frame: map
|
||||||
|
robot_base_frame: base_link
|
||||||
|
update_frequency: 1.0
|
||||||
|
rolling_window: false
|
||||||
|
track_unknown_space: true
|
||||||
|
resolution: 0.05
|
||||||
|
|
||||||
|
plugins:
|
||||||
|
- {name: navigation_map, type: StaticLayer}
|
||||||
|
- {name: obstacles, type: VoxelLayer}
|
||||||
|
- {name: inflation, type: InflationLayer}
|
||||||
|
|
||||||
|
navigation_map:
|
||||||
|
enabled: true
|
||||||
|
map_topic: /map
|
||||||
|
track_unknown_space: true
|
||||||
|
trinary_costmap: true
|
||||||
|
lethal_cost_threshold: 100
|
||||||
|
|
||||||
|
obstacles:
|
||||||
|
enabled: true
|
||||||
|
footprint_clearing_enabled: true
|
||||||
|
origin_z: 0.0
|
||||||
|
z_resolution: 0.2
|
||||||
|
z_voxels: 16
|
||||||
|
unknown_threshold: 15
|
||||||
|
mark_threshold: 0
|
||||||
|
combination_method: 1
|
||||||
|
|
||||||
|
inflation:
|
||||||
|
enabled: true
|
||||||
|
inflate_unknown: false
|
||||||
|
inflation_radius: 0.60
|
||||||
|
cost_scaling_factor: 10.0
|
||||||
|
```
|
||||||
|
|
||||||
|
Khi dùng static map, kích thước, resolution và origin có thể được lấy từ
|
||||||
|
`OccupancyGrid`. Nếu tắt static map, phải khai báo `width`, `height`,
|
||||||
|
`resolution`, `origin_x` và `origin_y` hợp lệ.
|
||||||
|
|
||||||
|
### 4.3. Local costmap
|
||||||
|
|
||||||
|
```yaml
|
||||||
|
local_costmap:
|
||||||
|
library_path: libplugins
|
||||||
|
global_frame: odom
|
||||||
|
robot_base_frame: base_link
|
||||||
|
update_frequency: 6.0
|
||||||
|
rolling_window: true
|
||||||
|
track_unknown_space: false
|
||||||
|
width: 8.0
|
||||||
|
height: 8.0
|
||||||
|
resolution: 0.05
|
||||||
|
origin_x: 0.0
|
||||||
|
origin_y: 0.0
|
||||||
|
|
||||||
|
plugins:
|
||||||
|
- {name: obstacles, type: VoxelLayer}
|
||||||
|
- {name: inflation, type: InflationLayer}
|
||||||
|
|
||||||
|
obstacles:
|
||||||
|
enabled: true
|
||||||
|
footprint_clearing_enabled: true
|
||||||
|
origin_z: 0.0
|
||||||
|
z_resolution: 0.15
|
||||||
|
z_voxels: 8
|
||||||
|
unknown_threshold: 7
|
||||||
|
mark_threshold: 0
|
||||||
|
combination_method: 1
|
||||||
|
|
||||||
|
inflation:
|
||||||
|
enabled: true
|
||||||
|
inflate_unknown: false
|
||||||
|
inflation_radius: 0.55
|
||||||
|
cost_scaling_factor: 10.0
|
||||||
|
```
|
||||||
|
|
||||||
|
Với rolling window, code tự cập nhật origin theo pose robot; `origin_x` và
|
||||||
|
`origin_y` ban đầu không phải tâm cửa sổ cố định quanh robot.
|
||||||
|
|
||||||
|
## 5. Ý nghĩa và cách chỉnh từng nhóm tham số
|
||||||
|
|
||||||
|
### 5.1. Hình học và kích thước costmap
|
||||||
|
|
||||||
|
| Tham số | Đơn vị | Mặc định package | Ý nghĩa và cách chỉnh |
|
||||||
|
| --- | ---: | ---: | --- |
|
||||||
|
| `global_frame` | frame | `map` | Global costmap thường dùng `map`; local costmap thường dùng `odom`. |
|
||||||
|
| `robot_base_frame` | frame | `base_link` | Phải có TF ổn định từ frame này tới `global_frame`. |
|
||||||
|
| `rolling_window` | bool | `false` | Bật cho local costmap để cửa sổ đi theo robot. |
|
||||||
|
| `track_unknown_space` | bool | `false` | Bật khi planner cần phân biệt vùng chưa biết với vùng trống. |
|
||||||
|
| `width`, `height` | m | `0.0` | Kích thước cửa sổ. Tăng để nhìn xa hơn nhưng tăng CPU/RAM theo diện tích. |
|
||||||
|
| `resolution` | m/cell | `0.0` | Giảm để chi tiết hơn nhưng số cell tăng theo nghịch đảo bình phương. Giá trị chạy T800 thường là `0.05`. |
|
||||||
|
| `origin_x`, `origin_y` | m | `0.0` | Góc dưới trái của costmap không rolling. Với static map thường lấy từ map. |
|
||||||
|
| `update_frequency` | Hz | `1.0` | Tốc độ tính costmap. Local cần nhanh hơn global nhưng không nên cao hơn khả năng cấp dữ liệu/CPU. |
|
||||||
|
| `transform_tolerance` | s | `0.0` | Dung sai TF. Chỉ tăng vừa đủ cho jitter; không dùng để che lỗi timestamp hoặc TF bị mất. |
|
||||||
|
| `footprint_padding` | m | `0.0` | Biên an toàn cộng đều quanh footprint. |
|
||||||
|
| `robot_radius` | m | `0.0` | Dùng cho robot tròn. Với T800 dạng chữ nhật nên khai báo polygon `footprint`. |
|
||||||
|
|
||||||
|
Số cell 2D xấp xỉ:
|
||||||
|
|
||||||
|
```text
|
||||||
|
(width / resolution) * (height / resolution)
|
||||||
|
```
|
||||||
|
|
||||||
|
Ví dụ cửa sổ `8 m x 8 m`, resolution `0.05 m` có `160 x 160 = 25,600`
|
||||||
|
cell. Nếu dùng `16` lớp voxel thì phần voxel có khoảng `409,600` ô.
|
||||||
|
|
||||||
|
### 5.2. Footprint
|
||||||
|
|
||||||
|
`footprint` là polygon theo mét trong `robot_base_frame`. Đây là tham số an toàn,
|
||||||
|
phải đo theo kích thước ngoài cùng thực tế của robot và tải hàng, không đo theo
|
||||||
|
khung chassis bên trong.
|
||||||
|
|
||||||
|
Quy trình chỉnh:
|
||||||
|
|
||||||
|
1. Đo khoảng cách từ tâm `robot_base_frame` tới mép trước, sau, trái, phải.
|
||||||
|
2. Khai báo các đỉnh theo thứ tự quanh polygon, không tự cắt nhau.
|
||||||
|
3. Kiểm tra pose quay tại chỗ gần tường và góc kệ.
|
||||||
|
4. Chỉ dùng `footprint_padding` cho sai số nhỏ; không dùng padding để bù một
|
||||||
|
footprint sai lớn.
|
||||||
|
|
||||||
|
### 5.3. StaticLayer
|
||||||
|
|
||||||
|
| Tham số | Mặc định | Ý nghĩa và cách chỉnh |
|
||||||
|
| --- | ---: | --- |
|
||||||
|
| `enabled` | `true` | Bật/tắt layer. |
|
||||||
|
| `map_topic` | `map` | Topic `OccupancyGrid`. |
|
||||||
|
| `first_map_only` | `false` | `true` nếu map không thay đổi và muốn bỏ các map gửi lại. |
|
||||||
|
| `subscribe_to_updates` | `false` | Bật nếu map server gửi `OccupancyGridUpdate`. |
|
||||||
|
| `track_unknown_space` | `true` | Giữ ô `unknown_cost_value` là `NO_INFORMATION`; tắt để coi unknown là free. |
|
||||||
|
| `use_maximum` | `false` | `false`: overwrite master; `true`: lấy max để không làm mất cost đã có. |
|
||||||
|
| `lethal_cost_threshold` | `100` | Occupancy value từ ngưỡng này trở lên được coi là lethal; code clamp trong `[0, 100]`. |
|
||||||
|
| `unknown_cost_value` | `-1` | Giá trị unknown trong map đầu vào. |
|
||||||
|
| `trinary_costmap` | `true` | Chỉ phân loại free/lethal/unknown; tắt để scale dải occupancy thành cost. |
|
||||||
|
| `base_frame_id` | `map` | Frame dùng bởi layer map trong implementation T800. |
|
||||||
|
|
||||||
|
### 5.4. Observation source của ObstacleLayer/VoxelLayer
|
||||||
|
|
||||||
|
`observation_sources` là chuỗi các tên source cách nhau bằng khoảng trắng, ví
|
||||||
|
dụ `b_scan pc_marking pc_clearing`. Mỗi tên phải có một map tham số cùng tên.
|
||||||
|
|
||||||
|
| Tham số | Mặc định code | Ý nghĩa và cách chỉnh |
|
||||||
|
| --- | ---: | --- |
|
||||||
|
| `topic` | `map` | Topic input. Đây cũng là key dispatch callback, phải khớp tuyệt đối. |
|
||||||
|
| `data_type` | `PointCloud` | Một trong `LaserScan`, `PointCloud`, `PointCloud2`, `DepthCameraData`. |
|
||||||
|
| `sensor_frame` | rỗng | Để rỗng để dùng `header.frame_id`; chỉ đặt khi cần ép origin của sensor. |
|
||||||
|
| `marking` | `true` | Đánh dấu điểm quan sát thành vật cản. |
|
||||||
|
| `clearing` | `false` | Raytrace PointCloud/LaserScan để xóa vùng trống. Không điều khiển depth-frustum clearing. |
|
||||||
|
| `inf_is_valid` | `false` | Với `LaserScan`, coi `+Inf` là tia không gặp vật cản để clearing. Không áp dụng cho point cloud. |
|
||||||
|
| `observation_persistence` | `0.0 s` | `0`: chỉ giữ mẫu mới nhất; tăng khi sensor thưa nhưng có thể tạo ghost obstacle. |
|
||||||
|
| `expected_update_rate` | `0.0 s` | Khoảng thời gian cập nhật mong đợi; `0`: không kiểm tra stale. Trong code đây là duration, không phải Hz. |
|
||||||
|
| `min_obstacle_height` | `0.0 m` | Bỏ điểm thấp hơn ngưỡng, hữu ích để lọc sàn. |
|
||||||
|
| `max_obstacle_height` | `2.0 m` | Bỏ điểm cao hơn ngưỡng. Phải phù hợp chiều cao robot/kệ và dải z của voxel. |
|
||||||
|
| `obstacle_range` | `2.5 m` | Khoảng cách tối đa dùng để marking. |
|
||||||
|
| `raytrace_range` | `3.0 m` | Khoảng cách tối đa dùng để clearing. Thường đặt lớn hơn `obstacle_range`. |
|
||||||
|
|
||||||
|
Lưu ý `expected_update_rate` được truyền vào `robot::Duration`. Ví dụ `0.5`
|
||||||
|
nghĩa là kỳ vọng có dữ liệu ít nhất mỗi `0.5 s`, tương đương tối thiểu `2 Hz`.
|
||||||
|
|
||||||
|
### 5.5. VoxelLayer
|
||||||
|
|
||||||
|
| Tham số | Mặc định YAML | Ý nghĩa và cách chỉnh |
|
||||||
|
| --- | ---: | --- |
|
||||||
|
| `enabled` | `true` | Bật/tắt layer. |
|
||||||
|
| `origin_z` | `0.0 m` | Đáy của voxel grid trong hệ tọa độ costmap. |
|
||||||
|
| `z_resolution` | `0.2 m` | Chiều cao mỗi voxel. Giảm để phân giải z tốt hơn nhưng dễ nhiễu và tốn xử lý hơn. |
|
||||||
|
| `z_voxels` | `16` | Số lớp z; implementation dùng tối đa 16 bit cho mỗi cột, nên giữ trong `1..16`. |
|
||||||
|
| `max_obstacle_height` | `3.0 m` | Trần điểm hợp lệ của layer. |
|
||||||
|
| `unknown_threshold` | `15` | Số voxel unknown cần để cột 2D còn unknown; cần chỉnh cùng `z_voxels`. |
|
||||||
|
| `mark_threshold` | `0` | Số voxel marked cần để cột 2D thành vật cản. Tăng nếu một điểm nhiễu đơn lẻ thường tạo vật cản giả. |
|
||||||
|
| `combination_method` | `1` | `0`: overwrite master, `1`: lấy maximum. Thường dùng `1` để không xóa cost layer trước. |
|
||||||
|
| `footprint_clearing_enabled` | `true` | Xóa vật cản nằm trong footprint hiện tại của robot. |
|
||||||
|
|
||||||
|
Dải z của voxel xấp xỉ:
|
||||||
|
|
||||||
|
```text
|
||||||
|
[origin_z, origin_z + z_resolution * z_voxels)
|
||||||
|
```
|
||||||
|
|
||||||
|
Dải này phải bao phủ vùng `min_obstacle_height..max_obstacle_height` mà robot
|
||||||
|
cần quan sát. Không tăng `max_obstacle_height` vượt khỏi voxel grid mà không
|
||||||
|
đồng thời kiểm tra `origin_z`, `z_resolution` và `z_voxels`.
|
||||||
|
|
||||||
|
### 5.6. Depth frustum clearing
|
||||||
|
|
||||||
|
| Tham số | Mặc định code | Ý nghĩa và cách chỉnh |
|
||||||
|
| --- | ---: | --- |
|
||||||
|
| `frustum_clearing_enabled` | `false` | Chọn đường clearing trực tiếp từ `DepthCameraData`. |
|
||||||
|
| `frustum_clearing_pixel_step` | `8 px` | Lấy một tia mỗi N pixel theo cả hai chiều. Tăng để giảm CPU, giảm để clear dày hơn. Code clamp tối thiểu là `1`. |
|
||||||
|
| `frustum_min_range` | `0.20 m` | Không clear vùng quá gần camera, nơi depth thường không đáng tin. |
|
||||||
|
| `frustum_max_range` | `3.0 m` | Chiều dài ray tối đa khi pixel không có depth hợp lệ hoặc depth ở xa. |
|
||||||
|
|
||||||
|
Mỗi pixel được sample tạo một tia từ camera. Với depth hợp lệ, ray dừng trước
|
||||||
|
điểm đo khoảng `2 * resolution` để không xóa chính vật cản. Với pixel không hợp
|
||||||
|
lệ, ray có thể clear tới `frustum_max_range`; vì vậy không đặt range vượt vùng
|
||||||
|
camera thực sự đáng tin.
|
||||||
|
|
||||||
|
Gợi ý tuning:
|
||||||
|
|
||||||
|
- Bắt đầu với `pixel_step: 8`.
|
||||||
|
- Nếu còn các dải ghost obstacle mỏng giữa các tia, thử `6`, rồi `4`.
|
||||||
|
- Nếu CPU cao, thử `10`, `12` hoặc giảm `frustum_max_range`.
|
||||||
|
- `frustum_min_range` nên lớn hơn hoặc bằng khoảng mù gần của camera.
|
||||||
|
- `frustum_max_range` nên nhỉnh hơn `pc_marking.obstacle_range`, nhưng không
|
||||||
|
vượt quá range depth ổn định trong môi trường thực tế.
|
||||||
|
|
||||||
|
### 5.7. InflationLayer
|
||||||
|
|
||||||
|
| Tham số | Mặc định | Ý nghĩa và cách chỉnh |
|
||||||
|
| --- | ---: | --- |
|
||||||
|
| `enabled` | `true` | Bật/tắt inflation. |
|
||||||
|
| `inflation_radius` | `0.55 m` | Bán kính tối đa có cost quanh vật cản. Tăng để robot tránh xa hơn. |
|
||||||
|
| `cost_scaling_factor` | `15.0` | Hệ số suy giảm mũ. **Tăng** giá trị làm cost giảm nhanh hơn và vùng cost mạnh hẹp hơn; **giảm** giá trị làm robot giữ khoảng cách mềm xa hơn. |
|
||||||
|
| `inflate_unknown` | `false` | Có inflation vùng unknown hay không. Bật có thể làm planner thận trọng hơn nhưng dễ chặn đường trong map chưa hoàn chỉnh. |
|
||||||
|
|
||||||
|
`inflation_radius` phải được chọn sau khi footprint đúng. Bán kính này nên lớn
|
||||||
|
hơn inscribed radius cộng biên an toàn mong muốn; tăng radius không thể sửa một
|
||||||
|
footprint sai.
|
||||||
|
|
||||||
|
### 5.8. Performance metrics
|
||||||
|
|
||||||
|
| Tham số | Mặc định | Ý nghĩa |
|
||||||
|
| --- | ---: | --- |
|
||||||
|
| `performance_metrics_enabled` | `false` | Log thời gian `updateBounds` và `updateCosts` theo từng layer. |
|
||||||
|
| `performance_metrics_period` | `5.0 s` | Chu kỳ tổng hợp và in metrics. |
|
||||||
|
|
||||||
|
Bật metrics trong lúc tuning CPU, sau đó có thể tắt để giảm log runtime.
|
||||||
|
|
||||||
|
## 6. Quy trình tuning khuyến nghị
|
||||||
|
|
||||||
|
Chỉ thay một nhóm tham số mỗi lần và lưu lại bag/log trước khi chỉnh.
|
||||||
|
|
||||||
|
1. **Kiểm tra TF và timestamp**: phải có transform liên tục từ từng sensor frame
|
||||||
|
tới `map`/`odom`. Không tuning costmap khi TF còn lỗi.
|
||||||
|
2. **Chốt footprint**: đo robot và tải hàng thật, kiểm tra quay tại chỗ.
|
||||||
|
3. **Chọn resolution và kích thước cửa sổ**: bắt đầu `0.05 m`; local thường
|
||||||
|
`6..10 m` tùy vận tốc và khoảng phanh.
|
||||||
|
4. **Chỉ bật marking**: xác nhận vật cản xuất hiện đúng vị trí, đúng height và
|
||||||
|
range.
|
||||||
|
5. **Bật clearing**: laser/point cloud dùng raytrace; depth camera dùng source
|
||||||
|
`DepthCameraData` với frustum clearing.
|
||||||
|
6. **Chỉnh voxel**: đặt dải z, sau đó tăng `mark_threshold` nếu nhiễu đơn điểm.
|
||||||
|
7. **Chỉnh inflation**: chỉnh `inflation_radius` trước, sau đó mới chỉnh
|
||||||
|
`cost_scaling_factor` theo khoảng cách đường đi mong muốn.
|
||||||
|
8. **Đo tải CPU**: bật performance metrics; chỉ tăng frequency hoặc giảm
|
||||||
|
resolution khi chu kỳ cập nhật vẫn hoàn thành ổn định.
|
||||||
|
|
||||||
|
Các ràng buộc nên giữ:
|
||||||
|
|
||||||
|
```text
|
||||||
|
resolution > 0
|
||||||
|
width > 0 và height > 0 nếu không lấy size từ static map
|
||||||
|
raytrace_range >= obstacle_range
|
||||||
|
frustum_max_range >= frustum_min_range >= 0
|
||||||
|
1 <= z_voxels <= 16
|
||||||
|
origin_z + z_resolution * z_voxels đủ bao phủ dải vật cản cần quan sát
|
||||||
|
InflationLayer nằm sau các layer tạo vật cản
|
||||||
|
```
|
||||||
|
|
||||||
|
## 7. Tuning theo triệu chứng
|
||||||
|
|
||||||
|
| Triệu chứng | Kiểm tra trước | Hướng chỉnh |
|
||||||
|
| --- | --- | --- |
|
||||||
|
| Vật cản đã đi nhưng vẫn còn trên costmap | TF, topic clearing, dữ liệu có còn cập nhật | Bật đúng `clearing`; với depth dùng `DepthCameraData` + `frustum_clearing_enabled`; giảm `observation_persistence`; giảm `pixel_step` nếu còn khe giữa tia. |
|
||||||
|
| Vật cản thật không được đánh dấu | Topic/type/frame, range và height | Kiểm tra `marking`, `min/max_obstacle_height`, `obstacle_range`; với voxel thử `mark_threshold: 0` trước. |
|
||||||
|
| Vật cản chớp tắt | Tần số sensor, packet drop, TF | Đặt `expected_update_rate` đúng chu kỳ; tăng nhẹ `observation_persistence` nhưng phải kiểm tra ghost obstacle. |
|
||||||
|
| Robot đi quá sát vật cản | Footprint trước, inflation sau | Tăng `inflation_radius` hoặc giảm `cost_scaling_factor`. |
|
||||||
|
| Robot tránh quá xa/không tìm được đường | Footprint, unknown space, inflation | Giảm `inflation_radius` hoặc tăng `cost_scaling_factor`; kiểm tra `inflate_unknown`. |
|
||||||
|
| Costmap local trễ hoặc CPU cao | Metrics theo layer | Tăng `resolution`, giảm `width/height`, giảm `update_frequency`, tăng depth `pixel_step`, giảm range hoặc giảm mật độ `/camera/depth/points_proc`. |
|
||||||
|
| Costmap báo stale/not current | Sensor thực tế có đúng chu kỳ không | Tăng giá trị `expected_update_rate` theo đơn vị giây hoặc đặt `0` để tắt kiểm tra trong lúc chẩn đoán. |
|
||||||
|
| Clearing xóa xuyên vật cản depth | Depth invalid, range quá lớn, TF camera | Giảm `frustum_max_range`, tăng `frustum_min_range`, kiểm tra encoding/calibration và đảm bảo point-cloud marking hoạt động. |
|
||||||
|
| Sensor origin nằm ngoài voxel map | `global_frame`, rolling window, TF z | Sửa TF/origin, tăng cửa sổ phù hợp; không chỉ tăng tolerance. |
|
||||||
|
| Vật cản sàn/nhiễu thấp xuất hiện | Height filter và calibration | Tăng `min_obstacle_height` từng bước nhỏ; không tăng quá đáy vật cản robot cần tránh. |
|
||||||
|
|
||||||
|
## 8. Lưu ý riêng cho depth camera T800
|
||||||
|
|
||||||
|
- Marking và clearing có contract khác nhau: `/camera/depth/points_proc`
|
||||||
|
(`PointCloud2`) dùng cho marking/persistence; `/camera/depth/data`
|
||||||
|
(`DepthCameraData`) dùng cho frustum clearing.
|
||||||
|
- Không cấu hình cùng một full point cloud để vừa marking vừa clearing nếu mục
|
||||||
|
tiêu là giảm tải. Đường frustum dùng raw depth semantics và sampling theo
|
||||||
|
pixel, tránh xử lý toàn bộ cloud thêm lần nữa.
|
||||||
|
- Mỗi camera nên có cặp source riêng, ví dụ `pc0_marking pc0_clearing
|
||||||
|
pc1_marking pc1_clearing`, với topic, frame, range và pixel step riêng.
|
||||||
|
- `observation_persistence: 0.0` giữ mẫu mới nhất. Chỉ tăng khi đã đo được tần
|
||||||
|
suất sensor và hiểu rõ thời gian ghost obstacle chấp nhận được.
|
||||||
|
- Khi mất marking, kiểm tra lần lượt output sau bước chuyển depth thành
|
||||||
|
`/camera/depth/points_proc`, TF sang costmap frame và buffer của
|
||||||
|
`ObstacleLayer` trước khi chỉnh `mark_threshold`.
|
||||||
|
|
||||||
|
## 9. Build và kiểm tra
|
||||||
|
|
||||||
|
Build package trong workspace:
|
||||||
|
|
||||||
|
```bash
|
||||||
|
cd /home/duongtd/T800_ws
|
||||||
|
catkin_make --pkg robot_costmap_2d
|
||||||
|
```
|
||||||
|
|
||||||
|
Các executable test được tạo khi `BUILD_COSTMAP_TESTS=ON`:
|
||||||
|
|
||||||
|
```bash
|
||||||
|
./devel/lib/robot_costmap_2d/test_array_parser
|
||||||
|
./devel/lib/robot_costmap_2d/test_costmap
|
||||||
|
./devel/lib/robot_costmap_2d/test_plugin
|
||||||
|
```
|
||||||
|
|
||||||
|
Kiểm tra tối thiểu trước khi chạy robot:
|
||||||
|
|
||||||
|
- YAML parse được và đúng namespace global/local.
|
||||||
|
- `library_path` tìm thấy `libplugins`.
|
||||||
|
- Plugin được tạo đúng tên và đúng thứ tự.
|
||||||
|
- TF giữa sensor, `robot_base_frame` và `global_frame` sẵn sàng.
|
||||||
|
- Sensor topic, `data_type` và `header.frame_id` khớp cấu hình.
|
||||||
|
- Costmap update ổn định, không stale và không vượt ngân sách chu kỳ.
|
||||||
|
- Footprint và inflation đã được kiểm tra ở tốc độ thấp trước.
|
||||||
|
|
||||||
|
## 10. Cấu trúc package
|
||||||
|
|
||||||
|
```text
|
||||||
|
costmap_2d/
|
||||||
|
├── config/ # Fallback YAML của package
|
||||||
|
├── include/robot_costmap_2d/ # Public headers
|
||||||
|
├── plugins/ # Layer implementations
|
||||||
|
├── src/ # Costmap core và observation buffer
|
||||||
|
├── test/ # Unit/integration tests
|
||||||
|
├── CMakeLists.txt
|
||||||
|
└── package.xml
|
||||||
|
```
|
||||||
@@ -25,6 +25,8 @@ robot_costmap_2d:
|
|||||||
- [-0.3, 0.3]
|
- [-0.3, 0.3]
|
||||||
|
|
||||||
transform_tolerance: 0.0
|
transform_tolerance: 0.0
|
||||||
|
performance_metrics_enabled: false
|
||||||
|
performance_metrics_period: 5.0
|
||||||
update_frequency: 1.0
|
update_frequency: 1.0
|
||||||
width: 0.0
|
width: 0.0
|
||||||
height: 0.0
|
height: 0.0
|
||||||
@@ -33,4 +35,4 @@ robot_costmap_2d:
|
|||||||
origin_y: 0.0
|
origin_y: 0.0
|
||||||
|
|
||||||
footprint_padding: 0.0
|
footprint_padding: 0.0
|
||||||
robot_radius: 0.0
|
robot_radius: 0.0
|
||||||
|
|||||||
@@ -8,3 +8,8 @@ voxel_layer:
|
|||||||
unknown_threshold: 15.0
|
unknown_threshold: 15.0
|
||||||
mark_threshold: 0
|
mark_threshold: 0
|
||||||
combination_method: 1
|
combination_method: 1
|
||||||
|
frustum_clearing_enabled: true
|
||||||
|
frustum_clearing_pixel_step: 8
|
||||||
|
frustum_min_range: 0.20
|
||||||
|
frustum_max_range: 3.0
|
||||||
|
frustum_depth_camera_topic: /camera/depth/data
|
||||||
|
|||||||
@@ -425,6 +425,7 @@ protected:
|
|||||||
double origin_y_;
|
double origin_y_;
|
||||||
unsigned char* costmap_;
|
unsigned char* costmap_;
|
||||||
unsigned char default_value_;
|
unsigned char default_value_;
|
||||||
|
std::vector<unsigned char> rolling_window_scratch_;
|
||||||
|
|
||||||
class MarkCell
|
class MarkCell
|
||||||
{
|
{
|
||||||
|
|||||||
@@ -42,6 +42,9 @@
|
|||||||
#include <robot_costmap_2d/layered_costmap.h>
|
#include <robot_costmap_2d/layered_costmap.h>
|
||||||
#include <boost/thread.hpp>
|
#include <boost/thread.hpp>
|
||||||
|
|
||||||
|
#include <cstdint>
|
||||||
|
#include <vector>
|
||||||
|
|
||||||
namespace robot_costmap_2d
|
namespace robot_costmap_2d
|
||||||
{
|
{
|
||||||
/**
|
/**
|
||||||
@@ -77,8 +80,7 @@ public:
|
|||||||
virtual ~InflationLayer()
|
virtual ~InflationLayer()
|
||||||
{
|
{
|
||||||
deleteKernels();
|
deleteKernels();
|
||||||
if (seen_)
|
delete inflation_access_;
|
||||||
delete[] seen_;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
virtual void onInitialize();
|
virtual void onInitialize();
|
||||||
@@ -184,10 +186,13 @@ private:
|
|||||||
|
|
||||||
unsigned int cell_inflation_radius_;
|
unsigned int cell_inflation_radius_;
|
||||||
unsigned int cached_cell_inflation_radius_;
|
unsigned int cached_cell_inflation_radius_;
|
||||||
std::map<double, std::vector<CellData> > inflation_cells_;
|
std::vector<std::vector<CellData>> inflation_cells_;
|
||||||
|
std::vector<double> distance_levels_;
|
||||||
|
std::vector<unsigned int> distance_bin_lookup_;
|
||||||
|
unsigned int distance_lookup_size_ = 0;
|
||||||
|
|
||||||
bool* seen_;
|
std::vector<std::uint32_t> seen_;
|
||||||
int seen_size_;
|
std::uint32_t seen_generation_ = 0;
|
||||||
|
|
||||||
unsigned char** cached_costs_;
|
unsigned char** cached_costs_;
|
||||||
double** cached_distances_;
|
double** cached_distances_;
|
||||||
|
|||||||
@@ -43,6 +43,8 @@
|
|||||||
#include <robot_costmap_2d/costmap_2d.h>
|
#include <robot_costmap_2d/costmap_2d.h>
|
||||||
#include <vector>
|
#include <vector>
|
||||||
#include <string>
|
#include <string>
|
||||||
|
#include <chrono>
|
||||||
|
#include <cstdint>
|
||||||
|
|
||||||
namespace robot_costmap_2d
|
namespace robot_costmap_2d
|
||||||
{
|
{
|
||||||
@@ -71,6 +73,8 @@ public:
|
|||||||
*/
|
*/
|
||||||
void updateMap(double robot_x, double robot_y, double robot_yaw);
|
void updateMap(double robot_x, double robot_y, double robot_yaw);
|
||||||
|
|
||||||
|
void setPerformanceMetrics(bool enabled, double reporting_period_seconds);
|
||||||
|
|
||||||
inline const std::string& getGlobalFrameID() const noexcept
|
inline const std::string& getGlobalFrameID() const noexcept
|
||||||
{
|
{
|
||||||
return global_frame_;
|
return global_frame_;
|
||||||
@@ -155,6 +159,17 @@ public:
|
|||||||
double getInscribedRadius() { return inscribed_radius_; }
|
double getInscribedRadius() { return inscribed_radius_; }
|
||||||
|
|
||||||
private:
|
private:
|
||||||
|
struct LayerPerformance
|
||||||
|
{
|
||||||
|
std::uint64_t bounds_nanoseconds = 0;
|
||||||
|
std::uint64_t costs_nanoseconds = 0;
|
||||||
|
std::uint64_t bounds_calls = 0;
|
||||||
|
std::uint64_t costs_calls = 0;
|
||||||
|
};
|
||||||
|
|
||||||
|
void resetPerformanceMetrics();
|
||||||
|
void maybeReportPerformance();
|
||||||
|
|
||||||
Costmap2D costmap_;
|
Costmap2D costmap_;
|
||||||
std::string global_frame_;
|
std::string global_frame_;
|
||||||
|
|
||||||
@@ -170,6 +185,15 @@ private:
|
|||||||
bool size_locked_;
|
bool size_locked_;
|
||||||
double circumscribed_radius_, inscribed_radius_;
|
double circumscribed_radius_, inscribed_radius_;
|
||||||
std::vector<robot_geometry_msgs::Point> footprint_;
|
std::vector<robot_geometry_msgs::Point> footprint_;
|
||||||
|
|
||||||
|
bool performance_metrics_enabled_ = false;
|
||||||
|
double performance_metrics_period_seconds_ = 5.0;
|
||||||
|
std::chrono::steady_clock::time_point performance_window_start_;
|
||||||
|
std::uint64_t performance_cycle_nanoseconds_ = 0;
|
||||||
|
std::uint64_t performance_reset_nanoseconds_ = 0;
|
||||||
|
std::uint64_t performance_cycles_ = 0;
|
||||||
|
std::vector<std::uint64_t> performance_cycle_samples_;
|
||||||
|
std::vector<LayerPerformance> layer_performance_;
|
||||||
};
|
};
|
||||||
|
|
||||||
} // namespace robot_costmap_2d
|
} // namespace robot_costmap_2d
|
||||||
|
|||||||
@@ -34,10 +34,140 @@
|
|||||||
|
|
||||||
#include <robot_geometry_msgs/Point.h>
|
#include <robot_geometry_msgs/Point.h>
|
||||||
#include <robot_sensor_msgs/PointCloud2.h>
|
#include <robot_sensor_msgs/PointCloud2.h>
|
||||||
|
#include <robot_sensor_msgs/DepthCameraData.h>
|
||||||
|
#include <boost/make_shared.hpp>
|
||||||
|
#include <boost/shared_ptr.hpp>
|
||||||
|
#include <utility>
|
||||||
|
|
||||||
namespace robot_costmap_2d
|
namespace robot_costmap_2d
|
||||||
{
|
{
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief A depth frame and its per-source frustum-clearing configuration.
|
||||||
|
*
|
||||||
|
* The message is shared so returning buffered observations does not copy the
|
||||||
|
* full depth image on every costmap update.
|
||||||
|
*/
|
||||||
|
class DepthCameraObservation
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
DepthCameraObservation()
|
||||||
|
: data_handle_(),
|
||||||
|
data_(nullptr),
|
||||||
|
topic_(),
|
||||||
|
pixel_step_(0),
|
||||||
|
min_range_(0.0),
|
||||||
|
max_range_(0.0)
|
||||||
|
{
|
||||||
|
}
|
||||||
|
|
||||||
|
DepthCameraObservation(
|
||||||
|
const robot_sensor_msgs::DepthCameraData& data,
|
||||||
|
std::string topic,
|
||||||
|
const robot::Time& received_time,
|
||||||
|
unsigned int pixel_step,
|
||||||
|
double min_range,
|
||||||
|
double max_range)
|
||||||
|
: data_handle_(boost::make_shared<robot_sensor_msgs::DepthCameraData>(data)),
|
||||||
|
data_(data_handle_.get()),
|
||||||
|
topic_(std::move(topic)),
|
||||||
|
received_time_(received_time),
|
||||||
|
pixel_step_(pixel_step),
|
||||||
|
min_range_(min_range),
|
||||||
|
max_range_(max_range)
|
||||||
|
{
|
||||||
|
}
|
||||||
|
|
||||||
|
DepthCameraObservation(
|
||||||
|
robot_sensor_msgs::DepthCameraData::ConstPtr data,
|
||||||
|
std::string topic,
|
||||||
|
const robot::Time& received_time,
|
||||||
|
unsigned int pixel_step,
|
||||||
|
double min_range,
|
||||||
|
double max_range)
|
||||||
|
: data_handle_(std::move(data)),
|
||||||
|
data_(data_handle_.get()),
|
||||||
|
topic_(std::move(topic)),
|
||||||
|
received_time_(received_time),
|
||||||
|
pixel_step_(pixel_step),
|
||||||
|
min_range_(min_range),
|
||||||
|
max_range_(max_range)
|
||||||
|
{
|
||||||
|
}
|
||||||
|
|
||||||
|
DepthCameraObservation(const DepthCameraObservation& other)
|
||||||
|
: data_handle_(other.data_handle_),
|
||||||
|
data_(data_handle_.get()),
|
||||||
|
topic_(other.topic_),
|
||||||
|
received_time_(other.received_time_),
|
||||||
|
pixel_step_(other.pixel_step_),
|
||||||
|
min_range_(other.min_range_),
|
||||||
|
max_range_(other.max_range_)
|
||||||
|
{
|
||||||
|
}
|
||||||
|
|
||||||
|
DepthCameraObservation(DepthCameraObservation&& other) noexcept
|
||||||
|
: data_handle_(std::move(other.data_handle_)),
|
||||||
|
data_(data_handle_.get()),
|
||||||
|
topic_(std::move(other.topic_)),
|
||||||
|
received_time_(other.received_time_),
|
||||||
|
pixel_step_(other.pixel_step_),
|
||||||
|
min_range_(other.min_range_),
|
||||||
|
max_range_(other.max_range_)
|
||||||
|
{
|
||||||
|
other.data_ = nullptr;
|
||||||
|
other.pixel_step_ = 0;
|
||||||
|
other.min_range_ = 0.0;
|
||||||
|
other.max_range_ = 0.0;
|
||||||
|
}
|
||||||
|
|
||||||
|
DepthCameraObservation& operator=(const DepthCameraObservation& other)
|
||||||
|
{
|
||||||
|
if (this == &other)
|
||||||
|
return *this;
|
||||||
|
|
||||||
|
data_handle_ = other.data_handle_;
|
||||||
|
data_ = data_handle_.get();
|
||||||
|
topic_ = other.topic_;
|
||||||
|
received_time_ = other.received_time_;
|
||||||
|
pixel_step_ = other.pixel_step_;
|
||||||
|
min_range_ = other.min_range_;
|
||||||
|
max_range_ = other.max_range_;
|
||||||
|
return *this;
|
||||||
|
}
|
||||||
|
|
||||||
|
DepthCameraObservation& operator=(DepthCameraObservation&& other) noexcept
|
||||||
|
{
|
||||||
|
if (this == &other)
|
||||||
|
return *this;
|
||||||
|
|
||||||
|
data_handle_ = std::move(other.data_handle_);
|
||||||
|
data_ = data_handle_.get();
|
||||||
|
topic_ = std::move(other.topic_);
|
||||||
|
received_time_ = other.received_time_;
|
||||||
|
pixel_step_ = other.pixel_step_;
|
||||||
|
min_range_ = other.min_range_;
|
||||||
|
max_range_ = other.max_range_;
|
||||||
|
|
||||||
|
other.data_ = nullptr;
|
||||||
|
other.pixel_step_ = 0;
|
||||||
|
other.min_range_ = 0.0;
|
||||||
|
other.max_range_ = 0.0;
|
||||||
|
|
||||||
|
return *this;
|
||||||
|
}
|
||||||
|
|
||||||
|
~DepthCameraObservation() = default;
|
||||||
|
|
||||||
|
robot_sensor_msgs::DepthCameraData::ConstPtr data_handle_;
|
||||||
|
const robot_sensor_msgs::DepthCameraData* data_;
|
||||||
|
std::string topic_;
|
||||||
|
robot::Time received_time_;
|
||||||
|
unsigned int pixel_step_;
|
||||||
|
double min_range_;
|
||||||
|
double max_range_;
|
||||||
|
};
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* @brief Stores an observation in terms of a point cloud and the origin of the source
|
* @brief Stores an observation in terms of a point cloud and the origin of the source
|
||||||
* @note Tried to make members and constructor arguments const but the compiler would not accept the default
|
* @note Tried to make members and constructor arguments const but the compiler would not accept the default
|
||||||
@@ -50,14 +180,12 @@ public:
|
|||||||
* @brief Creates an empty observation
|
* @brief Creates an empty observation
|
||||||
*/
|
*/
|
||||||
Observation() :
|
Observation() :
|
||||||
cloud_(new robot_sensor_msgs::PointCloud2()), obstacle_range_(0.0), raytrace_range_(0.0)
|
cloud_handle_(boost::make_shared<robot_sensor_msgs::PointCloud2>()),
|
||||||
|
cloud_(cloud_handle_.get()), obstacle_range_(0.0), raytrace_range_(0.0)
|
||||||
{
|
{
|
||||||
}
|
}
|
||||||
|
|
||||||
virtual ~Observation()
|
virtual ~Observation() = default;
|
||||||
{
|
|
||||||
delete cloud_;
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* @brief Creates an observation from an origin point and a point cloud
|
* @brief Creates an observation from an origin point and a point cloud
|
||||||
@@ -68,7 +196,17 @@ public:
|
|||||||
*/
|
*/
|
||||||
Observation(robot_geometry_msgs::Point& origin, const robot_sensor_msgs::PointCloud2 &cloud,
|
Observation(robot_geometry_msgs::Point& origin, const robot_sensor_msgs::PointCloud2 &cloud,
|
||||||
double obstacle_range, double raytrace_range) :
|
double obstacle_range, double raytrace_range) :
|
||||||
origin_(origin), cloud_(new robot_sensor_msgs::PointCloud2(cloud)),
|
origin_(origin), cloud_handle_(boost::make_shared<robot_sensor_msgs::PointCloud2>(cloud)),
|
||||||
|
cloud_(cloud_handle_.get()),
|
||||||
|
obstacle_range_(obstacle_range), raytrace_range_(raytrace_range)
|
||||||
|
{
|
||||||
|
}
|
||||||
|
|
||||||
|
Observation(robot_geometry_msgs::Point origin,
|
||||||
|
boost::shared_ptr<robot_sensor_msgs::PointCloud2> cloud,
|
||||||
|
double obstacle_range, double raytrace_range) :
|
||||||
|
origin_(std::move(origin)), cloud_handle_(std::move(cloud)),
|
||||||
|
cloud_(cloud_handle_.get()),
|
||||||
obstacle_range_(obstacle_range), raytrace_range_(raytrace_range)
|
obstacle_range_(obstacle_range), raytrace_range_(raytrace_range)
|
||||||
{
|
{
|
||||||
}
|
}
|
||||||
@@ -78,22 +216,59 @@ public:
|
|||||||
* @param obs The observation to copy
|
* @param obs The observation to copy
|
||||||
*/
|
*/
|
||||||
Observation(const Observation& obs) :
|
Observation(const Observation& obs) :
|
||||||
origin_(obs.origin_), cloud_(new robot_sensor_msgs::PointCloud2(*(obs.cloud_))),
|
origin_(obs.origin_), cloud_handle_(obs.cloud_handle_), cloud_(cloud_handle_.get()),
|
||||||
obstacle_range_(obs.obstacle_range_), raytrace_range_(obs.raytrace_range_)
|
obstacle_range_(obs.obstacle_range_), raytrace_range_(obs.raytrace_range_)
|
||||||
{
|
{
|
||||||
}
|
}
|
||||||
|
|
||||||
|
Observation(Observation&& obs) noexcept :
|
||||||
|
origin_(std::move(obs.origin_)), cloud_handle_(std::move(obs.cloud_handle_)),
|
||||||
|
cloud_(cloud_handle_.get()), obstacle_range_(obs.obstacle_range_),
|
||||||
|
raytrace_range_(obs.raytrace_range_)
|
||||||
|
{
|
||||||
|
obs.cloud_ = nullptr;
|
||||||
|
}
|
||||||
|
|
||||||
|
Observation& operator=(const Observation& obs)
|
||||||
|
{
|
||||||
|
if (this == &obs)
|
||||||
|
return *this;
|
||||||
|
|
||||||
|
origin_ = obs.origin_;
|
||||||
|
cloud_handle_ = obs.cloud_handle_;
|
||||||
|
cloud_ = cloud_handle_.get();
|
||||||
|
obstacle_range_ = obs.obstacle_range_;
|
||||||
|
raytrace_range_ = obs.raytrace_range_;
|
||||||
|
return *this;
|
||||||
|
}
|
||||||
|
|
||||||
|
Observation& operator=(Observation&& obs) noexcept
|
||||||
|
{
|
||||||
|
if (this == &obs)
|
||||||
|
return *this;
|
||||||
|
|
||||||
|
origin_ = std::move(obs.origin_);
|
||||||
|
cloud_handle_ = std::move(obs.cloud_handle_);
|
||||||
|
cloud_ = cloud_handle_.get();
|
||||||
|
obstacle_range_ = obs.obstacle_range_;
|
||||||
|
raytrace_range_ = obs.raytrace_range_;
|
||||||
|
obs.cloud_ = nullptr;
|
||||||
|
return *this;
|
||||||
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* @brief Creates an observation from a point cloud
|
* @brief Creates an observation from a point cloud
|
||||||
* @param cloud The point cloud of the observation
|
* @param cloud The point cloud of the observation
|
||||||
* @param obstacle_range The range out to which an observation should be able to insert obstacles
|
* @param obstacle_range The range out to which an observation should be able to insert obstacles
|
||||||
*/
|
*/
|
||||||
Observation(const robot_sensor_msgs::PointCloud2 &cloud, double obstacle_range) :
|
Observation(const robot_sensor_msgs::PointCloud2 &cloud, double obstacle_range) :
|
||||||
cloud_(new robot_sensor_msgs::PointCloud2(cloud)), obstacle_range_(obstacle_range), raytrace_range_(0.0)
|
cloud_handle_(boost::make_shared<robot_sensor_msgs::PointCloud2>(cloud)),
|
||||||
|
cloud_(cloud_handle_.get()), obstacle_range_(obstacle_range), raytrace_range_(0.0)
|
||||||
{
|
{
|
||||||
}
|
}
|
||||||
|
|
||||||
robot_geometry_msgs::Point origin_;
|
robot_geometry_msgs::Point origin_;
|
||||||
|
boost::shared_ptr<robot_sensor_msgs::PointCloud2> cloud_handle_;
|
||||||
robot_sensor_msgs::PointCloud2* cloud_;
|
robot_sensor_msgs::PointCloud2* cloud_;
|
||||||
double obstacle_range_, raytrace_range_;
|
double obstacle_range_, raytrace_range_;
|
||||||
};
|
};
|
||||||
|
|||||||
@@ -1,391 +1,3 @@
|
|||||||
// // /*********************************************************************
|
|
||||||
// // *
|
|
||||||
// // * Software License Agreement (BSD License)
|
|
||||||
// // *
|
|
||||||
// // * Copyright (c) 2008, 2013, Willow Garage, Inc.
|
|
||||||
// // * All rights reserved.
|
|
||||||
// // *
|
|
||||||
// // * Redistribution and use in source and binary forms, with or without
|
|
||||||
// // * modification, are permitted provided that the following conditions
|
|
||||||
// // * are met:
|
|
||||||
// // *
|
|
||||||
// // * * Redistributions of source code must retain the above copyright
|
|
||||||
// // * notice, this list of conditions and the following disclaimer.
|
|
||||||
// // * * Redistributions in binary form must reproduce the above
|
|
||||||
// // * copyright notice, this list of conditions and the following
|
|
||||||
// // * disclaimer in the documentation and/or other materials provided
|
|
||||||
// // * with the distribution.
|
|
||||||
// // * * Neither the name of Willow Garage, Inc. nor the names of its
|
|
||||||
// // * contributors may be used to endorse or promote products derived
|
|
||||||
// // * from this software without specific prior written permission.
|
|
||||||
// // *
|
|
||||||
// // * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
|
||||||
// // * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
|
||||||
// // * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
|
||||||
// // * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
|
||||||
// // * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
|
||||||
// // * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
|
||||||
// // * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
|
||||||
// // * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
|
||||||
// // * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
|
||||||
// // * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
|
||||||
// // * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
|
||||||
// // * POSSIBILITY OF SUCH DAMAGE.
|
|
||||||
// // *
|
|
||||||
// // * Author: Eitan Marder-Eppstein
|
|
||||||
// // *********************************************************************/
|
|
||||||
// // #ifndef ROBOT_COSTMAP_2D_OBSERVATION_BUFFER_H_
|
|
||||||
// // #define ROBOT_COSTMAP_2D_OBSERVATION_BUFFER_H_
|
|
||||||
|
|
||||||
// // #include <vector>
|
|
||||||
// // #include <list>
|
|
||||||
// // #include <string>
|
|
||||||
// // #include <robot/robot.h>
|
|
||||||
// // #include <robot_costmap_2d/observation.h>
|
|
||||||
// // #include <tf3/buffer_core.h>
|
|
||||||
// // #include <robot_sensor_msgs/PointCloud2.h>
|
|
||||||
|
|
||||||
// // // Thread support
|
|
||||||
// // #include <boost/thread.hpp>
|
|
||||||
|
|
||||||
// // namespace robot_costmap_2d
|
|
||||||
// // {
|
|
||||||
// // /**
|
|
||||||
// // * @class ObservationBuffer
|
|
||||||
// // * @brief Takes in point clouds from sensors, transforms them to the desired frame, and stores them
|
|
||||||
// // */
|
|
||||||
// // class ObservationBuffer
|
|
||||||
// // {
|
|
||||||
// // public:
|
|
||||||
// // /**
|
|
||||||
// // * @brief Constructs an observation buffer
|
|
||||||
// // * @param topic_name The topic of the observations, used as an identifier for error and warning messages
|
|
||||||
// // * @param observation_keep_time Defines the persistence of observations in seconds, 0 means only keep the latest
|
|
||||||
// // * @param expected_update_rate How often this buffer is expected to be updated, 0 means there is no limit
|
|
||||||
// // * @param min_obstacle_height The minimum height of a hitpoint to be considered legal
|
|
||||||
// // * @param max_obstacle_height The minimum height of a hitpoint to be considered legal
|
|
||||||
// // * @param obstacle_range The range to which the sensor should be trusted for inserting obstacles
|
|
||||||
// // * @param raytrace_range The range to which the sensor should be trusted for raytracing to clear out space
|
|
||||||
// // * @param tf2_buffer A reference to a tf2 Buffer
|
|
||||||
// // * @param global_frame The frame to transform PointClouds into
|
|
||||||
// // * @param sensor_frame The frame of the origin of the sensor, can be left blank to be read from the messages
|
|
||||||
// // * @param tf_tolerance The amount of time to wait for a transform to be available when setting a new global frame
|
|
||||||
// // */
|
|
||||||
// // ObservationBuffer(std::string topic_name, double observation_keep_time, double expected_update_rate,
|
|
||||||
// // double min_obstacle_height, double max_obstacle_height, double obstacle_range,
|
|
||||||
// // double raytrace_range, tf3::BufferCore& tf3_buffer, std::string global_frame,
|
|
||||||
// // std::string sensor_frame, double tf_tolerance);
|
|
||||||
|
|
||||||
// // /**
|
|
||||||
// // * @brief Destructor... cleans up
|
|
||||||
// // */
|
|
||||||
// // ~ObservationBuffer();
|
|
||||||
|
|
||||||
// // /**
|
|
||||||
// // * @brief Sets the global frame of an observation buffer. This will
|
|
||||||
// // * transform all the currently cached observations to the new global
|
|
||||||
// // * frame
|
|
||||||
// // * @param new_global_frame The name of the new global frame.
|
|
||||||
// // * @return True if the operation succeeds, false otherwise
|
|
||||||
// // */
|
|
||||||
// // bool setGlobalFrame(const std::string new_global_frame);
|
|
||||||
|
|
||||||
// // /**
|
|
||||||
// // * @brief Transforms a PointCloud to the global frame and buffers it
|
|
||||||
// // * <b>Note: The burden is on the user to make sure the transform is available... ie they should use a MessageNotifier</b>
|
|
||||||
// // * @param cloud The cloud to be buffered
|
|
||||||
// // */
|
|
||||||
// // void bufferCloud(const robot_sensor_msgs::PointCloud2& cloud);
|
|
||||||
|
|
||||||
// // /**
|
|
||||||
// // * @brief Pushes copies of all current observations onto the end of the vector passed in
|
|
||||||
// // * @param observations The vector to be filled
|
|
||||||
// // */
|
|
||||||
// // void getObservations(std::vector<Observation>& observations);
|
|
||||||
|
|
||||||
// // /**
|
|
||||||
// // * @brief Check if the observation buffer is being update at its expected rate
|
|
||||||
// // * @return True if it is being updated at the expected rate, false otherwise
|
|
||||||
// // */
|
|
||||||
// // bool isCurrent() const;
|
|
||||||
|
|
||||||
// // /**
|
|
||||||
// // * @brief Lock the observation buffer
|
|
||||||
// // */
|
|
||||||
// // inline void lock()
|
|
||||||
// // {
|
|
||||||
// // lock_.lock();
|
|
||||||
// // }
|
|
||||||
|
|
||||||
// // /**
|
|
||||||
// // * @brief Lock the observation buffer
|
|
||||||
// // */
|
|
||||||
// // inline void unlock()
|
|
||||||
// // {
|
|
||||||
// // lock_.unlock();
|
|
||||||
// // }
|
|
||||||
|
|
||||||
// // /**
|
|
||||||
// // * @brief Reset last updated timestamp
|
|
||||||
// // */
|
|
||||||
// // void resetLastUpdated();
|
|
||||||
|
|
||||||
// // private:
|
|
||||||
// // /**
|
|
||||||
// // * @brief Removes any stale observations from the buffer list
|
|
||||||
// // */
|
|
||||||
// // void purgeStaleObservations();
|
|
||||||
|
|
||||||
// // // Helper: trích 4×4 transform matrix từ TransformStampedMsg
|
|
||||||
// // // Tránh gọi tf3::doTransform per-point (overhead virtual dispatch + exception check)
|
|
||||||
// // struct Transform4x4 {
|
|
||||||
// // double m[4][4];
|
|
||||||
// // };
|
|
||||||
|
|
||||||
// // static inline Transform4x4 extractMatrix(const tf3::TransformStampedMsg& tfm)
|
|
||||||
// // {
|
|
||||||
// // // Quaternion → rotation matrix + translation
|
|
||||||
// // const auto& t = tfm.transform.translation;
|
|
||||||
// // const auto& q = tfm.transform.rotation;
|
|
||||||
|
|
||||||
// // double qx = q.x, qy = q.y, qz = q.z, qw = q.w;
|
|
||||||
// // Transform4x4 M;
|
|
||||||
// // M.m[0][0] = 1 - 2*(qy*qy + qz*qz); M.m[0][1] = 2*(qx*qy - qz*qw); M.m[0][2] = 2*(qx*qz + qy*qw); M.m[0][3] = t.x;
|
|
||||||
// // M.m[1][0] = 2*(qx*qy + qz*qw); M.m[1][1] = 1 - 2*(qx*qx + qz*qz); M.m[1][2] = 2*(qy*qz - qx*qw); M.m[1][3] = t.y;
|
|
||||||
// // M.m[2][0] = 2*(qx*qz - qy*qw); M.m[2][1] = 2*(qy*qz + qx*qw); M.m[2][2] = 1 - 2*(qx*qx + qy*qy); M.m[2][3] = t.z;
|
|
||||||
// // M.m[3][0] = 0; M.m[3][1] = 0; M.m[3][2] = 0; M.m[3][3] = 1;
|
|
||||||
// // return M;
|
|
||||||
// // }
|
|
||||||
|
|
||||||
// // double voxel_size_;
|
|
||||||
// // tf3::BufferCore& tf3_buffer_;
|
|
||||||
// // const robot::Duration observation_keep_time_;
|
|
||||||
// // const robot::Duration expected_update_rate_;
|
|
||||||
// // robot::Time last_updated_;
|
|
||||||
// // std::string global_frame_;
|
|
||||||
// // std::string sensor_frame_;
|
|
||||||
// // std::list<Observation> observation_list_;
|
|
||||||
// // std::string topic_name_;
|
|
||||||
// // double min_obstacle_height_, max_obstacle_height_;
|
|
||||||
// // boost::recursive_mutex lock_; ///< @brief A lock for accessing data in callbacks safely
|
|
||||||
// // double obstacle_range_, raytrace_range_;
|
|
||||||
// // double tf_tolerance_;
|
|
||||||
// // };
|
|
||||||
// // } // namespace robot_costmap_2d
|
|
||||||
// // #endif // ROBOT_COSTMAP_2D_OBSERVATION_BUFFER_H_
|
|
||||||
|
|
||||||
// /*********************************************************************
|
|
||||||
// *
|
|
||||||
// * Software License Agreement (BSD License)
|
|
||||||
// *
|
|
||||||
// * Copyright (c) 2008, 2013, Willow Garage, Inc.
|
|
||||||
// * All rights reserved.
|
|
||||||
// *
|
|
||||||
// * Redistribution and use in source and binary forms, with or without
|
|
||||||
// * modification, are permitted provided that the following conditions
|
|
||||||
// * are met:
|
|
||||||
// *
|
|
||||||
// * * Redistributions of source code must retain the above copyright
|
|
||||||
// * notice, this list of conditions and the following disclaimer.
|
|
||||||
// * * Redistributions in binary form must reproduce the above
|
|
||||||
// * copyright notice, this list of conditions and the following
|
|
||||||
// * disclaimer in the documentation and/or other materials provided
|
|
||||||
// * with the distribution.
|
|
||||||
// * * Neither the name of Willow Garage, Inc. nor the names of its
|
|
||||||
// * contributors may be used to endorse or promote products derived
|
|
||||||
// * from this software without specific prior written permission.
|
|
||||||
// *
|
|
||||||
// * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
|
||||||
// * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
|
||||||
// * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
|
||||||
// * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
|
||||||
// * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
|
||||||
// * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
|
||||||
// * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
|
||||||
// * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
|
||||||
// * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
|
||||||
// * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
|
||||||
// * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
|
||||||
// * POSSIBILITY OF SUCH DAMAGE.
|
|
||||||
// *
|
|
||||||
// * Author: Eitan Marder-Eppstein
|
|
||||||
// *********************************************************************/
|
|
||||||
// #ifndef ROBOT_COSTMAP_2D_OBSERVATION_BUFFER_H_
|
|
||||||
// #define ROBOT_COSTMAP_2D_OBSERVATION_BUFFER_H_
|
|
||||||
|
|
||||||
// #include <vector>
|
|
||||||
// #include <list>
|
|
||||||
// #include <string>
|
|
||||||
// #include <unordered_map>
|
|
||||||
// #include <cmath>
|
|
||||||
// #include <cstring>
|
|
||||||
|
|
||||||
// #include <robot/robot.h>
|
|
||||||
// #include <robot_costmap_2d/observation.h>
|
|
||||||
// #include <tf3/buffer_core.h>
|
|
||||||
// #include <robot_sensor_msgs/PointCloud2.h>
|
|
||||||
|
|
||||||
// // Thread support
|
|
||||||
// #include <boost/thread.hpp>
|
|
||||||
|
|
||||||
// namespace robot_costmap_2d
|
|
||||||
// {
|
|
||||||
// /**
|
|
||||||
// * @class ObservationBuffer
|
|
||||||
// * @brief Takes in point clouds from sensors, transforms them to the desired frame, and stores them.
|
|
||||||
// *
|
|
||||||
// * Optimizations vs original:
|
|
||||||
// * - bufferCloud: single-pass transform+filter+voxel-downsample.
|
|
||||||
// * Reduces 6.5 M points to at most (map_w × map_h) representative points,
|
|
||||||
// * which cuts CPU in updateBounds/raytraceFreespace by ~100–200×.
|
|
||||||
// * - extractMatrix: inline quaternion→rotation, avoids per-point virtual dispatch.
|
|
||||||
// * - voxel_size_ (default = costmap resolution, 0.05 m): configurable via
|
|
||||||
// * setVoxelSize() so ObstacleLayer can pass the real resolution.
|
|
||||||
// */
|
|
||||||
// class ObservationBuffer
|
|
||||||
// {
|
|
||||||
// public:
|
|
||||||
// /**
|
|
||||||
// * @brief Constructs an observation buffer
|
|
||||||
// * @param topic_name The topic of the observations, used as an identifier for error and warning messages
|
|
||||||
// * @param observation_keep_time Defines the persistence of observations in seconds, 0 means only keep the latest
|
|
||||||
// * @param expected_update_rate How often this buffer is expected to be updated, 0 means there is no limit
|
|
||||||
// * @param min_obstacle_height The minimum height of a hitpoint to be considered legal
|
|
||||||
// * @param max_obstacle_height The maximum height of a hitpoint to be considered legal
|
|
||||||
// * @param obstacle_range The range to which the sensor should be trusted for inserting obstacles
|
|
||||||
// * @param raytrace_range The range to which the sensor should be trusted for raytracing to clear out space
|
|
||||||
// * @param tf2_buffer A reference to a tf2 Buffer
|
|
||||||
// * @param global_frame The frame to transform PointClouds into
|
|
||||||
// * @param sensor_frame The frame of the origin of the sensor, can be left blank to be read from the messages
|
|
||||||
// * @param tf_tolerance The amount of time to wait for a transform to be available when setting a new global frame
|
|
||||||
// */
|
|
||||||
// ObservationBuffer(std::string topic_name, double observation_keep_time, double expected_update_rate,
|
|
||||||
// double min_obstacle_height, double max_obstacle_height, double obstacle_range,
|
|
||||||
// double raytrace_range, tf3::BufferCore& tf3_buffer, std::string global_frame,
|
|
||||||
// std::string sensor_frame, double tf_tolerance);
|
|
||||||
|
|
||||||
// /**
|
|
||||||
// * @brief Destructor... cleans up
|
|
||||||
// */
|
|
||||||
// ~ObservationBuffer();
|
|
||||||
|
|
||||||
// /**
|
|
||||||
// * @brief Sets the global frame of an observation buffer. This will
|
|
||||||
// * transform all the currently cached observations to the new global frame
|
|
||||||
// * @param new_global_frame The name of the new global frame.
|
|
||||||
// * @return True if the operation succeeds, false otherwise
|
|
||||||
// */
|
|
||||||
// bool setGlobalFrame(const std::string new_global_frame);
|
|
||||||
|
|
||||||
// /**
|
|
||||||
// * @brief Set the voxel size used for downsampling in bufferCloud().
|
|
||||||
// * Should match the costmap resolution (default 0.05 m).
|
|
||||||
// */
|
|
||||||
// inline void setVoxelSize(double voxel_size)
|
|
||||||
// {
|
|
||||||
// voxel_size_ = voxel_size;
|
|
||||||
// inv_voxel_size_ = (voxel_size > 1e-9) ? 1.0 / voxel_size : 20.0;
|
|
||||||
// }
|
|
||||||
|
|
||||||
// /**
|
|
||||||
// * @brief Transforms a PointCloud to the global frame, downsamples it via
|
|
||||||
// * voxel grid (one representative point per costmap cell), applies
|
|
||||||
// * height filtering, and buffers the result.
|
|
||||||
// */
|
|
||||||
// void bufferCloud(const robot_sensor_msgs::PointCloud2& cloud);
|
|
||||||
|
|
||||||
// /**
|
|
||||||
// * @brief Pushes copies of all current observations onto the end of the vector passed in
|
|
||||||
// * @param observations The vector to be filled
|
|
||||||
// */
|
|
||||||
// void getObservations(std::vector<Observation>& observations);
|
|
||||||
|
|
||||||
// /**
|
|
||||||
// * @brief Check if the observation buffer is being updated at its expected rate
|
|
||||||
// * @return True if it is being updated at the expected rate, false otherwise
|
|
||||||
// */
|
|
||||||
// bool isCurrent() const;
|
|
||||||
|
|
||||||
// /**
|
|
||||||
// * @brief Lock the observation buffer
|
|
||||||
// */
|
|
||||||
// inline void lock() { lock_.lock(); }
|
|
||||||
|
|
||||||
// /**
|
|
||||||
// * @brief Unlock the observation buffer
|
|
||||||
// */
|
|
||||||
// inline void unlock() { lock_.unlock(); }
|
|
||||||
|
|
||||||
// /**
|
|
||||||
// * @brief Reset last updated timestamp
|
|
||||||
// */
|
|
||||||
// void resetLastUpdated();
|
|
||||||
|
|
||||||
// private:
|
|
||||||
// /**
|
|
||||||
// * @brief Removes any stale observations from the buffer list
|
|
||||||
// */
|
|
||||||
// void purgeStaleObservations();
|
|
||||||
|
|
||||||
// // ── Transform helper ────────────────────────────────────────────────────
|
|
||||||
// // Encode a TF transform as a plain 4×4 double matrix so the hot loop in
|
|
||||||
// // bufferCloud can do a simple FMA multiply without any virtual dispatch,
|
|
||||||
// // exception handling, or iterator overhead.
|
|
||||||
// struct Transform4x4
|
|
||||||
// {
|
|
||||||
// double m[4][4];
|
|
||||||
// };
|
|
||||||
|
|
||||||
// static inline Transform4x4 extractMatrix(const tf3::TransformStampedMsg& tfm)
|
|
||||||
// {
|
|
||||||
// const auto& t = tfm.transform.translation;
|
|
||||||
// const auto& q = tfm.transform.rotation;
|
|
||||||
|
|
||||||
// const double qx = q.x, qy = q.y, qz = q.z, qw = q.w;
|
|
||||||
// Transform4x4 M;
|
|
||||||
// // Row 0
|
|
||||||
// M.m[0][0] = 1.0 - 2.0*(qy*qy + qz*qz);
|
|
||||||
// M.m[0][1] = 2.0*(qx*qy - qz*qw);
|
|
||||||
// M.m[0][2] = 2.0*(qx*qz + qy*qw);
|
|
||||||
// M.m[0][3] = t.x;
|
|
||||||
// // Row 1
|
|
||||||
// M.m[1][0] = 2.0*(qx*qy + qz*qw);
|
|
||||||
// M.m[1][1] = 1.0 - 2.0*(qx*qx + qz*qz);
|
|
||||||
// M.m[1][2] = 2.0*(qy*qz - qx*qw);
|
|
||||||
// M.m[1][3] = t.y;
|
|
||||||
// // Row 2
|
|
||||||
// M.m[2][0] = 2.0*(qx*qz - qy*qw);
|
|
||||||
// M.m[2][1] = 2.0*(qy*qz + qx*qw);
|
|
||||||
// M.m[2][2] = 1.0 - 2.0*(qx*qx + qy*qy);
|
|
||||||
// M.m[2][3] = t.z;
|
|
||||||
// // Row 3 (homogeneous)
|
|
||||||
// M.m[3][0] = 0.0; M.m[3][1] = 0.0; M.m[3][2] = 0.0; M.m[3][3] = 1.0;
|
|
||||||
// return M;
|
|
||||||
// }
|
|
||||||
|
|
||||||
// // ── Data members ────────────────────────────────────────────────────────
|
|
||||||
// tf3::BufferCore& tf3_buffer_;
|
|
||||||
// const robot::Duration observation_keep_time_;
|
|
||||||
// const robot::Duration expected_update_rate_;
|
|
||||||
// robot::Time last_updated_;
|
|
||||||
// std::string global_frame_;
|
|
||||||
// std::string sensor_frame_;
|
|
||||||
// std::list<Observation> observation_list_;
|
|
||||||
// std::string topic_name_;
|
|
||||||
// double min_obstacle_height_;
|
|
||||||
// double max_obstacle_height_;
|
|
||||||
// boost::recursive_mutex lock_;
|
|
||||||
// double obstacle_range_;
|
|
||||||
// double raytrace_range_;
|
|
||||||
// double tf_tolerance_;
|
|
||||||
|
|
||||||
// // Voxel-grid downsampling parameters (set via setVoxelSize)
|
|
||||||
// double voxel_size_ = 0.05; // metres – match costmap resolution
|
|
||||||
// double inv_voxel_size_ = 20.0; // 1/voxel_size_, cached
|
|
||||||
// };
|
|
||||||
|
|
||||||
// } // namespace robot_costmap_2d
|
|
||||||
// #endif // ROBOT_COSTMAP_2D_OBSERVATION_BUFFER_H_
|
|
||||||
/*********************************************************************
|
/*********************************************************************
|
||||||
*
|
*
|
||||||
* Software License Agreement (BSD License)
|
* Software License Agreement (BSD License)
|
||||||
@@ -463,6 +75,13 @@ public:
|
|||||||
double min_obstacle_height, double max_obstacle_height, double obstacle_range,
|
double min_obstacle_height, double max_obstacle_height, double obstacle_range,
|
||||||
double raytrace_range, tf3::BufferCore& tf3_buffer, std::string global_frame,
|
double raytrace_range, tf3::BufferCore& tf3_buffer, std::string global_frame,
|
||||||
std::string sensor_frame, double tf_tolerance);
|
std::string sensor_frame, double tf_tolerance);
|
||||||
|
|
||||||
|
ObservationBuffer(std::string topic_name, double observation_keep_time, double expected_update_rate,
|
||||||
|
double min_obstacle_height, double max_obstacle_height, double obstacle_range,
|
||||||
|
double raytrace_range, unsigned int frustum_pixel_step, double frustum_min_range,
|
||||||
|
double frustum_max_range, tf3::BufferCore& tf3_buffer, std::string global_frame,
|
||||||
|
std::string sensor_frame, double tf_tolerance);
|
||||||
|
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* @brief Destructor... cleans up
|
* @brief Destructor... cleans up
|
||||||
@@ -485,12 +104,24 @@ public:
|
|||||||
*/
|
*/
|
||||||
void bufferCloud(const robot_sensor_msgs::PointCloud2& cloud);
|
void bufferCloud(const robot_sensor_msgs::PointCloud2& cloud);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Store the newest depth frame without converting it to PointCloud2.
|
||||||
|
*/
|
||||||
|
void bufferDepthCamera(const robot_sensor_msgs::DepthCameraData& depth);
|
||||||
|
|
||||||
|
void bufferDepthCamera(robot_sensor_msgs::DepthCameraData::ConstPtr depth);
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* @brief Pushes copies of all current observations onto the end of the vector passed in
|
* @brief Pushes copies of all current observations onto the end of the vector passed in
|
||||||
* @param observations The vector to be filled
|
* @param observations The vector to be filled
|
||||||
*/
|
*/
|
||||||
void getObservations(std::vector<Observation>& observations);
|
void getObservations(std::vector<Observation>& observations);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Append the current depth observation, if it has not expired.
|
||||||
|
*/
|
||||||
|
void getDepthObservations(std::vector<DepthCameraObservation>& observations);
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* @brief Check if the observation buffer is being update at its expected rate
|
* @brief Check if the observation buffer is being update at its expected rate
|
||||||
* @return True if it is being updated at the expected rate, false otherwise
|
* @return True if it is being updated at the expected rate, false otherwise
|
||||||
@@ -524,6 +155,8 @@ private:
|
|||||||
*/
|
*/
|
||||||
void purgeStaleObservations();
|
void purgeStaleObservations();
|
||||||
|
|
||||||
|
void purgeStaleDepthObservations();
|
||||||
|
|
||||||
tf3::BufferCore& tf3_buffer_;
|
tf3::BufferCore& tf3_buffer_;
|
||||||
const robot::Duration observation_keep_time_;
|
const robot::Duration observation_keep_time_;
|
||||||
const robot::Duration expected_update_rate_;
|
const robot::Duration expected_update_rate_;
|
||||||
@@ -531,11 +164,16 @@ private:
|
|||||||
std::string global_frame_;
|
std::string global_frame_;
|
||||||
std::string sensor_frame_;
|
std::string sensor_frame_;
|
||||||
std::list<Observation> observation_list_;
|
std::list<Observation> observation_list_;
|
||||||
|
std::list<DepthCameraObservation> depth_observation_list_;
|
||||||
|
// DepthCameraObservation depth_observation_;
|
||||||
std::string topic_name_;
|
std::string topic_name_;
|
||||||
double min_obstacle_height_, max_obstacle_height_;
|
double min_obstacle_height_, max_obstacle_height_;
|
||||||
boost::recursive_mutex lock_; ///< @brief A lock for accessing data in callbacks safely
|
boost::recursive_mutex lock_; ///< @brief A lock for accessing data in callbacks safely
|
||||||
double obstacle_range_, raytrace_range_;
|
double obstacle_range_, raytrace_range_;
|
||||||
double tf_tolerance_;
|
double tf_tolerance_;
|
||||||
|
unsigned int frustum_pixel_step_;
|
||||||
|
double frustum_min_range_;
|
||||||
|
double frustum_max_range_;
|
||||||
};
|
};
|
||||||
} // namespace robot_costmap_2d
|
} // namespace robot_costmap_2d
|
||||||
#endif // ROBOT_COSTMAP_2D_OBSERVATION_BUFFER_H_
|
#endif // ROBOT_COSTMAP_2D_OBSERVATION_BUFFER_H_
|
||||||
|
|||||||
@@ -46,13 +46,14 @@
|
|||||||
|
|
||||||
#include <robot_nav_msgs/OccupancyGrid.h>
|
#include <robot_nav_msgs/OccupancyGrid.h>
|
||||||
|
|
||||||
|
#include <mutex>
|
||||||
|
|
||||||
|
#include <robot_sensor_msgs/DepthCameraData.h>
|
||||||
#include <robot_sensor_msgs/LaserScan.h>
|
#include <robot_sensor_msgs/LaserScan.h>
|
||||||
#include <robot_laser_geometry/laser_geometry.hpp>
|
#include <robot_laser_geometry/laser_geometry.hpp>
|
||||||
#include <robot_sensor_msgs/DepthCameraData.h>
|
|
||||||
#include <robot_sensor_msgs/PointCloud.h>
|
#include <robot_sensor_msgs/PointCloud.h>
|
||||||
#include <robot_sensor_msgs/PointCloud2.h>
|
#include <robot_sensor_msgs/PointCloud2.h>
|
||||||
#include <robot_sensor_msgs/point_cloud_conversion.h>
|
#include <robot_sensor_msgs/point_cloud_conversion.h>
|
||||||
#include <mutex>
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
@@ -69,8 +70,7 @@ struct CallBackInfo
|
|||||||
class ObstacleLayer : public CostmapLayer
|
class ObstacleLayer : public CostmapLayer
|
||||||
{
|
{
|
||||||
public:
|
public:
|
||||||
ObstacleLayer() :
|
ObstacleLayer()
|
||||||
have_depth_camera_data_(false)
|
|
||||||
{
|
{
|
||||||
costmap_ = NULL; // this is the unsigned char* member of parent class Costmap2D.
|
costmap_ = NULL; // this is the unsigned char* member of parent class Costmap2D.
|
||||||
}
|
}
|
||||||
@@ -131,6 +131,12 @@ protected:
|
|||||||
void pointCloud2Callback(const robot_sensor_msgs::PointCloud2& message,
|
void pointCloud2Callback(const robot_sensor_msgs::PointCloud2& message,
|
||||||
const boost::shared_ptr<robot_costmap_2d::ObservationBuffer>& buffer);
|
const boost::shared_ptr<robot_costmap_2d::ObservationBuffer>& buffer);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Buffer a depth image and its camera model for frustum clearing.
|
||||||
|
*/
|
||||||
|
void depthImageCallback(robot_sensor_msgs::DepthCameraData::ConstPtr message,
|
||||||
|
const boost::shared_ptr<robot_costmap_2d::ObservationBuffer>& buffer);
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* @brief Get the observations used to mark space
|
* @brief Get the observations used to mark space
|
||||||
* @param marking_observations A reference to a vector that will be populated with the observations
|
* @param marking_observations A reference to a vector that will be populated with the observations
|
||||||
@@ -145,6 +151,13 @@ protected:
|
|||||||
*/
|
*/
|
||||||
bool getClearingObservations(std::vector<robot_costmap_2d::Observation>& clearing_observations) const;
|
bool getClearingObservations(std::vector<robot_costmap_2d::Observation>& clearing_observations) const;
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Collect fresh depth frames from every configured frustum-clearing source.
|
||||||
|
* @return True when every configured depth source is current.
|
||||||
|
*/
|
||||||
|
bool getFrustumClearingObservations(
|
||||||
|
std::vector<robot_costmap_2d::DepthCameraObservation>& frustum_clearing_observations) const;
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* @brief Clear freespace based on one observation
|
* @brief Clear freespace based on one observation
|
||||||
* @param clearing_observation The observation used to raytrace
|
* @param clearing_observation The observation used to raytrace
|
||||||
@@ -173,6 +186,9 @@ protected:
|
|||||||
std::vector<boost::shared_ptr<robot_costmap_2d::ObservationBuffer> > marking_buffers_; ///< @brief Used to store observation buffers used for marking obstacles
|
std::vector<boost::shared_ptr<robot_costmap_2d::ObservationBuffer> > marking_buffers_; ///< @brief Used to store observation buffers used for marking obstacles
|
||||||
std::vector<boost::shared_ptr<robot_costmap_2d::ObservationBuffer> > clearing_buffers_; ///< @brief Used to store observation buffers used for clearing obstacles
|
std::vector<boost::shared_ptr<robot_costmap_2d::ObservationBuffer> > clearing_buffers_; ///< @brief Used to store observation buffers used for clearing obstacles
|
||||||
|
|
||||||
|
std::vector<boost::shared_ptr<robot_costmap_2d::ObservationBuffer> > depth_observation_buffers_;
|
||||||
|
std::vector<boost::shared_ptr<robot_costmap_2d::ObservationBuffer> > depth_clearing_buffers_;
|
||||||
|
|
||||||
// Used only for testing purposes
|
// Used only for testing purposes
|
||||||
std::vector<robot_costmap_2d::Observation> static_clearing_observations_, static_marking_observations_;
|
std::vector<robot_costmap_2d::Observation> static_clearing_observations_, static_marking_observations_;
|
||||||
|
|
||||||
@@ -181,10 +197,10 @@ protected:
|
|||||||
|
|
||||||
int combination_method_;
|
int combination_method_;
|
||||||
std::vector<CallBackInfo> callback_infos_;
|
std::vector<CallBackInfo> callback_infos_;
|
||||||
std::string depth_camera_topic_;
|
std::vector<CallBackInfo> callback_depth_infos_;
|
||||||
mutable std::mutex depth_camera_mutex_;
|
std::string depth_camera_data_topic_;
|
||||||
robot_sensor_msgs::DepthCameraData latest_depth_camera_data_;
|
mutable std::mutex depth_camera_data_mutex_;
|
||||||
bool have_depth_camera_data_;
|
robot_sensor_msgs::DepthCameraData::ConstPtr pending_depth_camera_data_;
|
||||||
|
|
||||||
private:
|
private:
|
||||||
bool getParams(const std::string& config_file_name, robot::NodeHandle &nh);
|
bool getParams(const std::string& config_file_name, robot::NodeHandle &nh);
|
||||||
|
|||||||
@@ -45,7 +45,6 @@
|
|||||||
#include <robot_nav_msgs/OccupancyGrid.h>
|
#include <robot_nav_msgs/OccupancyGrid.h>
|
||||||
#include <robot_sensor_msgs/LaserScan.h>
|
#include <robot_sensor_msgs/LaserScan.h>
|
||||||
#include <robot_laser_geometry/laser_geometry.hpp>
|
#include <robot_laser_geometry/laser_geometry.hpp>
|
||||||
#include <robot_sensor_msgs/Image.h>
|
|
||||||
#include <robot_sensor_msgs/PointCloud.h>
|
#include <robot_sensor_msgs/PointCloud.h>
|
||||||
#include <robot_sensor_msgs/PointCloud2.h>
|
#include <robot_sensor_msgs/PointCloud2.h>
|
||||||
#include <robot_sensor_msgs/point_cloud_conversion.h>
|
#include <robot_sensor_msgs/point_cloud_conversion.h>
|
||||||
@@ -59,11 +58,7 @@ class VoxelLayer : public ObstacleLayer
|
|||||||
{
|
{
|
||||||
public:
|
public:
|
||||||
VoxelLayer() :
|
VoxelLayer() :
|
||||||
robot_voxel_grid_(0, 0, 0),
|
robot_voxel_grid_(0, 0, 0)
|
||||||
frustum_clearing_enabled_(false),
|
|
||||||
frustum_clearing_pixel_step_(8),
|
|
||||||
frustum_min_range_(0.2),
|
|
||||||
frustum_max_range_(3.0)
|
|
||||||
{
|
{
|
||||||
costmap_ = NULL; // this is the unsigned char* member of parent class's parent class Costmap2D.
|
costmap_ = NULL; // this is the unsigned char* member of parent class's parent class Costmap2D.
|
||||||
}
|
}
|
||||||
@@ -96,24 +91,68 @@ private:
|
|||||||
void clearNonLethal(double wx, double wy, double w_size_x, double w_size_y, bool clear_no_info);
|
void clearNonLethal(double wx, double wy, double w_size_x, double w_size_y, bool clear_no_info);
|
||||||
virtual void raytraceFreespace(const robot_costmap_2d::Observation& clearing_observation, double* min_x, double* min_y,
|
virtual void raytraceFreespace(const robot_costmap_2d::Observation& clearing_observation, double* min_x, double* min_y,
|
||||||
double* max_x, double* max_y);
|
double* max_x, double* max_y);
|
||||||
bool raytraceDepthFrustum(double* min_x, double* min_y, double* max_x, double* max_y);
|
// bool raytraceDepthFrustum(double* min_x, double* min_y, double* max_x, double* max_y);
|
||||||
|
bool raytraceDepthFrustum(const robot_costmap_2d::DepthCameraObservation& observation,
|
||||||
|
double* min_x, double* min_y, double* max_x, double* max_y);
|
||||||
bool readDepthMeters(const robot_sensor_msgs::Image& depth, unsigned int u, unsigned int v,
|
bool readDepthMeters(const robot_sensor_msgs::Image& depth, unsigned int u, unsigned int v,
|
||||||
double& depth_m, bool& is_valid) const;
|
double& depth_m, bool& is_valid) const;
|
||||||
|
void updateDepthRayCache(unsigned int width, unsigned int height, unsigned int pixel_step,
|
||||||
|
double fx, double fy, double cx, double cy,
|
||||||
|
unsigned int u_offset, unsigned int v_offset);
|
||||||
bool clipRaytraceEndpoint(double ox, double oy, double oz, double& wx, double& wy, double& wz);
|
bool clipRaytraceEndpoint(double ox, double oy, double oz, double& wx, double& wy, double& wz);
|
||||||
bool clearVoxelRay(double ox, double oy, double oz, double wx, double wy, double wz,
|
bool clearVoxelRay(double ox, double oy, double oz, double wx, double wy, double wz,
|
||||||
double raytrace_range, double* min_x, double* min_y, double* max_x, double* max_y);
|
double raytrace_range, unsigned int cell_raytrace_range,
|
||||||
|
double* min_x, double* min_y, double* max_x, double* max_y);
|
||||||
|
/// Frees LETHAL cells this layer marked that have not been re-marked within
|
||||||
|
/// obstacle_decay_time. Handles ghost cells frustum clearing can never
|
||||||
|
/// reach: cells that left the camera FOV, occlusion shadows, and cells
|
||||||
|
/// inside the skip band in front of a measured surface.
|
||||||
|
void decayStaleObstacles(double now_sec, double* min_x, double* min_y,
|
||||||
|
double* max_x, double* max_y);
|
||||||
|
|
||||||
|
|
||||||
bool publish_voxel_;
|
bool publish_voxel_;
|
||||||
robot_voxel_grid::VoxelGrid robot_voxel_grid_;
|
robot_voxel_grid::VoxelGrid robot_voxel_grid_;
|
||||||
double z_resolution_, origin_z_;
|
double z_resolution_, origin_z_;
|
||||||
unsigned int unknown_threshold_, mark_threshold_, size_z_;
|
unsigned int unknown_threshold_, mark_threshold_, size_z_;
|
||||||
bool frustum_clearing_enabled_;
|
/// Seconds a marked cell survives without being re-observed before it is
|
||||||
unsigned int frustum_clearing_pixel_step_;
|
/// freed. <= 0 disables decay (default). Only enable on camera-only
|
||||||
double frustum_min_range_;
|
/// costmaps: decay clears obstacles the sensor cannot currently see.
|
||||||
double frustum_max_range_;
|
double obstacle_decay_time_{0.0};
|
||||||
std::string frustum_depth_camera_topic_;
|
/// Clearing rays stop this far [m] before the measured surface. < 0 keeps
|
||||||
|
/// the legacy 2 * resolution behaviour.
|
||||||
|
double frustum_skip_distance_{-1.0};
|
||||||
|
/// Per-cell robot::Time seconds of the last marking; 0 = no valid stamp.
|
||||||
|
std::vector<double> cell_last_marked_;
|
||||||
robot_sensor_msgs::PointCloud clearing_endpoints_;
|
robot_sensor_msgs::PointCloud clearing_endpoints_;
|
||||||
|
std::vector<unsigned char> rolling_costmap_scratch_;
|
||||||
|
std::vector<unsigned int> rolling_voxel_scratch_;
|
||||||
|
std::vector<double> rolling_stamp_scratch_;
|
||||||
|
|
||||||
|
struct DepthRay
|
||||||
|
{
|
||||||
|
unsigned int u;
|
||||||
|
unsigned int v;
|
||||||
|
double x;
|
||||||
|
double y;
|
||||||
|
double z;
|
||||||
|
};
|
||||||
|
std::vector<DepthRay> depth_ray_cache_;
|
||||||
|
unsigned int cached_depth_width_ = 0;
|
||||||
|
unsigned int cached_depth_height_ = 0;
|
||||||
|
unsigned int cached_depth_pixel_step_ = 0;
|
||||||
|
double cached_fx_ = 0.0;
|
||||||
|
double cached_fy_ = 0.0;
|
||||||
|
double cached_cx_ = 0.0;
|
||||||
|
double cached_cy_ = 0.0;
|
||||||
|
unsigned int cached_u_offset_ = 0;
|
||||||
|
unsigned int cached_v_offset_ = 0;
|
||||||
|
/// Advances every frustum pass to dither the sampled pixel grid. A static
|
||||||
|
/// camera otherwise re-traces the same fixed pixel_step subgrid each frame,
|
||||||
|
/// so cells between adjacent rays (angular gap pixel_step / fx) are never
|
||||||
|
/// crossed and stay marked until the robot moves. Cycling the grid phase
|
||||||
|
/// sweeps every pixel over pixel_step^2 frames at unchanged per-frame cost.
|
||||||
|
unsigned int frustum_phase_ = 0;
|
||||||
|
|
||||||
inline bool worldToMap3DFloat(double wx, double wy, double wz, double& mx, double& my, double& mz)
|
inline bool worldToMap3DFloat(double wx, double wy, double wz, double& mx, double& my, double& mz)
|
||||||
{
|
{
|
||||||
|
|||||||
@@ -58,7 +58,6 @@ InflationLayer::InflationLayer()
|
|||||||
, inflate_unknown_(false)
|
, inflate_unknown_(false)
|
||||||
, cell_inflation_radius_(0)
|
, cell_inflation_radius_(0)
|
||||||
, cached_cell_inflation_radius_(0)
|
, cached_cell_inflation_radius_(0)
|
||||||
, seen_(NULL)
|
|
||||||
, cached_costs_(NULL)
|
, cached_costs_(NULL)
|
||||||
, cached_distances_(NULL)
|
, cached_distances_(NULL)
|
||||||
, last_min_x_(-std::numeric_limits<float>::max())
|
, last_min_x_(-std::numeric_limits<float>::max())
|
||||||
@@ -76,10 +75,8 @@ void InflationLayer::onInitialize()
|
|||||||
|
|
||||||
boost::unique_lock < boost::recursive_mutex > lock(*inflation_access_);
|
boost::unique_lock < boost::recursive_mutex > lock(*inflation_access_);
|
||||||
current_ = true;
|
current_ = true;
|
||||||
if (seen_)
|
seen_.clear();
|
||||||
delete[] seen_;
|
seen_generation_ = 0;
|
||||||
seen_ = NULL;
|
|
||||||
seen_size_ = 0;
|
|
||||||
need_reinflation_ = false;
|
need_reinflation_ = false;
|
||||||
std::string config_file_name = "inflation_layer_params.yaml";
|
std::string config_file_name = "inflation_layer_params.yaml";
|
||||||
// std::cout << "InflationLayer: " << config_file_name << std::endl;
|
// std::cout << "InflationLayer: " << config_file_name << std::endl;
|
||||||
@@ -144,10 +141,8 @@ void InflationLayer::matchSize()
|
|||||||
computeCaches();
|
computeCaches();
|
||||||
|
|
||||||
unsigned int size_x = costmap->getSizeInCellsX(), size_y = costmap->getSizeInCellsY();
|
unsigned int size_x = costmap->getSizeInCellsX(), size_y = costmap->getSizeInCellsY();
|
||||||
if (seen_)
|
seen_.assign(static_cast<std::size_t>(size_x) * size_y, 0);
|
||||||
delete[] seen_;
|
seen_generation_ = 0;
|
||||||
seen_size_ = size_x * size_y;
|
|
||||||
seen_ = new bool[seen_size_];
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void InflationLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x,
|
void InflationLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x,
|
||||||
@@ -203,26 +198,28 @@ void InflationLayer::updateCosts(robot_costmap_2d::Costmap2D& master_grid, int m
|
|||||||
if (cell_inflation_radius_ == 0)
|
if (cell_inflation_radius_ == 0)
|
||||||
return;
|
return;
|
||||||
|
|
||||||
// make sure the inflation list is empty at the beginning of the cycle (should always be true)
|
for (std::vector<CellData>& cells : inflation_cells_)
|
||||||
if(!inflation_cells_.empty())
|
cells.clear();
|
||||||
robot::log_error("The inflation list must be empty at the beginning of inflation\n");
|
|
||||||
|
|
||||||
unsigned char* master_array = master_grid.getCharMap();
|
unsigned char* master_array = master_grid.getCharMap();
|
||||||
unsigned int size_x = master_grid.getSizeInCellsX(), size_y = master_grid.getSizeInCellsY();
|
unsigned int size_x = master_grid.getSizeInCellsX(), size_y = master_grid.getSizeInCellsY();
|
||||||
|
|
||||||
if (seen_ == NULL) {
|
const std::size_t map_size = static_cast<std::size_t>(size_x) * size_y;
|
||||||
robot::log_error("InflationLayer::updateCosts(): seen_ array is NULL\n");
|
if (seen_.size() != map_size)
|
||||||
seen_size_ = size_x * size_y;
|
|
||||||
seen_ = new bool[seen_size_];
|
|
||||||
}
|
|
||||||
else if (seen_size_ != size_x * size_y)
|
|
||||||
{
|
{
|
||||||
robot::log_error("InflationLayer::updateCosts(): seen_ array size is wrong\n");
|
seen_.assign(map_size, 0);
|
||||||
delete[] seen_;
|
seen_generation_ = 0;
|
||||||
seen_size_ = size_x * size_y;
|
}
|
||||||
seen_ = new bool[seen_size_];
|
|
||||||
|
if (seen_generation_ == std::numeric_limits<std::uint32_t>::max())
|
||||||
|
{
|
||||||
|
std::fill(seen_.begin(), seen_.end(), 0);
|
||||||
|
seen_generation_ = 1;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
++seen_generation_;
|
||||||
}
|
}
|
||||||
memset(seen_, false, size_x * size_y * sizeof(bool));
|
|
||||||
|
|
||||||
// We need to include in the inflation cells outside the bounding
|
// We need to include in the inflation cells outside the bounding
|
||||||
// box min_i...max_j, by the amount cell_inflation_radius_. Cells
|
// box min_i...max_j, by the amount cell_inflation_radius_. Cells
|
||||||
@@ -238,11 +235,13 @@ void InflationLayer::updateCosts(robot_costmap_2d::Costmap2D& master_grid, int m
|
|||||||
max_i = std::min(int(size_x), max_i);
|
max_i = std::min(int(size_x), max_i);
|
||||||
max_j = std::min(int(size_y), max_j);
|
max_j = std::min(int(size_y), max_j);
|
||||||
|
|
||||||
// Inflation list; we append cells to visit in a list associated with its distance to the nearest obstacle
|
// Precomputed distance buckets preserve priority ordering without a tree lookup
|
||||||
// We use a map<distance, list> to emulate the priority queue used before, with a notable performance boost
|
// for every enqueued cell.
|
||||||
|
|
||||||
// Start with lethal obstacles: by definition distance is 0.0
|
// Start with lethal obstacles: by definition distance is 0.0
|
||||||
std::vector<CellData>& obs_bin = inflation_cells_[0.0];
|
if (inflation_cells_.empty())
|
||||||
|
return;
|
||||||
|
std::vector<CellData>& obs_bin = inflation_cells_.front();
|
||||||
for (int j = min_j; j < max_j; j++)
|
for (int j = min_j; j < max_j; j++)
|
||||||
{
|
{
|
||||||
for (int i = min_i; i < max_i; i++)
|
for (int i = min_i; i < max_i; i++)
|
||||||
@@ -258,23 +257,22 @@ void InflationLayer::updateCosts(robot_costmap_2d::Costmap2D& master_grid, int m
|
|||||||
|
|
||||||
// Process cells by increasing distance; new cells are appended to the corresponding distance bin, so they
|
// Process cells by increasing distance; new cells are appended to the corresponding distance bin, so they
|
||||||
// can overtake previously inserted but farther away cells
|
// can overtake previously inserted but farther away cells
|
||||||
std::map<double, std::vector<CellData> >::iterator bin;
|
for (std::vector<CellData>& bin : inflation_cells_)
|
||||||
for (bin = inflation_cells_.begin(); bin != inflation_cells_.end(); ++bin)
|
|
||||||
{
|
{
|
||||||
for (int i = 0; i < bin->second.size(); ++i)
|
for (std::size_t i = 0; i < bin.size(); ++i)
|
||||||
{
|
{
|
||||||
// process all cells at distance dist_bin.first
|
// process all cells at distance dist_bin.first
|
||||||
const CellData& cell = bin->second[i];
|
const CellData& cell = bin[i];
|
||||||
|
|
||||||
unsigned int index = cell.index_;
|
unsigned int index = cell.index_;
|
||||||
|
|
||||||
// ignore if already visited
|
// ignore if already visited
|
||||||
if (seen_[index])
|
if (seen_[index] == seen_generation_)
|
||||||
{
|
{
|
||||||
continue;
|
continue;
|
||||||
}
|
}
|
||||||
|
|
||||||
seen_[index] = true;
|
seen_[index] = seen_generation_;
|
||||||
|
|
||||||
unsigned int mx = cell.x_;
|
unsigned int mx = cell.x_;
|
||||||
unsigned int my = cell.y_;
|
unsigned int my = cell.y_;
|
||||||
@@ -301,7 +299,6 @@ void InflationLayer::updateCosts(robot_costmap_2d::Costmap2D& master_grid, int m
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
inflation_cells_.clear();
|
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
@@ -316,7 +313,7 @@ void InflationLayer::updateCosts(robot_costmap_2d::Costmap2D& master_grid, int m
|
|||||||
inline void InflationLayer::enqueue(unsigned int index, unsigned int mx, unsigned int my,
|
inline void InflationLayer::enqueue(unsigned int index, unsigned int mx, unsigned int my,
|
||||||
unsigned int src_x, unsigned int src_y)
|
unsigned int src_x, unsigned int src_y)
|
||||||
{
|
{
|
||||||
if (!seen_[index])
|
if (seen_[index] != seen_generation_)
|
||||||
{
|
{
|
||||||
// we compute our distance table one cell further than the inflation radius dictates so we can make the check below
|
// we compute our distance table one cell further than the inflation radius dictates so we can make the check below
|
||||||
double distance = distanceLookup(mx, my, src_x, src_y);
|
double distance = distanceLookup(mx, my, src_x, src_y);
|
||||||
@@ -325,8 +322,10 @@ inline void InflationLayer::enqueue(unsigned int index, unsigned int mx, unsigne
|
|||||||
if (distance > cell_inflation_radius_)
|
if (distance > cell_inflation_radius_)
|
||||||
return;
|
return;
|
||||||
|
|
||||||
// push the cell data onto the inflation list and mark
|
const unsigned int dx = std::abs(static_cast<int>(mx) - static_cast<int>(src_x));
|
||||||
inflation_cells_[distance].push_back(CellData(index, mx, my, src_x, src_y));
|
const unsigned int dy = std::abs(static_cast<int>(my) - static_cast<int>(src_y));
|
||||||
|
const unsigned int bin_index = distance_bin_lookup_[dx * distance_lookup_size_ + dy];
|
||||||
|
inflation_cells_[bin_index].push_back(CellData(index, mx, my, src_x, src_y));
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -354,6 +353,38 @@ void InflationLayer::computeCaches()
|
|||||||
}
|
}
|
||||||
|
|
||||||
cached_cell_inflation_radius_ = cell_inflation_radius_;
|
cached_cell_inflation_radius_ = cell_inflation_radius_;
|
||||||
|
|
||||||
|
distance_lookup_size_ = cell_inflation_radius_ + 2;
|
||||||
|
distance_levels_.clear();
|
||||||
|
for (unsigned int i = 0; i < distance_lookup_size_; ++i)
|
||||||
|
{
|
||||||
|
for (unsigned int j = 0; j < distance_lookup_size_; ++j)
|
||||||
|
{
|
||||||
|
if (cached_distances_[i][j] <= cell_inflation_radius_)
|
||||||
|
distance_levels_.push_back(cached_distances_[i][j]);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
std::sort(distance_levels_.begin(), distance_levels_.end());
|
||||||
|
distance_levels_.erase(
|
||||||
|
std::unique(distance_levels_.begin(), distance_levels_.end()), distance_levels_.end());
|
||||||
|
|
||||||
|
inflation_cells_.clear();
|
||||||
|
inflation_cells_.resize(distance_levels_.size());
|
||||||
|
distance_bin_lookup_.assign(
|
||||||
|
static_cast<std::size_t>(distance_lookup_size_) * distance_lookup_size_, 0);
|
||||||
|
for (unsigned int i = 0; i < distance_lookup_size_; ++i)
|
||||||
|
{
|
||||||
|
for (unsigned int j = 0; j < distance_lookup_size_; ++j)
|
||||||
|
{
|
||||||
|
const double distance = cached_distances_[i][j];
|
||||||
|
if (distance > cell_inflation_radius_)
|
||||||
|
continue;
|
||||||
|
distance_bin_lookup_[i * distance_lookup_size_ + j] =
|
||||||
|
static_cast<unsigned int>(
|
||||||
|
std::lower_bound(distance_levels_.begin(), distance_levels_.end(), distance) -
|
||||||
|
distance_levels_.begin());
|
||||||
|
}
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
for (unsigned int i = 0; i <= cell_inflation_radius_ + 1; ++i)
|
for (unsigned int i = 0; i <= cell_inflation_radius_ + 1; ++i)
|
||||||
@@ -367,6 +398,10 @@ void InflationLayer::computeCaches()
|
|||||||
|
|
||||||
void InflationLayer::deleteKernels()
|
void InflationLayer::deleteKernels()
|
||||||
{
|
{
|
||||||
|
inflation_cells_.clear();
|
||||||
|
distance_levels_.clear();
|
||||||
|
distance_bin_lookup_.clear();
|
||||||
|
distance_lookup_size_ = 0;
|
||||||
if (cached_distances_ != NULL)
|
if (cached_distances_ != NULL)
|
||||||
{
|
{
|
||||||
for (unsigned int i = 0; i <= cached_cell_inflation_radius_ + 1; ++i)
|
for (unsigned int i = 0; i <= cached_cell_inflation_radius_ + 1; ++i)
|
||||||
|
|||||||
@@ -131,6 +131,10 @@ bool ObstacleLayer::getParams(const std::string& config_file_name, robot::NodeHa
|
|||||||
double observation_keep_time = 0, expected_update_rate = 0, min_obstacle_height = 0, max_obstacle_height = 2;
|
double observation_keep_time = 0, expected_update_rate = 0, min_obstacle_height = 0, max_obstacle_height = 2;
|
||||||
std::string topic = "map", sensor_frame = "laser_frame", data_type = "PointCloud";
|
std::string topic = "map", sensor_frame = "laser_frame", data_type = "PointCloud";
|
||||||
bool inf_is_valid = false, clearing=false, marking=true;
|
bool inf_is_valid = false, clearing=false, marking=true;
|
||||||
|
bool frustum_clearing_enabled = false;
|
||||||
|
int frustum_pixel_step = 8;
|
||||||
|
double frustum_min_range = 0.2;
|
||||||
|
double frustum_max_range = 3.0;
|
||||||
|
|
||||||
robot::NodeHandle priv_nh(nh, source);
|
robot::NodeHandle priv_nh(nh, source);
|
||||||
topic = loadParam(layer[source],"topic", topic);
|
topic = loadParam(layer[source],"topic", topic);
|
||||||
@@ -143,6 +147,10 @@ bool ObstacleLayer::getParams(const std::string& config_file_name, robot::NodeHa
|
|||||||
inf_is_valid = loadParam(layer[source],"inf_is_valid", false);
|
inf_is_valid = loadParam(layer[source],"inf_is_valid", false);
|
||||||
clearing = loadParam(layer[source],"clearing", false);
|
clearing = loadParam(layer[source],"clearing", false);
|
||||||
marking = loadParam(layer[source],"marking", true);
|
marking = loadParam(layer[source],"marking", true);
|
||||||
|
frustum_clearing_enabled = loadParam(layer, "frustum_clearing_enabled", false);
|
||||||
|
frustum_pixel_step = loadParam(layer, "frustum_clearing_pixel_step", 8);
|
||||||
|
frustum_min_range = loadParam(layer, "frustum_min_range", 0.2);
|
||||||
|
frustum_max_range = loadParam(layer, "frustum_max_range", 3.0);
|
||||||
|
|
||||||
if (priv_nh.hasParam("topic"))
|
if (priv_nh.hasParam("topic"))
|
||||||
priv_nh.getParam("topic", topic);
|
priv_nh.getParam("topic", topic);
|
||||||
@@ -164,52 +172,83 @@ bool ObstacleLayer::getParams(const std::string& config_file_name, robot::NodeHa
|
|||||||
priv_nh.getParam("clearing", clearing);
|
priv_nh.getParam("clearing", clearing);
|
||||||
if (priv_nh.hasParam("marking"))
|
if (priv_nh.hasParam("marking"))
|
||||||
priv_nh.getParam("marking", marking);
|
priv_nh.getParam("marking", marking);
|
||||||
|
if (priv_nh.hasParam("frustum_clearing_enabled"))
|
||||||
if (!(data_type == "PointCloud2" || data_type == "PointCloud" || data_type == "LaserScan"))
|
priv_nh.getParam("frustum_clearing_enabled", frustum_clearing_enabled);
|
||||||
|
if (priv_nh.hasParam("frustum_clearing_pixel_step"))
|
||||||
{
|
{
|
||||||
robot::log_error("Only topics that use point clouds or laser scans are currently supported\n");
|
priv_nh.getParam("frustum_clearing_pixel_step", frustum_pixel_step);
|
||||||
throw std::runtime_error("Only topics that use point clouds or laser scans are currently supported");
|
frustum_pixel_step = std::max(1, frustum_pixel_step);
|
||||||
}
|
}
|
||||||
|
if (priv_nh.hasParam("frustum_min_range"))
|
||||||
|
priv_nh.getParam("frustum_min_range", frustum_min_range);
|
||||||
|
if (priv_nh.hasParam("frustum_max_range"))
|
||||||
|
priv_nh.getParam("frustum_max_range", frustum_max_range);
|
||||||
|
if (priv_nh.hasParam("frustum_depth_camera_topic"))
|
||||||
|
priv_nh.getParam("frustum_depth_camera_topic", depth_camera_data_topic_);
|
||||||
|
|
||||||
CallBackInfo info_tmp;
|
robot::log_info("frustum_clearing_enabled: %s, frustum_clearing_pixel_step: %d, frustum_min_range: %f, frustum_max_range: %f", frustum_clearing_enabled ? "true" : "false", frustum_pixel_step, frustum_min_range, frustum_max_range);
|
||||||
info_tmp.observation_source = source;
|
|
||||||
info_tmp.data_type = data_type;
|
|
||||||
info_tmp.topic = topic;
|
|
||||||
info_tmp.inf_is_valid = inf_is_valid;
|
|
||||||
callback_infos_.push_back(info_tmp);
|
|
||||||
|
|
||||||
std::string raytrace_range_param_name, obstacle_range_param_name;
|
|
||||||
|
|
||||||
double obstacle_range = 2.5;
|
double obstacle_range = 2.5;
|
||||||
obstacle_range = loadParam(layer[source],"obstacle_range", obstacle_range);
|
obstacle_range = loadParam(layer[source],"obstacle_range", obstacle_range);
|
||||||
double raytrace_range = 3.0;
|
double raytrace_range = 3.0;
|
||||||
raytrace_range = loadParam(layer[source],"raytrace_range", raytrace_range);
|
raytrace_range = loadParam(layer[source],"raytrace_range", raytrace_range);
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
if (priv_nh.hasParam("obstacle_range"))
|
if (priv_nh.hasParam("obstacle_range"))
|
||||||
priv_nh.getParam("obstacle_range", obstacle_range);
|
priv_nh.getParam("obstacle_range", obstacle_range);
|
||||||
if (priv_nh.hasParam("raytrace_range"))
|
if (priv_nh.hasParam("raytrace_range"))
|
||||||
priv_nh.getParam("raytrace_range", raytrace_range);
|
priv_nh.getParam("raytrace_range", raytrace_range);
|
||||||
|
|
||||||
// enabled_ = enabled;
|
if (!(data_type == "PointCloud2" || data_type == "PointCloud" || data_type == "LaserScan" || data_type == "DepthCameraData"))
|
||||||
|
{
|
||||||
|
robot::log_error("Only topics that use point clouds or laser scans are currently supported\n");
|
||||||
|
throw std::runtime_error("Only topics that use point clouds or laser scans are currently supported");
|
||||||
|
}
|
||||||
|
|
||||||
robot::log_info("Creating an observation buffer for topic %s, frame %s\n", topic.c_str(),
|
if(!frustum_clearing_enabled)
|
||||||
priv_nh.getNamespace().c_str());
|
{
|
||||||
|
|
||||||
// create an observation buffer
|
CallBackInfo info_tmp;
|
||||||
observation_buffers_.push_back(
|
info_tmp.observation_source = source;
|
||||||
boost::shared_ptr < ObservationBuffer
|
info_tmp.data_type = data_type;
|
||||||
> (new ObservationBuffer(topic, observation_keep_time, expected_update_rate, min_obstacle_height,
|
info_tmp.topic = topic;
|
||||||
max_obstacle_height, obstacle_range, raytrace_range, *tf_, global_frame_,
|
info_tmp.inf_is_valid = inf_is_valid;
|
||||||
sensor_frame, transform_tolerance)));
|
callback_infos_.push_back(info_tmp);
|
||||||
if (marking)
|
|
||||||
marking_buffers_.push_back(observation_buffers_.back());
|
|
||||||
|
|
||||||
// check if we'll also add this buffer to our clearing observation buffers
|
// enabled_ = enabled;
|
||||||
if (clearing)
|
|
||||||
clearing_buffers_.push_back(observation_buffers_.back());
|
|
||||||
|
|
||||||
|
robot::log_info("Creating an observation buffer for topic %s, frame %s\n", topic.c_str(),
|
||||||
|
priv_nh.getNamespace().c_str());
|
||||||
|
|
||||||
|
// create an observation buffer
|
||||||
|
observation_buffers_.push_back(
|
||||||
|
boost::shared_ptr < ObservationBuffer
|
||||||
|
> (new ObservationBuffer(topic, observation_keep_time, expected_update_rate, min_obstacle_height,
|
||||||
|
max_obstacle_height, obstacle_range, raytrace_range, *tf_, global_frame_,
|
||||||
|
sensor_frame, transform_tolerance)));
|
||||||
|
if (marking)
|
||||||
|
marking_buffers_.push_back(observation_buffers_.back());
|
||||||
|
|
||||||
|
// check if we'll also add this buffer to our clearing observation buffers
|
||||||
|
if (clearing)
|
||||||
|
clearing_buffers_.push_back(observation_buffers_.back());
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
CallBackInfo info_tmp;
|
||||||
|
info_tmp.observation_source = source;
|
||||||
|
info_tmp.data_type = data_type;
|
||||||
|
info_tmp.topic = topic;
|
||||||
|
info_tmp.inf_is_valid = inf_is_valid;
|
||||||
|
callback_depth_infos_.push_back(info_tmp);
|
||||||
|
|
||||||
|
depth_observation_buffers_.push_back(
|
||||||
|
boost::shared_ptr < ObservationBuffer
|
||||||
|
> (new ObservationBuffer(topic, observation_keep_time, expected_update_rate, min_obstacle_height,
|
||||||
|
max_obstacle_height, obstacle_range, raytrace_range, frustum_pixel_step,
|
||||||
|
frustum_min_range, frustum_max_range, *tf_, global_frame_,
|
||||||
|
sensor_frame, transform_tolerance)));
|
||||||
|
|
||||||
|
}
|
||||||
robot::log_info(
|
robot::log_info(
|
||||||
"Created an observation buffer for topic %s, global frame: %s, "
|
"Created an observation buffer for topic %s, global frame: %s, "
|
||||||
"expected update rate: %.2f, observation persistence: %.2f\n",
|
"expected update rate: %.2f, observation persistence: %.2f\n",
|
||||||
@@ -230,29 +269,93 @@ void ObstacleLayer::handleImpl(const void* data,
|
|||||||
const std::type_info& type,
|
const std::type_info& type,
|
||||||
const std::string& topic)
|
const std::string& topic)
|
||||||
{
|
{
|
||||||
if(!stop_receiving_data_)
|
if (!enabled_ || stop_receiving_data_)
|
||||||
|
return;
|
||||||
|
|
||||||
|
if (type == typeid(robot_sensor_msgs::DepthCameraData::ConstPtr))
|
||||||
{
|
{
|
||||||
if (type == typeid(robot_sensor_msgs::DepthCameraData) &&
|
const robot_sensor_msgs::DepthCameraData::ConstPtr& depth_camera_data_ptr =
|
||||||
(depth_camera_topic_.empty() || topic == depth_camera_topic_))
|
*static_cast<const robot_sensor_msgs::DepthCameraData::ConstPtr*>(data);
|
||||||
{
|
if (!depth_camera_data_ptr)
|
||||||
|
return;
|
||||||
|
|
||||||
const robot_sensor_msgs::DepthCameraData& depth_camera_data =
|
const robot_sensor_msgs::DepthCameraData& depth_camera_data =
|
||||||
*static_cast<const robot_sensor_msgs::DepthCameraData*>(data);
|
*depth_camera_data_ptr;
|
||||||
if (depth_camera_data.camera_info.K[0] <= 0.0 || depth_camera_data.camera_info.K[4] <= 0.0)
|
const robot_sensor_msgs::Image& depth = depth_camera_data.depth;
|
||||||
|
const robot_sensor_msgs::CameraInfo& camera_info = depth_camera_data.camera_info;
|
||||||
|
|
||||||
|
std::size_t bytes_per_pixel = 0;
|
||||||
|
if (depth.encoding == "16UC1" || depth.encoding == "mono16")
|
||||||
|
bytes_per_pixel = 2;
|
||||||
|
else if (depth.encoding == "32FC1")
|
||||||
|
bytes_per_pixel = 4;
|
||||||
|
else
|
||||||
|
{
|
||||||
|
robot::log_error("ObstacleLayer received unsupported depth encoding: %s\n", depth.encoding.c_str());
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
const bool invalid_dimensions = depth.width == 0 || depth.height == 0 ||
|
||||||
|
depth.step < static_cast<std::size_t>(depth.width) * bytes_per_pixel ||
|
||||||
|
depth.data.size() < static_cast<std::size_t>(depth.step) * depth.height;
|
||||||
|
if (invalid_dimensions)
|
||||||
|
{
|
||||||
|
robot::log_error("ObstacleLayer received malformed DepthCameraData image\n");
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (camera_info.K[0] <= 0.0 || camera_info.K[4] <= 0.0)
|
||||||
{
|
{
|
||||||
robot::log_error("ObstacleLayer received invalid camera intrinsics for depth clearing\n");
|
robot::log_error("ObstacleLayer received invalid camera intrinsics for depth clearing\n");
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
std::lock_guard<std::mutex> lock(depth_camera_mutex_);
|
if ((camera_info.width != 0 && camera_info.width != depth.width) ||
|
||||||
latest_depth_camera_data_ = depth_camera_data;
|
(camera_info.height != 0 && camera_info.height != depth.height))
|
||||||
have_depth_camera_data_ = true;
|
{
|
||||||
|
robot::log_error("ObstacleLayer received mismatched depth image and camera info dimensions\n");
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
const std::string& depth_frame = depth.header.frame_id;
|
||||||
|
const std::string& camera_frame = camera_info.header.frame_id;
|
||||||
|
if (!depth_frame.empty() && !camera_frame.empty() && depth_frame != camera_frame)
|
||||||
|
{
|
||||||
|
robot::log_error("ObstacleLayer received mismatched depth and camera-info frames: %s != %s\n",
|
||||||
|
depth_frame.c_str(), camera_frame.c_str());
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (depth_camera_data.header.frame_id.empty() && depth_frame.empty() && camera_frame.empty())
|
||||||
|
{
|
||||||
|
robot::log_error("ObstacleLayer received DepthCameraData without an optical frame\n");
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
// std::lock_guard<std::mutex> lock(depth_camera_data_mutex_);
|
||||||
|
// pending_depth_camera_data_ = depth_camera_data_ptr;
|
||||||
|
if (depth_observation_buffers_.empty() || callback_depth_infos_.empty())
|
||||||
|
return;
|
||||||
|
|
||||||
|
int size_callback_depth = static_cast<int>(callback_depth_infos_.size());
|
||||||
|
for(int i = 0; i < size_callback_depth; i++)
|
||||||
|
{
|
||||||
|
boost::shared_ptr<ObservationBuffer>& buffer = depth_observation_buffers_[i];
|
||||||
|
if (type == typeid(robot_sensor_msgs::DepthCameraData::ConstPtr) &&
|
||||||
|
topic == callback_depth_infos_[i].topic)
|
||||||
|
{
|
||||||
|
// robot::log_error_throttle(1.0,"TEST");
|
||||||
|
depthImageCallback(depth_camera_data_ptr, buffer);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
if (observation_buffers_.empty() || callback_infos_.empty())
|
||||||
return;
|
return;
|
||||||
}
|
|
||||||
|
|
||||||
if(observation_buffers_.empty() || callback_infos_.empty()) return;
|
|
||||||
|
|
||||||
int size_callback = static_cast<int>(callback_infos_.size());
|
int size_callback = static_cast<int>(callback_infos_.size());
|
||||||
for(int i = 0; i < size_callback; i++)
|
for (int i = 0; i < size_callback; i++)
|
||||||
{
|
{
|
||||||
boost::shared_ptr<ObservationBuffer>& buffer = observation_buffers_[i];
|
boost::shared_ptr<ObservationBuffer>& buffer = observation_buffers_[i];
|
||||||
|
|
||||||
@@ -303,11 +406,6 @@ void ObstacleLayer::handleImpl(const void* data,
|
|||||||
// }
|
// }
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
else
|
|
||||||
{
|
|
||||||
robot::log_info("Stop receiving data!\n");
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void ObstacleLayer::laserScanCallback(const robot_sensor_msgs::LaserScan& message,
|
void ObstacleLayer::laserScanCallback(const robot_sensor_msgs::LaserScan& message,
|
||||||
@@ -408,6 +506,14 @@ void ObstacleLayer::pointCloud2Callback(const robot_sensor_msgs::PointCloud2& me
|
|||||||
buffer->unlock();
|
buffer->unlock();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void ObstacleLayer::depthImageCallback(robot_sensor_msgs::DepthCameraData::ConstPtr message,
|
||||||
|
const boost::shared_ptr<ObservationBuffer>& buffer)
|
||||||
|
{
|
||||||
|
buffer->lock();
|
||||||
|
buffer->bufferDepthCamera(std::move(message));
|
||||||
|
buffer->unlock();
|
||||||
|
}
|
||||||
|
|
||||||
void ObstacleLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x,
|
void ObstacleLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x,
|
||||||
double* min_y, double* max_x, double* max_y)
|
double* min_y, double* max_x, double* max_y)
|
||||||
{
|
{
|
||||||
@@ -446,6 +552,9 @@ void ObstacleLayer::updateBounds(double robot_x, double robot_y, double robot_ya
|
|||||||
robot_sensor_msgs::PointCloud2ConstIterator<float> iter_y(cloud, "y");
|
robot_sensor_msgs::PointCloud2ConstIterator<float> iter_y(cloud, "y");
|
||||||
robot_sensor_msgs::PointCloud2ConstIterator<float> iter_z(cloud, "z");
|
robot_sensor_msgs::PointCloud2ConstIterator<float> iter_z(cloud, "z");
|
||||||
|
|
||||||
|
std::size_t rejected_height = 0;
|
||||||
|
std::size_t rejected_range = 0;
|
||||||
|
std::size_t rejected_bounds = 0;
|
||||||
for (; iter_x !=iter_x.end(); ++iter_x, ++iter_y, ++iter_z)
|
for (; iter_x !=iter_x.end(); ++iter_x, ++iter_y, ++iter_z)
|
||||||
{
|
{
|
||||||
double px = *iter_x, py = *iter_y, pz = *iter_z;
|
double px = *iter_x, py = *iter_y, pz = *iter_z;
|
||||||
@@ -453,7 +562,7 @@ void ObstacleLayer::updateBounds(double robot_x, double robot_y, double robot_ya
|
|||||||
// if the obstacle is too high or too far away from the robot we won't add it
|
// if the obstacle is too high or too far away from the robot we won't add it
|
||||||
if (pz > max_obstacle_height_)
|
if (pz > max_obstacle_height_)
|
||||||
{
|
{
|
||||||
robot::log_error("The point is too high\n");
|
++rejected_height;
|
||||||
continue;
|
continue;
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -464,7 +573,7 @@ void ObstacleLayer::updateBounds(double robot_x, double robot_y, double robot_ya
|
|||||||
// if the point is far enough away... we won't consider it
|
// if the point is far enough away... we won't consider it
|
||||||
if (sq_dist >= sq_obstacle_range)
|
if (sq_dist >= sq_obstacle_range)
|
||||||
{
|
{
|
||||||
robot::log_error("The point is too far away\n");
|
++rejected_range;
|
||||||
continue;
|
continue;
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -472,7 +581,7 @@ void ObstacleLayer::updateBounds(double robot_x, double robot_y, double robot_ya
|
|||||||
unsigned int mx, my;
|
unsigned int mx, my;
|
||||||
if (!worldToMap(px, py, mx, my))
|
if (!worldToMap(px, py, mx, my))
|
||||||
{
|
{
|
||||||
robot::log_error("Computing map coords failed\n");
|
++rejected_bounds;
|
||||||
continue;
|
continue;
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -480,6 +589,14 @@ void ObstacleLayer::updateBounds(double robot_x, double robot_y, double robot_ya
|
|||||||
costmap_[index] = LETHAL_OBSTACLE;
|
costmap_[index] = LETHAL_OBSTACLE;
|
||||||
touch(px, py, min_x, min_y, max_x, max_y);
|
touch(px, py, min_x, min_y, max_x, max_y);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
if (rejected_height + rejected_range + rejected_bounds > 0)
|
||||||
|
{
|
||||||
|
robot::log_info_throttle(
|
||||||
|
5.0,
|
||||||
|
"ObstacleLayer filtered points: height=%zu range=%zu outside_map=%zu\n",
|
||||||
|
rejected_height, rejected_range, rejected_bounds);
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
updateFootprint(robot_x, robot_y, robot_yaw, min_x, min_y, max_x, max_y);
|
updateFootprint(robot_x, robot_y, robot_yaw, min_x, min_y, max_x, max_y);
|
||||||
@@ -566,6 +683,23 @@ bool ObstacleLayer::getClearingObservations(std::vector<Observation>& clearing_o
|
|||||||
return current;
|
return current;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
bool ObstacleLayer::getFrustumClearingObservations(std::vector<DepthCameraObservation>& frustum_clearing_observations) const
|
||||||
|
{
|
||||||
|
bool current = true;
|
||||||
|
// DepthCameraObservation depth_obs;
|
||||||
|
|
||||||
|
for (const boost::shared_ptr<ObservationBuffer>& buffer : depth_observation_buffers_)
|
||||||
|
{
|
||||||
|
buffer->lock();
|
||||||
|
buffer->getDepthObservations(frustum_clearing_observations);
|
||||||
|
current = buffer->isCurrent() && current;
|
||||||
|
buffer->unlock();
|
||||||
|
// frustum_clearing_observations.push_back(depth_obs);
|
||||||
|
}
|
||||||
|
|
||||||
|
return current;
|
||||||
|
}
|
||||||
|
|
||||||
void ObstacleLayer::raytraceFreespace(const Observation& clearing_observation, double* min_x, double* min_y,
|
void ObstacleLayer::raytraceFreespace(const Observation& clearing_observation, double* min_x, double* min_y,
|
||||||
double* max_x, double* max_y)
|
double* max_x, double* max_y)
|
||||||
{
|
{
|
||||||
|
|||||||
@@ -98,11 +98,8 @@ bool VoxelLayer::getParams(const std::string& config_file_name, robot::NodeHandl
|
|||||||
unknown_threshold_ = loadParam(layer, "unknown_threshold", 15.0) + (VOXEL_BITS - size_z_);
|
unknown_threshold_ = loadParam(layer, "unknown_threshold", 15.0) + (VOXEL_BITS - size_z_);
|
||||||
mark_threshold_ = loadParam(layer, "mark_threshold", 0);
|
mark_threshold_ = loadParam(layer, "mark_threshold", 0);
|
||||||
combination_method_ = loadParam(layer, "combination_method", 0.0);
|
combination_method_ = loadParam(layer, "combination_method", 0.0);
|
||||||
frustum_clearing_enabled_ = loadParam(layer, "frustum_clearing_enabled", false);
|
obstacle_decay_time_ = loadParam(layer, "obstacle_decay_time", 0.0);
|
||||||
frustum_clearing_pixel_step_ = loadParam(layer, "frustum_clearing_pixel_step", 8);
|
frustum_skip_distance_ = loadParam(layer, "frustum_skip_distance", -1.0);
|
||||||
frustum_min_range_ = loadParam(layer, "frustum_min_range", 0.2);
|
|
||||||
frustum_max_range_ = loadParam(layer, "frustum_max_range", 3.0);
|
|
||||||
frustum_depth_camera_topic_ = loadParam(layer, "frustum_depth_camera_topic", std::string("/camera/depth/image_raw"));
|
|
||||||
|
|
||||||
int size_z, unknown_threshold, mark_threshold, frustum_pixel_step;
|
int size_z, unknown_threshold, mark_threshold, frustum_pixel_step;
|
||||||
if (nh.hasParam("enabled"))
|
if (nh.hasParam("enabled"))
|
||||||
@@ -130,25 +127,18 @@ bool VoxelLayer::getParams(const std::string& config_file_name, robot::NodeHandl
|
|||||||
}
|
}
|
||||||
if (nh.hasParam("combination_method"))
|
if (nh.hasParam("combination_method"))
|
||||||
nh.getParam("combination_method", combination_method_);
|
nh.getParam("combination_method", combination_method_);
|
||||||
if (nh.hasParam("frustum_clearing_enabled"))
|
if (nh.hasParam("obstacle_decay_time"))
|
||||||
nh.getParam("frustum_clearing_enabled", frustum_clearing_enabled_);
|
nh.getParam("obstacle_decay_time", obstacle_decay_time_);
|
||||||
if (nh.hasParam("frustum_clearing_pixel_step"))
|
if (nh.hasParam("frustum_skip_distance"))
|
||||||
{
|
nh.getParam("frustum_skip_distance", frustum_skip_distance_);
|
||||||
nh.getParam("frustum_clearing_pixel_step", frustum_pixel_step);
|
|
||||||
frustum_clearing_pixel_step_ = std::max(1, frustum_pixel_step);
|
|
||||||
}
|
|
||||||
if (nh.hasParam("frustum_min_range"))
|
|
||||||
nh.getParam("frustum_min_range", frustum_min_range_);
|
|
||||||
if (nh.hasParam("frustum_max_range"))
|
|
||||||
nh.getParam("frustum_max_range", frustum_max_range_);
|
|
||||||
if (nh.hasParam("frustum_depth_camera_topic"))
|
|
||||||
nh.getParam("frustum_depth_camera_topic", frustum_depth_camera_topic_);
|
|
||||||
else if (nh.hasParam("frustum_depth_topic"))
|
|
||||||
nh.getParam("frustum_depth_topic", frustum_depth_camera_topic_);
|
|
||||||
|
|
||||||
frustum_clearing_pixel_step_ = std::max(1u, frustum_clearing_pixel_step_);
|
if (obstacle_decay_time_ > 0.0)
|
||||||
frustum_max_range_ = std::max(frustum_max_range_, frustum_min_range_ + resolution_);
|
{
|
||||||
depth_camera_topic_ = frustum_depth_camera_topic_;
|
robot::log_info(
|
||||||
|
"VoxelLayer obstacle decay enabled: LETHAL cells not re-observed for %.1f s are freed. "
|
||||||
|
"Intended for camera-only costmaps; decay clears obstacles outside the current FOV.\n",
|
||||||
|
obstacle_decay_time_);
|
||||||
|
}
|
||||||
|
|
||||||
this->matchSize();
|
this->matchSize();
|
||||||
}
|
}
|
||||||
@@ -164,6 +154,7 @@ void VoxelLayer::matchSize()
|
|||||||
{
|
{
|
||||||
ObstacleLayer::matchSize();
|
ObstacleLayer::matchSize();
|
||||||
robot_voxel_grid_.resize(size_x_, size_y_, size_z_);
|
robot_voxel_grid_.resize(size_x_, size_y_, size_z_);
|
||||||
|
cell_last_marked_.assign(static_cast<std::size_t>(size_x_) * size_y_, 0.0);
|
||||||
if (!(robot_voxel_grid_.sizeX() == size_x_ && robot_voxel_grid_.sizeY() == size_y_))
|
if (!(robot_voxel_grid_.sizeX() == size_x_ && robot_voxel_grid_.sizeY() == size_y_))
|
||||||
{
|
{
|
||||||
std::cerr << "[FATAL] Voxel grid size mismatch: "
|
std::cerr << "[FATAL] Voxel grid size mismatch: "
|
||||||
@@ -185,6 +176,7 @@ void VoxelLayer::resetMaps()
|
|||||||
{
|
{
|
||||||
Costmap2D::resetMaps();
|
Costmap2D::resetMaps();
|
||||||
robot_voxel_grid_.reset();
|
robot_voxel_grid_.reset();
|
||||||
|
cell_last_marked_.assign(static_cast<std::size_t>(size_x_) * size_y_, 0.0);
|
||||||
}
|
}
|
||||||
|
|
||||||
void VoxelLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x,
|
void VoxelLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x,
|
||||||
@@ -196,6 +188,7 @@ void VoxelLayer::updateBounds(double robot_x, double robot_y, double robot_yaw,
|
|||||||
|
|
||||||
bool current = true;
|
bool current = true;
|
||||||
std::vector<Observation> observations, clearing_observations;
|
std::vector<Observation> observations, clearing_observations;
|
||||||
|
std::vector<DepthCameraObservation> depth_observations;
|
||||||
|
|
||||||
// get the marking observations
|
// get the marking observations
|
||||||
current = getMarkingObservations(observations) && current;
|
current = getMarkingObservations(observations) && current;
|
||||||
@@ -203,11 +196,15 @@ void VoxelLayer::updateBounds(double robot_x, double robot_y, double robot_yaw,
|
|||||||
// get the clearing observations
|
// get the clearing observations
|
||||||
current = getClearingObservations(clearing_observations) && current;
|
current = getClearingObservations(clearing_observations) && current;
|
||||||
|
|
||||||
|
current = getFrustumClearingObservations(depth_observations) && current;
|
||||||
|
|
||||||
// update the global current status
|
// update the global current status
|
||||||
current_ = current;
|
current_ = current;
|
||||||
|
|
||||||
if (frustum_clearing_enabled_)
|
for (const DepthCameraObservation& depth_observation : depth_observations)
|
||||||
raytraceDepthFrustum(min_x, min_y, max_x, max_y);
|
{
|
||||||
|
raytraceDepthFrustum(depth_observation, min_x, min_y, max_x, max_y);
|
||||||
|
}
|
||||||
|
|
||||||
// raytrace freespace
|
// raytrace freespace
|
||||||
for (unsigned int i = 0; i < clearing_observations.size(); ++i)
|
for (unsigned int i = 0; i < clearing_observations.size(); ++i)
|
||||||
@@ -216,6 +213,8 @@ void VoxelLayer::updateBounds(double robot_x, double robot_y, double robot_yaw,
|
|||||||
}
|
}
|
||||||
|
|
||||||
// place the new obstacles into a priority queue... each with a priority of zero to begin with
|
// place the new obstacles into a priority queue... each with a priority of zero to begin with
|
||||||
|
const double now_sec =
|
||||||
|
obstacle_decay_time_ > 0.0 ? robot::Time::now().toSec() : 0.0;
|
||||||
for (std::vector<Observation>::const_iterator it = observations.begin(); it != observations.end(); ++it)
|
for (std::vector<Observation>::const_iterator it = observations.begin(); it != observations.end(); ++it)
|
||||||
{
|
{
|
||||||
const Observation& obs = *it;
|
const Observation& obs = *it;
|
||||||
@@ -261,36 +260,56 @@ void VoxelLayer::updateBounds(double robot_x, double robot_y, double robot_yaw,
|
|||||||
unsigned int index = getIndex(mx, my);
|
unsigned int index = getIndex(mx, my);
|
||||||
|
|
||||||
costmap_[index] = LETHAL_OBSTACLE;
|
costmap_[index] = LETHAL_OBSTACLE;
|
||||||
|
if (obstacle_decay_time_ > 0.0)
|
||||||
|
cell_last_marked_[index] = now_sec;
|
||||||
touch(double(*iter_x), double(*iter_y), min_x, min_y, max_x, max_y);
|
touch(double(*iter_x), double(*iter_y), min_x, min_y, max_x, max_y);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// if (publish_voxel_)
|
if (obstacle_decay_time_ > 0.0)
|
||||||
// {
|
decayStaleObstacles(now_sec, min_x, min_y, max_x, max_y);
|
||||||
// robot_costmap_2d::VoxelGrid grid_msg;
|
|
||||||
// unsigned int size = robot_voxel_grid_.sizeX() * robot_voxel_grid_.sizeY();
|
|
||||||
// grid_msg.size_x = robot_voxel_grid_.sizeX();
|
|
||||||
// grid_msg.size_y = robot_voxel_grid_.sizeY();
|
|
||||||
// grid_msg.size_z = robot_voxel_grid_.sizeZ();
|
|
||||||
// grid_msg.data.resize(size);
|
|
||||||
// memcpy(&grid_msg.data[0], robot_voxel_grid_.getData(), size * sizeof(unsigned int));
|
|
||||||
|
|
||||||
// grid_msg.origin.x = origin_x_;
|
|
||||||
// grid_msg.origin.y = origin_y_;
|
|
||||||
// grid_msg.origin.z = origin_z_;
|
|
||||||
|
|
||||||
// grid_msg.resolutions.x = resolution_;
|
|
||||||
// grid_msg.resolutions.y = resolution_;
|
|
||||||
// grid_msg.resolutions.z = z_resolution_;
|
|
||||||
// grid_msg.header.frame_id = global_frame_;
|
|
||||||
// grid_msg.header.stamp = robot::Time::now();
|
|
||||||
// voxel_pub_.publish(grid_msg);
|
|
||||||
// }
|
|
||||||
|
|
||||||
updateFootprint(robot_x, robot_y, robot_yaw, min_x, min_y, max_x, max_y);
|
updateFootprint(robot_x, robot_y, robot_yaw, min_x, min_y, max_x, max_y);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void VoxelLayer::decayStaleObstacles(double now_sec, double* min_x, double* min_y,
|
||||||
|
double* max_x, double* max_y)
|
||||||
|
{
|
||||||
|
const std::size_t map_size = static_cast<std::size_t>(size_x_) * size_y_;
|
||||||
|
if (cell_last_marked_.size() != map_size)
|
||||||
|
cell_last_marked_.assign(map_size, 0.0);
|
||||||
|
|
||||||
|
for (std::size_t index = 0; index < map_size; ++index)
|
||||||
|
{
|
||||||
|
if (costmap_[index] != LETHAL_OBSTACLE)
|
||||||
|
continue;
|
||||||
|
|
||||||
|
double& stamp = cell_last_marked_[index];
|
||||||
|
// A LETHAL cell without a stamp was marked before decay tracking covered
|
||||||
|
// it (e.g. right after a resize). Start its timer now instead of freeing
|
||||||
|
// an obstacle we have no age evidence for.
|
||||||
|
if (stamp <= 0.0)
|
||||||
|
{
|
||||||
|
stamp = now_sec;
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (now_sec - stamp <= obstacle_decay_time_)
|
||||||
|
continue;
|
||||||
|
|
||||||
|
costmap_[index] = FREE_SPACE;
|
||||||
|
robot_voxel_grid_.clearVoxelColumn(static_cast<unsigned int>(index));
|
||||||
|
stamp = 0.0;
|
||||||
|
|
||||||
|
unsigned int mx, my;
|
||||||
|
indexToCells(static_cast<unsigned int>(index), mx, my);
|
||||||
|
double wx, wy;
|
||||||
|
mapToWorld(mx, my, wx, wy);
|
||||||
|
touch(wx, wy, min_x, min_y, max_x, max_y);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
void VoxelLayer::clearNonLethal(double wx, double wy, double w_size_x, double w_size_y, bool clear_no_info)
|
void VoxelLayer::clearNonLethal(double wx, double wy, double w_size_x, double w_size_y, bool clear_no_info)
|
||||||
{
|
{
|
||||||
// get the cell coordinates of the center point of the window
|
// get the cell coordinates of the center point of the window
|
||||||
@@ -361,14 +380,6 @@ void VoxelLayer::raytraceFreespace(const Observation& clearing_observation, doub
|
|||||||
ox, oy, oz);
|
ox, oy, oz);
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
// bool publish_clearing_points = (clearing_endpoints_pub_.getNumSubscribers() > 0);
|
|
||||||
// if (publish_clearing_points)
|
|
||||||
// {
|
|
||||||
// clearing_endpoints_.points.clear();
|
|
||||||
// clearing_endpoints_.points.reserve(clearing_observation_cloud_size);
|
|
||||||
// }
|
|
||||||
|
|
||||||
// we can pre-compute the enpoints of the map outside of the inner loop... we'll need these later
|
// we can pre-compute the enpoints of the map outside of the inner loop... we'll need these later
|
||||||
double map_end_x = origin_x_ + getSizeInMetersX();
|
double map_end_x = origin_x_ + getSizeInMetersX();
|
||||||
double map_end_y = origin_y_ + getSizeInMetersY();
|
double map_end_y = origin_y_ + getSizeInMetersY();
|
||||||
@@ -443,26 +454,8 @@ void VoxelLayer::raytraceFreespace(const Observation& clearing_observation, doub
|
|||||||
cell_raytrace_range);
|
cell_raytrace_range);
|
||||||
|
|
||||||
updateRaytraceBounds(ox, oy, wpx, wpy, clearing_observation.raytrace_range_, min_x, min_y, max_x, max_y);
|
updateRaytraceBounds(ox, oy, wpx, wpy, clearing_observation.raytrace_range_, min_x, min_y, max_x, max_y);
|
||||||
|
|
||||||
// if (publish_clearing_points)
|
|
||||||
// {
|
|
||||||
// robot_geometry_msgs::Point32 point;
|
|
||||||
// point.x = wpx;
|
|
||||||
// point.y = wpy;
|
|
||||||
// point.z = wpz;
|
|
||||||
// clearing_endpoints_.points.push_back(point);
|
|
||||||
// }
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// if (publish_clearing_points)
|
|
||||||
// {
|
|
||||||
// clearing_endpoints_.header.frame_id = global_frame_;
|
|
||||||
// clearing_endpoints_.header.stamp = clearing_observation.cloud_->header.stamp;
|
|
||||||
// clearing_endpoints_.header.seq = clearing_observation.cloud_->header.seq;
|
|
||||||
|
|
||||||
// clearing_endpoints_pub_.publish(clearing_endpoints_);
|
|
||||||
// }
|
|
||||||
}
|
}
|
||||||
|
|
||||||
bool VoxelLayer::readDepthMeters(const robot_sensor_msgs::Image& depth, unsigned int u, unsigned int v,
|
bool VoxelLayer::readDepthMeters(const robot_sensor_msgs::Image& depth, unsigned int u, unsigned int v,
|
||||||
@@ -524,6 +517,47 @@ bool VoxelLayer::readDepthMeters(const robot_sensor_msgs::Image& depth, unsigned
|
|||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void VoxelLayer::updateDepthRayCache(unsigned int width, unsigned int height,
|
||||||
|
unsigned int pixel_step, double fx, double fy,
|
||||||
|
double cx, double cy,
|
||||||
|
unsigned int u_offset, unsigned int v_offset)
|
||||||
|
{
|
||||||
|
if (cached_depth_width_ == width && cached_depth_height_ == height &&
|
||||||
|
cached_depth_pixel_step_ == pixel_step && cached_fx_ == fx && cached_fy_ == fy &&
|
||||||
|
cached_cx_ == cx && cached_cy_ == cy &&
|
||||||
|
cached_u_offset_ == u_offset && cached_v_offset_ == v_offset)
|
||||||
|
{
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
cached_depth_width_ = width;
|
||||||
|
cached_depth_height_ = height;
|
||||||
|
cached_depth_pixel_step_ = pixel_step;
|
||||||
|
cached_fx_ = fx;
|
||||||
|
cached_fy_ = fy;
|
||||||
|
cached_cx_ = cx;
|
||||||
|
cached_cy_ = cy;
|
||||||
|
cached_u_offset_ = u_offset;
|
||||||
|
cached_v_offset_ = v_offset;
|
||||||
|
|
||||||
|
const std::size_t rows = (height + pixel_step - 1) / pixel_step;
|
||||||
|
const std::size_t columns = (width + pixel_step - 1) / pixel_step;
|
||||||
|
depth_ray_cache_.clear();
|
||||||
|
depth_ray_cache_.reserve(rows * columns);
|
||||||
|
|
||||||
|
for (unsigned int v = v_offset; v < height; v += pixel_step)
|
||||||
|
{
|
||||||
|
for (unsigned int u = u_offset; u < width; u += pixel_step)
|
||||||
|
{
|
||||||
|
const double x = (static_cast<double>(u) - cx) / fx;
|
||||||
|
const double y = (static_cast<double>(v) - cy) / fy;
|
||||||
|
const double inverse_norm = 1.0 / std::sqrt(x * x + y * y + 1.0);
|
||||||
|
depth_ray_cache_.push_back(
|
||||||
|
DepthRay{u, v, x * inverse_norm, y * inverse_norm, inverse_norm});
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
bool VoxelLayer::clipRaytraceEndpoint(double ox, double oy, double oz, double& wx, double& wy, double& wz)
|
bool VoxelLayer::clipRaytraceEndpoint(double ox, double oy, double oz, double& wx, double& wy, double& wz)
|
||||||
{
|
{
|
||||||
double a = wx - ox;
|
double a = wx - ox;
|
||||||
@@ -562,7 +596,8 @@ bool VoxelLayer::clipRaytraceEndpoint(double ox, double oy, double oz, double& w
|
|||||||
}
|
}
|
||||||
|
|
||||||
bool VoxelLayer::clearVoxelRay(double ox, double oy, double oz, double wx, double wy, double wz,
|
bool VoxelLayer::clearVoxelRay(double ox, double oy, double oz, double wx, double wy, double wz,
|
||||||
double raytrace_range, double* min_x, double* min_y, double* max_x, double* max_y)
|
double raytrace_range, unsigned int cell_raytrace_range,
|
||||||
|
double* min_x, double* min_y, double* max_x, double* max_y)
|
||||||
{
|
{
|
||||||
double sensor_x, sensor_y, sensor_z;
|
double sensor_x, sensor_y, sensor_z;
|
||||||
if (!worldToMap3DFloat(ox, oy, oz, sensor_x, sensor_y, sensor_z))
|
if (!worldToMap3DFloat(ox, oy, oz, sensor_x, sensor_y, sensor_z))
|
||||||
@@ -577,21 +612,18 @@ bool VoxelLayer::clearVoxelRay(double ox, double oy, double oz, double wx, doubl
|
|||||||
|
|
||||||
robot_voxel_grid_.clearVoxelLineInMap(sensor_x, sensor_y, sensor_z, point_x, point_y, point_z, costmap_,
|
robot_voxel_grid_.clearVoxelLineInMap(sensor_x, sensor_y, sensor_z, point_x, point_y, point_z, costmap_,
|
||||||
unknown_threshold_, mark_threshold_, FREE_SPACE, NO_INFORMATION,
|
unknown_threshold_, mark_threshold_, FREE_SPACE, NO_INFORMATION,
|
||||||
cellDistance(raytrace_range));
|
cell_raytrace_range);
|
||||||
updateRaytraceBounds(ox, oy, wx, wy, raytrace_range, min_x, min_y, max_x, max_y);
|
updateRaytraceBounds(ox, oy, wx, wy, raytrace_range, min_x, min_y, max_x, max_y);
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
bool VoxelLayer::raytraceDepthFrustum(double* min_x, double* min_y, double* max_x, double* max_y)
|
bool VoxelLayer::raytraceDepthFrustum(const DepthCameraObservation& observation,
|
||||||
|
double* min_x, double* min_y, double* max_x, double* max_y)
|
||||||
{
|
{
|
||||||
robot_sensor_msgs::DepthCameraData depth_camera_data;
|
if (!observation.data_)
|
||||||
{
|
return false;
|
||||||
std::lock_guard<std::mutex> lock(depth_camera_mutex_);
|
|
||||||
if (!have_depth_camera_data_)
|
|
||||||
return false;
|
|
||||||
depth_camera_data = latest_depth_camera_data_;
|
|
||||||
}
|
|
||||||
|
|
||||||
|
const robot_sensor_msgs::DepthCameraData& depth_camera_data = *observation.data_;
|
||||||
const robot_sensor_msgs::Image& depth = depth_camera_data.depth;
|
const robot_sensor_msgs::Image& depth = depth_camera_data.depth;
|
||||||
const robot_sensor_msgs::CameraInfo& camera_info = depth_camera_data.camera_info;
|
const robot_sensor_msgs::CameraInfo& camera_info = depth_camera_data.camera_info;
|
||||||
|
|
||||||
@@ -605,13 +637,17 @@ bool VoxelLayer::raytraceDepthFrustum(double* min_x, double* min_y, double* max_
|
|||||||
if (fx <= 0.0 || fy <= 0.0)
|
if (fx <= 0.0 || fy <= 0.0)
|
||||||
return false;
|
return false;
|
||||||
|
|
||||||
const std::string depth_frame = depth.header.frame_id.empty() ? camera_info.header.frame_id : depth.header.frame_id;
|
std::string depth_frame = depth.header.frame_id.empty() ? depth_camera_data.header.frame_id : depth.header.frame_id;
|
||||||
|
if (depth_frame.empty())
|
||||||
|
depth_frame = camera_info.header.frame_id;
|
||||||
if (depth_frame.empty() || tf_ == nullptr)
|
if (depth_frame.empty() || tf_ == nullptr)
|
||||||
return false;
|
return false;
|
||||||
|
|
||||||
robot_geometry_msgs::PointStamped local_origin;
|
robot_geometry_msgs::PointStamped local_origin;
|
||||||
local_origin.header = depth.header;
|
local_origin.header = depth.header;
|
||||||
local_origin.header.frame_id = depth_frame;
|
local_origin.header.frame_id = depth_frame;
|
||||||
|
if (local_origin.header.stamp.isZero())
|
||||||
|
local_origin.header.stamp = depth_camera_data.header.stamp;
|
||||||
local_origin.point.x = 0.0;
|
local_origin.point.x = 0.0;
|
||||||
local_origin.point.y = 0.0;
|
local_origin.point.y = 0.0;
|
||||||
local_origin.point.z = 0.0;
|
local_origin.point.z = 0.0;
|
||||||
@@ -625,8 +661,9 @@ bool VoxelLayer::raytraceDepthFrustum(double* min_x, double* min_y, double* max_
|
|||||||
}
|
}
|
||||||
catch (tf3::TransformException& ex)
|
catch (tf3::TransformException& ex)
|
||||||
{
|
{
|
||||||
robot::log_error("VoxelLayer frustum clearing TF exception from %s to %s: %s\n",
|
robot::log_error_throttle(
|
||||||
depth_frame.c_str(), global_frame_.c_str(), ex.what());
|
5.0, "VoxelLayer depth topic [%s] TF exception from %s to %s: %s\n",
|
||||||
|
observation.topic_.c_str(), depth_frame.c_str(), global_frame_.c_str(), ex.what());
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -637,66 +674,82 @@ bool VoxelLayer::raytraceDepthFrustum(double* min_x, double* min_y, double* max_
|
|||||||
double sensor_x, sensor_y, sensor_z;
|
double sensor_x, sensor_y, sensor_z;
|
||||||
if (!worldToMap3DFloat(ox, oy, oz, sensor_x, sensor_y, sensor_z))
|
if (!worldToMap3DFloat(ox, oy, oz, sensor_x, sensor_y, sensor_z))
|
||||||
{
|
{
|
||||||
robot::log_error(
|
robot::log_error_throttle(
|
||||||
"The origin for the depth sensor at (%.2f, %.2f, %.2f) is out of map bounds. So, the costmap cannot frustum-clear for it.\n",
|
5.0, "VoxelLayer depth topic [%s] origin at (%.2f, %.2f, %.2f) is outside the voxel map\n",
|
||||||
ox, oy, oz);
|
observation.topic_.c_str(), ox, oy, oz);
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
const unsigned int step = std::max(1u, frustum_clearing_pixel_step_);
|
const unsigned int step = std::max(1u, observation.pixel_step_);
|
||||||
const double skip_dist = 2.0 * resolution_;
|
const double min_range = observation.min_range_;
|
||||||
|
const double max_range = observation.max_range_;
|
||||||
|
const double skip_dist =
|
||||||
|
frustum_skip_distance_ >= 0.0 ? frustum_skip_distance_ : 2.0 * resolution_;
|
||||||
const unsigned int width = std::min(depth.width, camera_info.width == 0 ? depth.width : camera_info.width);
|
const unsigned int width = std::min(depth.width, camera_info.width == 0 ? depth.width : camera_info.width);
|
||||||
const unsigned int height = std::min(depth.height, camera_info.height == 0 ? depth.height : camera_info.height);
|
const unsigned int height = std::min(depth.height, camera_info.height == 0 ? depth.height : camera_info.height);
|
||||||
|
|
||||||
|
// Dither the sampled pixel grid every pass. With a fixed subgrid a static
|
||||||
|
// camera re-traces identical rays each frame; cells between adjacent rays
|
||||||
|
// (angular gap step / fx, ~5 cm at 2.5 m for step 8) are never crossed and
|
||||||
|
// stay marked until the robot moves. Cycling the phase sweeps every pixel
|
||||||
|
// column/row over step^2 frames at the same per-frame ray count.
|
||||||
|
const unsigned int u_offset = step > 1 ? frustum_phase_ % step : 0;
|
||||||
|
const unsigned int v_offset = step > 1 ? (frustum_phase_ / step) % step : 0;
|
||||||
|
++frustum_phase_;
|
||||||
|
updateDepthRayCache(width, height, step, fx, fy, cx, cy, u_offset, v_offset);
|
||||||
|
|
||||||
|
double qx = tfm.transform.rotation.x;
|
||||||
|
double qy = tfm.transform.rotation.y;
|
||||||
|
double qz = tfm.transform.rotation.z;
|
||||||
|
double qw = tfm.transform.rotation.w;
|
||||||
|
const double quaternion_norm = std::sqrt(qx * qx + qy * qy + qz * qz + qw * qw);
|
||||||
|
if (quaternion_norm <= 0.0)
|
||||||
|
return false;
|
||||||
|
qx /= quaternion_norm;
|
||||||
|
qy /= quaternion_norm;
|
||||||
|
qz /= quaternion_norm;
|
||||||
|
qw /= quaternion_norm;
|
||||||
|
|
||||||
|
const double r00 = 1.0 - 2.0 * (qy * qy + qz * qz);
|
||||||
|
const double r01 = 2.0 * (qx * qy - qz * qw);
|
||||||
|
const double r02 = 2.0 * (qx * qz + qy * qw);
|
||||||
|
const double r10 = 2.0 * (qx * qy + qz * qw);
|
||||||
|
const double r11 = 1.0 - 2.0 * (qx * qx + qz * qz);
|
||||||
|
const double r12 = 2.0 * (qy * qz - qx * qw);
|
||||||
|
const double r20 = 2.0 * (qx * qz - qy * qw);
|
||||||
|
const double r21 = 2.0 * (qy * qz + qx * qw);
|
||||||
|
const double r22 = 1.0 - 2.0 * (qx * qx + qy * qy);
|
||||||
|
const unsigned int cell_raytrace_range = cellDistance(max_range);
|
||||||
bool cleared_any = false;
|
bool cleared_any = false;
|
||||||
|
|
||||||
for (unsigned int v = 0; v < height; v += step)
|
for (const DepthRay& local_ray : depth_ray_cache_)
|
||||||
{
|
{
|
||||||
for (unsigned int u = 0; u < width; u += step)
|
double depth_m = 0.0;
|
||||||
{
|
bool valid = false;
|
||||||
double depth_m = 0.0;
|
if (!readDepthMeters(depth, local_ray.u, local_ray.v, depth_m, valid))
|
||||||
bool valid = false;
|
continue;
|
||||||
if (!readDepthMeters(depth, u, v, depth_m, valid))
|
|
||||||
continue;
|
|
||||||
|
|
||||||
double ray_len = frustum_max_range_;
|
double ray_len = max_range;
|
||||||
if (valid && depth_m < frustum_max_range_)
|
if (valid && depth_m < max_range)
|
||||||
ray_len = std::max(0.0, depth_m - skip_dist);
|
ray_len = std::max(0.0, depth_m - skip_dist);
|
||||||
|
|
||||||
if (ray_len <= frustum_min_range_)
|
if (ray_len <= min_range)
|
||||||
continue;
|
continue;
|
||||||
|
|
||||||
double dx = (static_cast<double>(u) - cx) / fx;
|
robot_geometry_msgs::Vector3 global_ray;
|
||||||
double dy = (static_cast<double>(v) - cy) / fy;
|
global_ray.x = r00 * local_ray.x + r01 * local_ray.y + r02 * local_ray.z;
|
||||||
double dz = 1.0;
|
global_ray.y = r10 * local_ray.x + r11 * local_ray.y + r12 * local_ray.z;
|
||||||
const double norm = std::sqrt(dx * dx + dy * dy + dz * dz);
|
global_ray.z = r20 * local_ray.x + r21 * local_ray.y + r22 * local_ray.z;
|
||||||
if (norm <= 0.0)
|
|
||||||
continue;
|
|
||||||
|
|
||||||
robot_geometry_msgs::Vector3 local_ray;
|
const double sx = ox + global_ray.x * min_range;
|
||||||
local_ray.x = dx / norm;
|
const double sy = oy + global_ray.y * min_range;
|
||||||
local_ray.y = dy / norm;
|
const double sz = oz + global_ray.z * min_range;
|
||||||
local_ray.z = dz / norm;
|
const double wx = ox + global_ray.x * ray_len;
|
||||||
|
const double wy = oy + global_ray.y * ray_len;
|
||||||
|
const double wz = oz + global_ray.z * ray_len;
|
||||||
|
|
||||||
robot_geometry_msgs::Vector3 global_ray;
|
cleared_any = clearVoxelRay(sx, sy, sz, wx, wy, wz, ray_len, cell_raytrace_range,
|
||||||
tf3::doTransform(local_ray, global_ray, tfm);
|
min_x, min_y, max_x, max_y) || cleared_any;
|
||||||
const double global_norm =
|
|
||||||
std::sqrt(global_ray.x * global_ray.x + global_ray.y * global_ray.y + global_ray.z * global_ray.z);
|
|
||||||
if (global_norm <= 0.0)
|
|
||||||
continue;
|
|
||||||
|
|
||||||
global_ray.x /= global_norm;
|
|
||||||
global_ray.y /= global_norm;
|
|
||||||
global_ray.z /= global_norm;
|
|
||||||
|
|
||||||
const double sx = ox + global_ray.x * frustum_min_range_;
|
|
||||||
const double sy = oy + global_ray.y * frustum_min_range_;
|
|
||||||
const double sz = oz + global_ray.z * frustum_min_range_;
|
|
||||||
const double wx = ox + global_ray.x * ray_len;
|
|
||||||
const double wy = oy + global_ray.y * ray_len;
|
|
||||||
const double wz = oz + global_ray.z * ray_len;
|
|
||||||
|
|
||||||
cleared_any = clearVoxelRay(sx, sy, sz, wx, wy, wz, ray_len, min_x, min_y, max_x, max_y) || cleared_any;
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
|
||||||
return cleared_any;
|
return cleared_any;
|
||||||
@@ -709,6 +762,11 @@ void VoxelLayer::updateOrigin(double new_origin_x, double new_origin_y)
|
|||||||
cell_ox = int((new_origin_x - origin_x_) / resolution_);
|
cell_ox = int((new_origin_x - origin_x_) / resolution_);
|
||||||
cell_oy = int((new_origin_y - origin_y_) / resolution_);
|
cell_oy = int((new_origin_y - origin_y_) / resolution_);
|
||||||
|
|
||||||
|
// Most update cycles do not cross a costmap cell boundary. Avoid copying and
|
||||||
|
// resetting the complete 2D/3D grids when the cell-aligned origin is unchanged.
|
||||||
|
if (cell_ox == 0 && cell_oy == 0)
|
||||||
|
return;
|
||||||
|
|
||||||
// compute the associated world coordinates for the origin cell
|
// compute the associated world coordinates for the origin cell
|
||||||
// beacuase we want to keep things grid-aligned
|
// beacuase we want to keep things grid-aligned
|
||||||
double new_grid_ox, new_grid_oy;
|
double new_grid_ox, new_grid_oy;
|
||||||
@@ -729,15 +787,24 @@ void VoxelLayer::updateOrigin(double new_origin_x, double new_origin_y)
|
|||||||
unsigned int cell_size_x = upper_right_x - lower_left_x;
|
unsigned int cell_size_x = upper_right_x - lower_left_x;
|
||||||
unsigned int cell_size_y = upper_right_y - lower_left_y;
|
unsigned int cell_size_y = upper_right_y - lower_left_y;
|
||||||
|
|
||||||
// we need a map to store the obstacles in the window temporarily
|
const std::size_t overlap_size = static_cast<std::size_t>(cell_size_x) * cell_size_y;
|
||||||
unsigned char* local_map = new unsigned char[cell_size_x * cell_size_y];
|
rolling_costmap_scratch_.resize(overlap_size);
|
||||||
unsigned int* local_voxel_map = new unsigned int[cell_size_x * cell_size_y];
|
rolling_voxel_scratch_.resize(overlap_size);
|
||||||
|
rolling_stamp_scratch_.resize(overlap_size);
|
||||||
|
unsigned char* local_map = rolling_costmap_scratch_.data();
|
||||||
|
unsigned int* local_voxel_map = rolling_voxel_scratch_.data();
|
||||||
|
double* local_stamp_map = rolling_stamp_scratch_.data();
|
||||||
unsigned int* voxel_map = robot_voxel_grid_.getData();
|
unsigned int* voxel_map = robot_voxel_grid_.getData();
|
||||||
|
|
||||||
// copy the local window in the costmap to the local map
|
if (overlap_size > 0)
|
||||||
copyMapRegion(costmap_, lower_left_x, lower_left_y, size_x_, local_map, 0, 0, cell_size_x, cell_size_x, cell_size_y);
|
{
|
||||||
copyMapRegion(voxel_map, lower_left_x, lower_left_y, size_x_, local_voxel_map, 0, 0, cell_size_x, cell_size_x,
|
copyMapRegion(costmap_, lower_left_x, lower_left_y, size_x_, local_map, 0, 0,
|
||||||
cell_size_y);
|
cell_size_x, cell_size_x, cell_size_y);
|
||||||
|
copyMapRegion(voxel_map, lower_left_x, lower_left_y, size_x_, local_voxel_map, 0, 0,
|
||||||
|
cell_size_x, cell_size_x, cell_size_y);
|
||||||
|
copyMapRegion(cell_last_marked_.data(), lower_left_x, lower_left_y, size_x_,
|
||||||
|
local_stamp_map, 0, 0, cell_size_x, cell_size_x, cell_size_y);
|
||||||
|
}
|
||||||
|
|
||||||
// we'll reset our maps to unknown space if appropriate
|
// we'll reset our maps to unknown space if appropriate
|
||||||
resetMaps();
|
resetMaps();
|
||||||
@@ -751,12 +818,16 @@ void VoxelLayer::updateOrigin(double new_origin_x, double new_origin_y)
|
|||||||
int start_y = lower_left_y - cell_oy;
|
int start_y = lower_left_y - cell_oy;
|
||||||
|
|
||||||
// now we want to copy the overlapping information back into the map, but in its new location
|
// now we want to copy the overlapping information back into the map, but in its new location
|
||||||
copyMapRegion(local_map, 0, 0, cell_size_x, costmap_, start_x, start_y, size_x_, cell_size_x, cell_size_y);
|
if (overlap_size > 0)
|
||||||
copyMapRegion(local_voxel_map, 0, 0, cell_size_x, voxel_map, start_x, start_y, size_x_, cell_size_x, cell_size_y);
|
{
|
||||||
|
copyMapRegion(local_map, 0, 0, cell_size_x, costmap_, start_x, start_y,
|
||||||
|
size_x_, cell_size_x, cell_size_y);
|
||||||
|
copyMapRegion(local_voxel_map, 0, 0, cell_size_x, voxel_map, start_x, start_y,
|
||||||
|
size_x_, cell_size_x, cell_size_y);
|
||||||
|
copyMapRegion(local_stamp_map, 0, 0, cell_size_x, cell_last_marked_.data(),
|
||||||
|
start_x, start_y, size_x_, cell_size_x, cell_size_y);
|
||||||
|
}
|
||||||
|
|
||||||
// make sure to clean up
|
|
||||||
delete[] local_map;
|
|
||||||
delete[] local_voxel_map;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// Export factory function
|
// Export factory function
|
||||||
|
|||||||
@@ -288,11 +288,17 @@ void Costmap2D::updateOrigin(double new_origin_x, double new_origin_y)
|
|||||||
unsigned int cell_size_x = upper_right_x - lower_left_x;
|
unsigned int cell_size_x = upper_right_x - lower_left_x;
|
||||||
unsigned int cell_size_y = upper_right_y - lower_left_y;
|
unsigned int cell_size_y = upper_right_y - lower_left_y;
|
||||||
|
|
||||||
// we need a map to store the obstacles in the window temporarily
|
const std::size_t overlap_size = static_cast<std::size_t>(cell_size_x) * cell_size_y;
|
||||||
unsigned char* local_map = new unsigned char[cell_size_x * cell_size_y];
|
|
||||||
|
|
||||||
// copy the local window in the costmap to the local map
|
// Reuse the temporary window to avoid allocating on every rolling-window shift.
|
||||||
copyMapRegion(costmap_, lower_left_x, lower_left_y, size_x_, local_map, 0, 0, cell_size_x, cell_size_x, cell_size_y);
|
rolling_window_scratch_.resize(overlap_size);
|
||||||
|
unsigned char* local_map = rolling_window_scratch_.data();
|
||||||
|
|
||||||
|
if (overlap_size > 0)
|
||||||
|
{
|
||||||
|
copyMapRegion(costmap_, lower_left_x, lower_left_y, size_x_, local_map, 0, 0,
|
||||||
|
cell_size_x, cell_size_x, cell_size_y);
|
||||||
|
}
|
||||||
|
|
||||||
// now we'll set the costmap to be completely unknown if we track unknown space
|
// now we'll set the costmap to be completely unknown if we track unknown space
|
||||||
resetMaps();
|
resetMaps();
|
||||||
@@ -306,10 +312,12 @@ void Costmap2D::updateOrigin(double new_origin_x, double new_origin_y)
|
|||||||
int start_y = lower_left_y - cell_oy;
|
int start_y = lower_left_y - cell_oy;
|
||||||
|
|
||||||
// now we want to copy the overlapping information back into the map, but in its new location
|
// now we want to copy the overlapping information back into the map, but in its new location
|
||||||
copyMapRegion(local_map, 0, 0, cell_size_x, costmap_, start_x, start_y, size_x_, cell_size_x, cell_size_y);
|
if (overlap_size > 0)
|
||||||
|
{
|
||||||
|
copyMapRegion(local_map, 0, 0, cell_size_x, costmap_, start_x, start_y,
|
||||||
|
size_x_, cell_size_x, cell_size_y);
|
||||||
|
}
|
||||||
|
|
||||||
// make sure to clean up
|
|
||||||
delete[] local_map;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
bool Costmap2D::setConvexPolygonCost(const std::vector<robot_geometry_msgs::Point>& polygon, unsigned char cost_value)
|
bool Costmap2D::setConvexPolygonCost(const std::vector<robot_geometry_msgs::Point>& polygon, unsigned char cost_value)
|
||||||
|
|||||||
@@ -155,9 +155,20 @@ void Costmap2DROBOT::getParams(const std::string& config_file_name,const std::st
|
|||||||
if (priv_nh.hasParam("track_unknown_space"))
|
if (priv_nh.hasParam("track_unknown_space"))
|
||||||
priv_nh.getParam("track_unknown_space", track_unknown_space);
|
priv_nh.getParam("track_unknown_space", track_unknown_space);
|
||||||
|
|
||||||
|
bool performance_metrics_enabled =
|
||||||
|
loadParam(layer, "performance_metrics_enabled", false);
|
||||||
|
double performance_metrics_period =
|
||||||
|
loadParam(layer, "performance_metrics_period", 5.0);
|
||||||
|
if (priv_nh.hasParam("performance_metrics_enabled"))
|
||||||
|
priv_nh.getParam("performance_metrics_enabled", performance_metrics_enabled);
|
||||||
|
if (priv_nh.hasParam("performance_metrics_period"))
|
||||||
|
priv_nh.getParam("performance_metrics_period", performance_metrics_period);
|
||||||
|
|
||||||
if (priv_nh.hasParam("library_path"))
|
if (priv_nh.hasParam("library_path"))
|
||||||
path_plugins = loader.findLibraryPath(name_);
|
path_plugins = loader.findLibraryPath(name_);
|
||||||
layered_costmap_ = new LayeredCostmap(global_frame_, rolling_window, track_unknown_space);
|
layered_costmap_ = new LayeredCostmap(global_frame_, rolling_window, track_unknown_space);
|
||||||
|
layered_costmap_->setPerformanceMetrics(
|
||||||
|
performance_metrics_enabled, performance_metrics_period);
|
||||||
|
|
||||||
// find size parameters
|
// find size parameters
|
||||||
double map_width_meters = loadParam(layer, "width", 0.0);
|
double map_width_meters = loadParam(layer, "width", 0.0);
|
||||||
@@ -348,30 +359,20 @@ void Costmap2DROBOT::copyParentParameters(const std::string& costmap_name,
|
|||||||
move_parameter(plugin_nh, costmap_plugin_nh, "lethal_cost_threshold", lethal_cost_threshold);
|
move_parameter(plugin_nh, costmap_plugin_nh, "lethal_cost_threshold", lethal_cost_threshold);
|
||||||
move_parameter(plugin_nh, costmap_plugin_nh, "track_unknown_space", track_unknown_space);
|
move_parameter(plugin_nh, costmap_plugin_nh, "track_unknown_space", track_unknown_space);
|
||||||
}
|
}
|
||||||
else if(plugin_type == "VoxelLayer" || plugin_type == "costmap_2d::VoxelLayer")
|
else if(plugin_type == "VoxelLayer")
|
||||||
{
|
{
|
||||||
double origin_z;
|
double origin_z;
|
||||||
double z_resolution;
|
double z_resolution;
|
||||||
double frustum_min_range;
|
|
||||||
double frustum_max_range;
|
|
||||||
int z_voxels;
|
int z_voxels;
|
||||||
int mark_threshold;
|
int mark_threshold;
|
||||||
int unknown_threshold;
|
int unknown_threshold;
|
||||||
int frustum_clearing_pixel_step;
|
|
||||||
bool publish_voxel_map;
|
bool publish_voxel_map;
|
||||||
bool frustum_clearing_enabled;
|
|
||||||
std::string frustum_depth_camera_topic;
|
|
||||||
move_parameter(plugin_nh, costmap_plugin_nh, "origin_z", origin_z);
|
move_parameter(plugin_nh, costmap_plugin_nh, "origin_z", origin_z);
|
||||||
move_parameter(plugin_nh, costmap_plugin_nh, "z_resolution", z_resolution);
|
move_parameter(plugin_nh, costmap_plugin_nh, "z_resolution", z_resolution);
|
||||||
move_parameter(plugin_nh, costmap_plugin_nh, "z_voxels", z_voxels);
|
move_parameter(plugin_nh, costmap_plugin_nh, "z_voxels", z_voxels);
|
||||||
move_parameter(plugin_nh, costmap_plugin_nh, "mark_threshold", mark_threshold);
|
move_parameter(plugin_nh, costmap_plugin_nh, "mark_threshold", mark_threshold);
|
||||||
move_parameter(plugin_nh, costmap_plugin_nh, "unknown_threshold", unknown_threshold);
|
move_parameter(plugin_nh, costmap_plugin_nh, "unknown_threshold", unknown_threshold);
|
||||||
move_parameter(plugin_nh, costmap_plugin_nh, "publish_voxel_map", publish_voxel_map);
|
move_parameter(plugin_nh, costmap_plugin_nh, "publish_voxel_map", publish_voxel_map);
|
||||||
move_parameter(plugin_nh, costmap_plugin_nh, "frustum_clearing_enabled", frustum_clearing_enabled);
|
|
||||||
move_parameter(plugin_nh, costmap_plugin_nh, "frustum_clearing_pixel_step", frustum_clearing_pixel_step);
|
|
||||||
move_parameter(plugin_nh, costmap_plugin_nh, "frustum_min_range", frustum_min_range);
|
|
||||||
move_parameter(plugin_nh, costmap_plugin_nh, "frustum_max_range", frustum_max_range);
|
|
||||||
move_parameter(plugin_nh, costmap_plugin_nh, "frustum_depth_camera_topic", frustum_depth_camera_topic);
|
|
||||||
if(plugin_nh.hasParam("observation_sources"))
|
if(plugin_nh.hasParam("observation_sources"))
|
||||||
{
|
{
|
||||||
std::string topics_string;
|
std::string topics_string;
|
||||||
@@ -395,6 +396,11 @@ void Costmap2DROBOT::copyParentParameters(const std::string& costmap_name,
|
|||||||
double max_obstacle_height;
|
double max_obstacle_height;
|
||||||
double obstacle_range;
|
double obstacle_range;
|
||||||
double raytrace_range;
|
double raytrace_range;
|
||||||
|
bool frustum_clearing_enabled = false;
|
||||||
|
int frustum_clearing_pixel_step = 8;
|
||||||
|
double frustum_min_range = 0.2;
|
||||||
|
double frustum_max_range = 3.0;
|
||||||
|
|
||||||
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "topic", topic);
|
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "topic", topic);
|
||||||
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "sensor_frame", sensor_frame);
|
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "sensor_frame", sensor_frame);
|
||||||
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "observation_persistence", observation_persistence);
|
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "observation_persistence", observation_persistence);
|
||||||
@@ -407,7 +413,12 @@ void Costmap2DROBOT::copyParentParameters(const std::string& costmap_name,
|
|||||||
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "max_obstacle_height", max_obstacle_height);
|
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "max_obstacle_height", max_obstacle_height);
|
||||||
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "obstacle_range", obstacle_range);
|
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "obstacle_range", obstacle_range);
|
||||||
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "raytrace_range", raytrace_range);
|
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "raytrace_range", raytrace_range);
|
||||||
|
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "frustum_clearing_enabled", frustum_clearing_enabled);
|
||||||
|
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "frustum_clearing_pixel_step", frustum_clearing_pixel_step);
|
||||||
|
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "frustum_min_range", frustum_min_range);
|
||||||
|
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "frustum_max_range", frustum_max_range);
|
||||||
robot::log_info("topic: %s data_type: %s clearing: %d marking: %d inf_is_valid: %d min_obstacle_height: %f max_obstacle_height: %f", topic.c_str(), data_type.c_str(), clearing, marking, inf_is_valid, min_obstacle_height, max_obstacle_height);
|
robot::log_info("topic: %s data_type: %s clearing: %d marking: %d inf_is_valid: %d min_obstacle_height: %f max_obstacle_height: %f", topic.c_str(), data_type.c_str(), clearing, marking, inf_is_valid, min_obstacle_height, max_obstacle_height);
|
||||||
|
robot::log_info("frustum_clearing_enabled: %s, frustum_clearing_pixel_step: %d, frustum_min_range: %f, frustum_max_range: %f", frustum_clearing_enabled ? "true" : "false", frustum_clearing_pixel_step, frustum_min_range, frustum_max_range);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@@ -444,6 +455,11 @@ void Costmap2DROBOT::copyParentParameters(const std::string& costmap_name,
|
|||||||
double max_obstacle_height;
|
double max_obstacle_height;
|
||||||
double obstacle_range;
|
double obstacle_range;
|
||||||
double raytrace_range;
|
double raytrace_range;
|
||||||
|
bool frustum_clearing_enabled = false;
|
||||||
|
int frustum_clearing_pixel_step = 8;
|
||||||
|
double frustum_min_range = 0.2;
|
||||||
|
double frustum_max_range = 3.0;
|
||||||
|
|
||||||
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "topic", topic);
|
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "topic", topic);
|
||||||
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "sensor_frame", sensor_frame);
|
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "sensor_frame", sensor_frame);
|
||||||
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "observation_persistence", observation_persistence);
|
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "observation_persistence", observation_persistence);
|
||||||
@@ -456,7 +472,12 @@ void Costmap2DROBOT::copyParentParameters(const std::string& costmap_name,
|
|||||||
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "max_obstacle_height", max_obstacle_height);
|
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "max_obstacle_height", max_obstacle_height);
|
||||||
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "obstacle_range", obstacle_range);
|
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "obstacle_range", obstacle_range);
|
||||||
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "raytrace_range", raytrace_range);
|
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "raytrace_range", raytrace_range);
|
||||||
|
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "frustum_clearing_enabled", frustum_clearing_enabled);
|
||||||
|
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "frustum_clearing_pixel_step", frustum_clearing_pixel_step);
|
||||||
|
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "frustum_min_range", frustum_min_range);
|
||||||
|
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "frustum_max_range", frustum_max_range);
|
||||||
robot::log_info("topic: %s data_type: %s clearing: %d marking: %d inf_is_valid: %d min_obstacle_height: %f max_obstacle_height: %f", topic.c_str(), data_type.c_str(), clearing, marking, inf_is_valid, min_obstacle_height, max_obstacle_height);
|
robot::log_info("topic: %s data_type: %s clearing: %d marking: %d inf_is_valid: %d min_obstacle_height: %f max_obstacle_height: %f", topic.c_str(), data_type.c_str(), clearing, marking, inf_is_valid, min_obstacle_height, max_obstacle_height);
|
||||||
|
robot::log_info("frustum_clearing_enabled: %s, frustum_clearing_pixel_step: %d, frustum_min_range: %f, frustum_max_range: %f", frustum_clearing_enabled ? "true" : "false", frustum_clearing_pixel_step, frustum_min_range, frustum_max_range);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@@ -682,35 +703,9 @@ bool Costmap2DROBOT::getRobotPose(robot_geometry_msgs::PoseStamped& global_pose)
|
|||||||
// get the global pose of the robot
|
// get the global pose of the robot
|
||||||
try
|
try
|
||||||
{
|
{
|
||||||
// use current time if possible (makes sure it's not in the future)
|
const tf3::TransformStampedMsg transform =
|
||||||
if (tf_.canTransform(global_frame_, robot_base_frame_, tf3::Time()))
|
tf_.lookupTransform(global_frame_, robot_base_frame_, tf3::Time());
|
||||||
{
|
tf3::doTransform(robot_pose, global_pose, transform);
|
||||||
tf3::TransformStampedMsg transform = tf_.lookupTransform(global_frame_, robot_base_frame_,tf3::Time());
|
|
||||||
tf3::doTransform(robot_pose, global_pose, transform);
|
|
||||||
// robot::log_error("%s ||| %f | %f | %f ||| %f | %f | %f | %f", transform.child_frame_id.c_str(),
|
|
||||||
// global_pose.pose.position.x,
|
|
||||||
// global_pose.pose.position.y,
|
|
||||||
// global_pose.pose.position.z,
|
|
||||||
// global_pose.pose.orientation.x,
|
|
||||||
// global_pose.pose.orientation.y,
|
|
||||||
// global_pose.pose.orientation.z,
|
|
||||||
// global_pose.pose.orientation.w);
|
|
||||||
// transform.transform.rotation.x,
|
|
||||||
// transform.transform.rotation.y,
|
|
||||||
// transform.transform.rotation.z,
|
|
||||||
// transform.transform.rotation.w);
|
|
||||||
}
|
|
||||||
// use the latest otherwise
|
|
||||||
else
|
|
||||||
{
|
|
||||||
// tf_.transform(robot_pose, global_pose, global_frame_);
|
|
||||||
tf3::TransformStampedMsg transform = tf_.lookupTransform(
|
|
||||||
global_frame_, // frame đích
|
|
||||||
robot_base_frame_, // frame nguồn
|
|
||||||
tf3::Time()
|
|
||||||
);
|
|
||||||
tf3::doTransform(robot_pose, global_pose, transform);
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
catch (tf3::LookupException& ex)
|
catch (tf3::LookupException& ex)
|
||||||
{
|
{
|
||||||
|
|||||||
@@ -69,6 +69,68 @@ namespace robot_costmap_2d
|
|||||||
costmap_.setDefaultValue(NO_INFORMATION);
|
costmap_.setDefaultValue(NO_INFORMATION);
|
||||||
else
|
else
|
||||||
costmap_.setDefaultValue(FREE_SPACE);
|
costmap_.setDefaultValue(FREE_SPACE);
|
||||||
|
performance_window_start_ = std::chrono::steady_clock::now();
|
||||||
|
}
|
||||||
|
|
||||||
|
void LayeredCostmap::setPerformanceMetrics(bool enabled, double reporting_period_seconds)
|
||||||
|
{
|
||||||
|
performance_metrics_enabled_ = enabled;
|
||||||
|
performance_metrics_period_seconds_ = reporting_period_seconds > 0.0 ? reporting_period_seconds : 5.0;
|
||||||
|
resetPerformanceMetrics();
|
||||||
|
}
|
||||||
|
|
||||||
|
void LayeredCostmap::resetPerformanceMetrics()
|
||||||
|
{
|
||||||
|
performance_window_start_ = std::chrono::steady_clock::now();
|
||||||
|
performance_cycle_nanoseconds_ = 0;
|
||||||
|
performance_reset_nanoseconds_ = 0;
|
||||||
|
performance_cycles_ = 0;
|
||||||
|
performance_cycle_samples_.clear();
|
||||||
|
performance_cycle_samples_.reserve(128);
|
||||||
|
layer_performance_.assign(plugins_.size(), LayerPerformance());
|
||||||
|
}
|
||||||
|
|
||||||
|
void LayeredCostmap::maybeReportPerformance()
|
||||||
|
{
|
||||||
|
if (!performance_metrics_enabled_ || performance_cycles_ == 0)
|
||||||
|
return;
|
||||||
|
|
||||||
|
const auto now = std::chrono::steady_clock::now();
|
||||||
|
const double elapsed = std::chrono::duration<double>(now - performance_window_start_).count();
|
||||||
|
if (elapsed < performance_metrics_period_seconds_)
|
||||||
|
return;
|
||||||
|
|
||||||
|
const double average_cycle_ms =
|
||||||
|
static_cast<double>(performance_cycle_nanoseconds_) / performance_cycles_ / 1.0e6;
|
||||||
|
const double average_reset_ms =
|
||||||
|
static_cast<double>(performance_reset_nanoseconds_) / performance_cycles_ / 1.0e6;
|
||||||
|
std::sort(performance_cycle_samples_.begin(), performance_cycle_samples_.end());
|
||||||
|
const auto percentile_ms = [this](double percentile) {
|
||||||
|
if (performance_cycle_samples_.empty())
|
||||||
|
return 0.0;
|
||||||
|
const std::size_t index = static_cast<std::size_t>(
|
||||||
|
percentile * static_cast<double>(performance_cycle_samples_.size() - 1));
|
||||||
|
return static_cast<double>(performance_cycle_samples_[index]) / 1.0e6;
|
||||||
|
};
|
||||||
|
robot::log_info(
|
||||||
|
"Costmap performance: cycles=%llu avg_cycle_ms=%.3f p95_cycle_ms=%.3f "
|
||||||
|
"p99_cycle_ms=%.3f avg_reset_ms=%.3f\n",
|
||||||
|
static_cast<unsigned long long>(performance_cycles_), average_cycle_ms,
|
||||||
|
percentile_ms(0.95), percentile_ms(0.99), average_reset_ms);
|
||||||
|
|
||||||
|
for (std::size_t i = 0; i < plugins_.size() && i < layer_performance_.size(); ++i)
|
||||||
|
{
|
||||||
|
const LayerPerformance& stats = layer_performance_[i];
|
||||||
|
const double average_bounds_ms = stats.bounds_calls == 0 ? 0.0 :
|
||||||
|
static_cast<double>(stats.bounds_nanoseconds) / stats.bounds_calls / 1.0e6;
|
||||||
|
const double average_costs_ms = stats.costs_calls == 0 ? 0.0 :
|
||||||
|
static_cast<double>(stats.costs_nanoseconds) / stats.costs_calls / 1.0e6;
|
||||||
|
robot::log_info(
|
||||||
|
"Costmap layer [%s]: avg_bounds_ms=%.3f avg_costs_ms=%.3f\n",
|
||||||
|
plugins_[i]->getName().c_str(), average_bounds_ms, average_costs_ms);
|
||||||
|
}
|
||||||
|
|
||||||
|
resetPerformanceMetrics();
|
||||||
}
|
}
|
||||||
|
|
||||||
LayeredCostmap::~LayeredCostmap()
|
LayeredCostmap::~LayeredCostmap()
|
||||||
@@ -94,6 +156,8 @@ namespace robot_costmap_2d
|
|||||||
|
|
||||||
void LayeredCostmap::updateMap(double robot_x, double robot_y, double robot_yaw)
|
void LayeredCostmap::updateMap(double robot_x, double robot_y, double robot_yaw)
|
||||||
{
|
{
|
||||||
|
const auto cycle_start = performance_metrics_enabled_ ? std::chrono::steady_clock::now() :
|
||||||
|
std::chrono::steady_clock::time_point();
|
||||||
// Lock for the remainder of this function, some plugins (e.g. VoxelLayer)
|
// Lock for the remainder of this function, some plugins (e.g. VoxelLayer)
|
||||||
// implement thread unsafe updateBounds() functions.
|
// implement thread unsafe updateBounds() functions.
|
||||||
boost::unique_lock<Costmap2D::mutex_t> lock(*(costmap_.getMutex()));
|
boost::unique_lock<Costmap2D::mutex_t> lock(*(costmap_.getMutex()));
|
||||||
@@ -111,23 +175,35 @@ namespace robot_costmap_2d
|
|||||||
|
|
||||||
minx_ = miny_ = 1e30;
|
minx_ = miny_ = 1e30;
|
||||||
maxx_ = maxy_ = -1e30;
|
maxx_ = maxy_ = -1e30;
|
||||||
for (vector<boost::shared_ptr<Layer>>::iterator plugin = plugins_.begin(); plugin != plugins_.end();
|
if (performance_metrics_enabled_ && layer_performance_.size() != plugins_.size())
|
||||||
++plugin)
|
layer_performance_.assign(plugins_.size(), LayerPerformance());
|
||||||
|
|
||||||
|
for (std::size_t plugin_index = 0; plugin_index < plugins_.size(); ++plugin_index)
|
||||||
{
|
{
|
||||||
if (!(*plugin)->isEnabled())
|
const boost::shared_ptr<Layer>& plugin = plugins_[plugin_index];
|
||||||
|
if (!plugin->isEnabled())
|
||||||
continue;
|
continue;
|
||||||
double prev_minx = minx_;
|
double prev_minx = minx_;
|
||||||
double prev_miny = miny_;
|
double prev_miny = miny_;
|
||||||
double prev_maxx = maxx_;
|
double prev_maxx = maxx_;
|
||||||
double prev_maxy = maxy_;
|
double prev_maxy = maxy_;
|
||||||
(*plugin)->updateBounds(robot_x, robot_y, robot_yaw, &minx_, &miny_, &maxx_, &maxy_);
|
const auto bounds_start = performance_metrics_enabled_ ? std::chrono::steady_clock::now() :
|
||||||
|
std::chrono::steady_clock::time_point();
|
||||||
|
plugin->updateBounds(robot_x, robot_y, robot_yaw, &minx_, &miny_, &maxx_, &maxy_);
|
||||||
|
if (performance_metrics_enabled_)
|
||||||
|
{
|
||||||
|
layer_performance_[plugin_index].bounds_nanoseconds +=
|
||||||
|
std::chrono::duration_cast<std::chrono::nanoseconds>(
|
||||||
|
std::chrono::steady_clock::now() - bounds_start).count();
|
||||||
|
++layer_performance_[plugin_index].bounds_calls;
|
||||||
|
}
|
||||||
if (minx_ > prev_minx || miny_ > prev_miny || maxx_ < prev_maxx || maxy_ < prev_maxy)
|
if (minx_ > prev_minx || miny_ > prev_miny || maxx_ < prev_maxx || maxy_ < prev_maxy)
|
||||||
{
|
{
|
||||||
robot::log_error("Illegal bounds change, was [tl: (%f, %f), br: (%f, %f)], but "
|
robot::log_error("Illegal bounds change, was [tl: (%f, %f), br: (%f, %f)], but "
|
||||||
"is now [tl: (%f, %f), br: (%f, %f)]. The offending layer is %s\n",
|
"is now [tl: (%f, %f), br: (%f, %f)]. The offending layer is %s\n",
|
||||||
prev_minx, prev_miny, prev_maxx, prev_maxy,
|
prev_minx, prev_miny, prev_maxx, prev_maxy,
|
||||||
minx_, miny_, maxx_, maxy_,
|
minx_, miny_, maxx_, maxy_,
|
||||||
(*plugin)->getName().c_str());
|
plugin->getName().c_str());
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -143,13 +219,32 @@ namespace robot_costmap_2d
|
|||||||
if (xn < x0 || yn < y0)
|
if (xn < x0 || yn < y0)
|
||||||
return;
|
return;
|
||||||
|
|
||||||
|
const auto reset_start = performance_metrics_enabled_ ? std::chrono::steady_clock::now() :
|
||||||
|
std::chrono::steady_clock::time_point();
|
||||||
costmap_.resetMap(x0, y0, xn, yn);
|
costmap_.resetMap(x0, y0, xn, yn);
|
||||||
|
if (performance_metrics_enabled_)
|
||||||
for (vector<boost::shared_ptr<Layer>>::iterator plugin = plugins_.begin(); plugin != plugins_.end();
|
|
||||||
++plugin)
|
|
||||||
{
|
{
|
||||||
if ((*plugin)->isEnabled())
|
performance_reset_nanoseconds_ +=
|
||||||
(*plugin)->updateCosts(costmap_, x0, y0, xn, yn);
|
std::chrono::duration_cast<std::chrono::nanoseconds>(
|
||||||
|
std::chrono::steady_clock::now() - reset_start).count();
|
||||||
|
}
|
||||||
|
|
||||||
|
for (std::size_t plugin_index = 0; plugin_index < plugins_.size(); ++plugin_index)
|
||||||
|
{
|
||||||
|
const boost::shared_ptr<Layer>& plugin = plugins_[plugin_index];
|
||||||
|
if (!plugin->isEnabled())
|
||||||
|
continue;
|
||||||
|
|
||||||
|
const auto costs_start = performance_metrics_enabled_ ? std::chrono::steady_clock::now() :
|
||||||
|
std::chrono::steady_clock::time_point();
|
||||||
|
plugin->updateCosts(costmap_, x0, y0, xn, yn);
|
||||||
|
if (performance_metrics_enabled_)
|
||||||
|
{
|
||||||
|
layer_performance_[plugin_index].costs_nanoseconds +=
|
||||||
|
std::chrono::duration_cast<std::chrono::nanoseconds>(
|
||||||
|
std::chrono::steady_clock::now() - costs_start).count();
|
||||||
|
++layer_performance_[plugin_index].costs_calls;
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
bx0_ = x0;
|
bx0_ = x0;
|
||||||
@@ -158,6 +253,17 @@ namespace robot_costmap_2d
|
|||||||
byn_ = yn;
|
byn_ = yn;
|
||||||
|
|
||||||
initialized_ = true;
|
initialized_ = true;
|
||||||
|
|
||||||
|
if (performance_metrics_enabled_)
|
||||||
|
{
|
||||||
|
const std::uint64_t cycle_nanoseconds = static_cast<std::uint64_t>(
|
||||||
|
std::chrono::duration_cast<std::chrono::nanoseconds>(
|
||||||
|
std::chrono::steady_clock::now() - cycle_start).count());
|
||||||
|
performance_cycle_nanoseconds_ += cycle_nanoseconds;
|
||||||
|
performance_cycle_samples_.push_back(cycle_nanoseconds);
|
||||||
|
++performance_cycles_;
|
||||||
|
maybeReportPerformance();
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
bool LayeredCostmap::isCurrent()
|
bool LayeredCostmap::isCurrent()
|
||||||
|
|||||||
File diff suppressed because it is too large
Load Diff
@@ -36,6 +36,14 @@
|
|||||||
|
|
||||||
#include <gtest/gtest.h>
|
#include <gtest/gtest.h>
|
||||||
#include <robot_costmap_2d/costmap_2d.h>
|
#include <robot_costmap_2d/costmap_2d.h>
|
||||||
|
#include <robot_costmap_2d/cost_values.h>
|
||||||
|
#include <robot_costmap_2d/inflation_layer.h>
|
||||||
|
#include <robot_costmap_2d/layered_costmap.h>
|
||||||
|
#include <robot_costmap_2d/observation_buffer.h>
|
||||||
|
#include <robot_costmap_2d/voxel_layer.h>
|
||||||
|
|
||||||
|
#include <boost/make_shared.hpp>
|
||||||
|
#include <cstdlib>
|
||||||
|
|
||||||
using namespace robot_costmap_2d;
|
using namespace robot_costmap_2d;
|
||||||
|
|
||||||
@@ -124,9 +132,100 @@ TEST(CostmapCoordinates, hard_coordinates_test)
|
|||||||
EXPECT_EQ(my, 2);
|
EXPECT_EQ(my, 2);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
TEST(CostmapPerformanceRegression, rolling_origin_preserves_overlap)
|
||||||
|
{
|
||||||
|
Costmap2D costmap(4, 3, 1.0, 0.0, 0.0, FREE_SPACE);
|
||||||
|
costmap.setCost(1, 1, LETHAL_OBSTACLE);
|
||||||
|
costmap.setCost(3, 2, INSCRIBED_INFLATED_OBSTACLE);
|
||||||
|
|
||||||
|
costmap.updateOrigin(0.25, 0.25);
|
||||||
|
EXPECT_DOUBLE_EQ(costmap.getOriginX(), 0.0);
|
||||||
|
EXPECT_DOUBLE_EQ(costmap.getOriginY(), 0.0);
|
||||||
|
EXPECT_EQ(costmap.getCost(1, 1), LETHAL_OBSTACLE);
|
||||||
|
|
||||||
|
costmap.updateOrigin(1.0, 0.0);
|
||||||
|
EXPECT_DOUBLE_EQ(costmap.getOriginX(), 1.0);
|
||||||
|
EXPECT_EQ(costmap.getCost(0, 1), LETHAL_OBSTACLE);
|
||||||
|
EXPECT_EQ(costmap.getCost(3, 2), FREE_SPACE);
|
||||||
|
}
|
||||||
|
|
||||||
|
TEST(CostmapPerformanceRegression, voxel_origin_subcell_shift_is_noop)
|
||||||
|
{
|
||||||
|
VoxelLayer layer;
|
||||||
|
layer.resizeMap(4, 3, 1.0, 0.0, 0.0);
|
||||||
|
layer.setCost(1, 1, LETHAL_OBSTACLE);
|
||||||
|
|
||||||
|
layer.updateOrigin(0.25, 0.25);
|
||||||
|
|
||||||
|
EXPECT_DOUBLE_EQ(layer.getOriginX(), 0.0);
|
||||||
|
EXPECT_DOUBLE_EQ(layer.getOriginY(), 0.0);
|
||||||
|
EXPECT_EQ(layer.getCost(1, 1), LETHAL_OBSTACLE);
|
||||||
|
}
|
||||||
|
|
||||||
|
TEST(CostmapPerformanceRegression, observation_copy_shares_cloud_payload)
|
||||||
|
{
|
||||||
|
robot_geometry_msgs::Point origin;
|
||||||
|
robot_sensor_msgs::PointCloud2 cloud;
|
||||||
|
cloud.height = 1;
|
||||||
|
cloud.width = 1;
|
||||||
|
cloud.point_step = 4;
|
||||||
|
cloud.row_step = 4;
|
||||||
|
cloud.data = {1, 2, 3, 4};
|
||||||
|
|
||||||
|
Observation observation(origin, cloud, 2.5, 3.0);
|
||||||
|
Observation copied = observation;
|
||||||
|
|
||||||
|
EXPECT_EQ(copied.cloud_, observation.cloud_);
|
||||||
|
EXPECT_EQ(copied.cloud_handle_.use_count(), 2);
|
||||||
|
EXPECT_EQ(copied.cloud_->data, cloud.data);
|
||||||
|
}
|
||||||
|
|
||||||
|
TEST(CostmapPerformanceRegression, latest_depth_frame_is_consumed_once)
|
||||||
|
{
|
||||||
|
tf3::BufferCore tf_buffer(tf3::Duration(10.0));
|
||||||
|
ObservationBuffer buffer(
|
||||||
|
"/camera/depth/data", 0.0, 0.5, 0.0, 2.0, 2.5, 3.0,
|
||||||
|
8, 0.2, 3.0, tf_buffer, "odom", "", 0.2);
|
||||||
|
|
||||||
|
robot_sensor_msgs::DepthCameraData::ConstPtr depth =
|
||||||
|
boost::make_shared<robot_sensor_msgs::DepthCameraData>();
|
||||||
|
buffer.bufferDepthCamera(depth);
|
||||||
|
|
||||||
|
std::vector<DepthCameraObservation> first_snapshot;
|
||||||
|
buffer.getDepthObservations(first_snapshot);
|
||||||
|
ASSERT_EQ(first_snapshot.size(), 1u);
|
||||||
|
EXPECT_EQ(first_snapshot.front().data_, depth.get());
|
||||||
|
EXPECT_EQ(first_snapshot.front().topic_, "/camera/depth/data");
|
||||||
|
|
||||||
|
std::vector<DepthCameraObservation> second_snapshot;
|
||||||
|
buffer.getDepthObservations(second_snapshot);
|
||||||
|
EXPECT_TRUE(second_snapshot.empty());
|
||||||
|
}
|
||||||
|
|
||||||
|
TEST(CostmapPerformanceRegression, inflation_buckets_preserve_radial_costs)
|
||||||
|
{
|
||||||
|
ASSERT_EQ(setenv("PNKX_NAV_CORE_CONFIG_DIR", ROBOT_COSTMAP_2D_DIR, 1), 0);
|
||||||
|
|
||||||
|
LayeredCostmap layered_costmap("map", false, false);
|
||||||
|
layered_costmap.resizeMap(7, 7, 1.0, 0.0, 0.0, true);
|
||||||
|
tf3::BufferCore tf_buffer(tf3::Duration(10.0));
|
||||||
|
InflationLayer inflation;
|
||||||
|
inflation.initialize(&layered_costmap, "inflation", &tf_buffer);
|
||||||
|
inflation.setInflationParameters(2.0, 1.0);
|
||||||
|
|
||||||
|
Costmap2D& master = *layered_costmap.getCostmap();
|
||||||
|
master.setCost(3, 3, LETHAL_OBSTACLE);
|
||||||
|
inflation.updateCosts(master, 0, 0, 7, 7);
|
||||||
|
|
||||||
|
EXPECT_EQ(master.getCost(3, 3), LETHAL_OBSTACLE);
|
||||||
|
EXPECT_EQ(master.getCost(2, 3), master.getCost(4, 3));
|
||||||
|
EXPECT_EQ(master.getCost(3, 2), master.getCost(3, 4));
|
||||||
|
EXPECT_GT(master.getCost(4, 3), master.getCost(5, 3));
|
||||||
|
EXPECT_EQ(master.getCost(6, 3), FREE_SPACE);
|
||||||
|
}
|
||||||
|
|
||||||
int main(int argc, char** argv)
|
int main(int argc, char** argv)
|
||||||
{
|
{
|
||||||
testing::InitGoogleTest( &argc, argv );
|
testing::InitGoogleTest( &argc, argv );
|
||||||
return RUN_ALL_TESTS();
|
return RUN_ALL_TESTS();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
Reference in New Issue
Block a user