From dcfc5809ccff27fdd2a391860b03bd8b95c48ae3 Mon Sep 17 00:00:00 2001 From: duongtd Date: Fri, 24 Jul 2026 10:27:56 +0700 Subject: [PATCH] otimal --- README.md | 71 ++++++++++++++++- config/config/costmap_common_params.yaml | 8 +- config/config/costmap_local_params.yaml | 4 +- config/config/costmap_params.yaml | 2 +- config/config/obstacle_layer_params.yaml | 4 +- config/costmap_common_params.yaml | 4 +- config/costmap_local_params.yaml | 4 +- launch/depth_local_costmap_test.launch | 16 +++- src/depth_local_costmap_noetic_test_node.cpp | 83 +++++++++++++++++++- 9 files changed, 176 insertions(+), 20 deletions(-) diff --git a/README.md b/README.md index 261cd27..7d468da 100644 --- a/README.md +++ b/README.md @@ -35,10 +35,11 @@ source devel/setup.bash roslaunch depth_local_costmap_noetic_test depth_local_costmap_test.launch ``` -Launch mặc định mở RViz với config sẵn. Nếu chỉ muốn chạy node không mở GUI: +Launch mặc định chạy headless để số CPU phản ánh pipeline, không tính RViz. +Muốn mở GUI: ```bash -roslaunch depth_local_costmap_noetic_test depth_local_costmap_test.launch rviz:=false +roslaunch depth_local_costmap_noetic_test depth_local_costmap_test.launch rviz:=true ``` Nếu robot dùng `base_footprint`: @@ -57,10 +58,10 @@ Không bật `publish_odom_tf` nếu đã có node khác publish TF này. Point cloud marking `/camera/depth/points_proc` được downsample bằng PCL `VoxelGrid` trước khi feed vào `robot_costmap_2d`, giống hướng trong -`amr_control/src/sensor_converter.cpp`. Mặc định leaf size là `0.10 m`: +`amr_control/src/sensor_converter.cpp`. Mặc định leaf size là `0.05 m`: ```bash -roslaunch depth_local_costmap_noetic_test depth_local_costmap_test.launch depth_cloud_voxel_leaf_size:=0.10 +roslaunch depth_local_costmap_noetic_test depth_local_costmap_test.launch depth_cloud_voxel_leaf_size:=0.05 ``` Nếu muốn so sánh full cloud: @@ -94,6 +95,68 @@ Các topic RViz giống hướng `amr_control::AmrPublisher`: - `/local_costmap/costmap` kiểu `nav_msgs/OccupancyGrid` - `/local_costmap/costmap/footprint` kiểu `geometry_msgs/PolygonStamped` +## Cấu hình CPU/10 Hz mặc định + +Các mặc định dùng cho một Intel D435i: + +- `robot_depth_image_proc`: chỉ một camera, xử lý tối đa `15 Hz`, RViz tắt. +- marking cloud: `filter_decimation=4`, sau đó PCL `VoxelGrid=0.05 m`. +- clearing depth: xử lý tối đa `15 Hz`. +- local costmap: update `10 Hz`, publish OccupancyGrid `5 Hz`. +- frustum clearing: `pixel_step=2`, `column_cover_distance=0.2 m`. + +Chạy hai pipeline headless: + +```bash +roslaunch robot_depth_image_proc depth_image_proc_gazebo.launch \ + multi_cam:=false processing_rate:=15.0 rviz:=false + +roslaunch depth_local_costmap_noetic_test depth_local_costmap_test.launch \ + local_costmap_update_frequency:=10.0 \ + depth_clearing_processing_rate:=15.0 \ + depth_cloud_voxel_leaf_size:=0.05 \ + frustum_clearing_pixel_step:=2 \ + column_cover_distance:=0.2 \ + rviz:=false +``` + +`processing_rate<=0` hoặc `depth_clearing_processing_rate<=0` sẽ tắt rate +limit để A/B test. Camera vẫn có thể publish depth raw ở 30 Hz; chỉ các hot +path copy/filter/project/clearing được giới hạn. + +Mỗi 5 giây, log `depth performance` và `Depth clearing performance` báo +input Hz, processed Hz, số frame bỏ qua, thời gian trung bình và max. Log +`Costmap performance` có `avg/p95/p99_cycle_ms`. + +Đo trên NUC trong ít nhất 60 giây sau warm-up: + +```bash +rostopic hz /camera/depth/image_raw +rostopic hz /camera/depth/points_proc +rostopic hz /local_costmap/costmap +pidstat -p "$(pgrep -d, -f 'depth_image_proc_node|depth_local_costmap_noetic_test_node')" 1 +``` + +Mục tiêu: + +- `/camera/depth/points_proc`: `14-16 Hz`. +- costmap update thực tế: ít nhất `9.5 Hz`, tính từ `cycles` trong log + `Costmap performance` chia cho cửa sổ 5 giây. +- `/local_costmap/costmap`: khoảng `5 Hz` vì OccupancyGrid được publish riêng. +- `p95 < 80 ms`, `p99 < 100 ms`. +- chạy headless: từng node không quá khoảng `30% CPU`, tổng không quá khoảng + `60%` theo thang `htop` mà `100%` là một logical CPU. + +`column_cover_distance=0.2 m` là giá trị clearing mạnh đã giải quyết ghost +trong bài test hiện tại. Nó không phải giá trị suy ra an toàn từ vertical FOV; +phải test lại với vật cản thật cao đến `1.0 m` trước khi đưa lên robot chạy. +Nếu cần baseline hình học bảo thủ, dùng `column_cover_distance:=-1.0`. + +Chỉ chuyển sang pha chia sẻ ảnh depth đã edge-filter giữa hai node nếu +`depth_image_proc_node` vẫn vượt mục tiêu CPU. Chỉ thay PCL `VoxelGrid` bằng +downsampler trực tiếp `PointCloud2` nếu profiler xác nhận PCL còn là hotspot; +hai thay đổi đó cần benchmark NUC sau bước hiện tại để giữ nguyên semantics. + Config local chính của package này dùng cùng kiểu tách file với `pnkx_nav_core/config`: diff --git a/config/config/costmap_common_params.yaml b/config/config/costmap_common_params.yaml index d9abf8a..d461596 100644 --- a/config/config/costmap_common_params.yaml +++ b/config/config/costmap_common_params.yaml @@ -30,9 +30,15 @@ obstacles: marking: true inf_is_valid: false frustum_clearing_enabled: true - frustum_clearing_pixel_step: 4 + frustum_clearing_pixel_step: 2 frustum_min_range: 0.20 frustum_max_range: 3.5 + frustum_skip_distance: 0.05 + frustum_column_clearing: true + column_clear_min_height: 0.10 + column_clear_max_height: -1.0 + column_skip_distance: 0.02 + column_cover_distance: 0.2 observation_persistence: 0.0 expected_update_rate: 0.5 obstacle_range: 2.5 diff --git a/config/config/costmap_local_params.yaml b/config/config/costmap_local_params.yaml index 778a0f5..63c20ce 100644 --- a/config/config/costmap_local_params.yaml +++ b/config/config/costmap_local_params.yaml @@ -4,8 +4,8 @@ local_costmap: library_path: /home/robotics/DuongTD/dev_ws/devel/lib/libplugins.so global_frame: odom robot_base_frame: base_link - update_frequency: 6.0 - publish_frequency: 6.0 + update_frequency: 10.0 + publish_frequency: 5.0 rolling_window: true track_unknown_space: false raytrace_range: 3.0 diff --git a/config/config/costmap_params.yaml b/config/config/costmap_params.yaml index d6ca2e7..716fa00 100644 --- a/config/config/costmap_params.yaml +++ b/config/config/costmap_params.yaml @@ -25,7 +25,7 @@ robot_costmap_2d: transform_tolerance: 1.0 performance_metrics_enabled: true performance_metrics_period: 5.0 - update_frequency: 6.0 + update_frequency: 10.0 width: 8.0 height: 8.0 resolution: 0.05 diff --git a/config/config/obstacle_layer_params.yaml b/config/config/obstacle_layer_params.yaml index c00cc0f..fc44e34 100644 --- a/config/config/obstacle_layer_params.yaml +++ b/config/config/obstacle_layer_params.yaml @@ -30,7 +30,7 @@ obstacle_layer: marking: true inf_is_valid: false frustum_clearing_enabled: true - frustum_clearing_pixel_step: 4 + frustum_clearing_pixel_step: 2 frustum_min_range: 0.20 frustum_max_range: 3.5 # 3D clearing rays stop this far [m] before the measured surface. @@ -50,7 +50,7 @@ obstacle_layer: column_skip_distance: 0.02 # Full columns only cleared beyond this distance [m] (vertical FOV must # cover the whole band). Negative: auto from camera intrinsics + mount. - column_cover_distance: -1.0 + column_cover_distance: 0.2 observation_persistence: 0.0 expected_update_rate: 0.5 obstacle_range: 2.5 diff --git a/config/costmap_common_params.yaml b/config/costmap_common_params.yaml index 1814f37..d461596 100644 --- a/config/costmap_common_params.yaml +++ b/config/costmap_common_params.yaml @@ -30,7 +30,7 @@ obstacles: marking: true inf_is_valid: false frustum_clearing_enabled: true - frustum_clearing_pixel_step: 4 + frustum_clearing_pixel_step: 2 frustum_min_range: 0.20 frustum_max_range: 3.5 frustum_skip_distance: 0.05 @@ -38,7 +38,7 @@ obstacles: column_clear_min_height: 0.10 column_clear_max_height: -1.0 column_skip_distance: 0.02 - column_cover_distance: -1.0 + column_cover_distance: 0.2 observation_persistence: 0.0 expected_update_rate: 0.5 obstacle_range: 2.5 diff --git a/config/costmap_local_params.yaml b/config/costmap_local_params.yaml index 778a0f5..63c20ce 100644 --- a/config/costmap_local_params.yaml +++ b/config/costmap_local_params.yaml @@ -4,8 +4,8 @@ local_costmap: library_path: /home/robotics/DuongTD/dev_ws/devel/lib/libplugins.so global_frame: odom robot_base_frame: base_link - update_frequency: 6.0 - publish_frequency: 6.0 + update_frequency: 10.0 + publish_frequency: 5.0 rolling_window: true track_unknown_space: false raytrace_range: 3.0 diff --git a/launch/depth_local_costmap_test.launch b/launch/depth_local_costmap_test.launch index 5e9c605..9b361ec 100644 --- a/launch/depth_local_costmap_test.launch +++ b/launch/depth_local_costmap_test.launch @@ -3,7 +3,7 @@ - + @@ -11,8 +11,12 @@ - - + + + + + + @@ -32,7 +36,7 @@ (tighter than frustum_skip_distance so ghosts hugging static obstacles still get cleared). --> - + + + + @@ -52,6 +59,7 @@ + diff --git a/src/depth_local_costmap_noetic_test_node.cpp b/src/depth_local_costmap_noetic_test_node.cpp index 36e4a97..6731e26 100644 --- a/src/depth_local_costmap_noetic_test_node.cpp +++ b/src/depth_local_costmap_noetic_test_node.cpp @@ -9,6 +9,7 @@ #include #include #include +#include #include #include #include @@ -32,7 +33,10 @@ #include #include +#include #include +#include +#include #include #include #include @@ -272,7 +276,7 @@ public: { private_nh.param("depth_cloud_topic", depth_cloud_topic_, std::string("/camera/depth/points_proc")); private_nh.param("enable_depth_cloud_voxel_filter", enable_voxel_filter_, true); - private_nh.param("depth_cloud_voxel_leaf_size", voxel_leaf_size_, 0.10); + private_nh.param("depth_cloud_voxel_leaf_size", voxel_leaf_size_, 0.05); voxel_leaf_size_ = std::max(0.0, voxel_leaf_size_); depth_cloud_sub_ = @@ -327,7 +331,7 @@ private: robot_costmap_2d::Costmap2DROBOT& local_costmap_; std::string depth_cloud_topic_; bool enable_voxel_filter_{true}; - double voxel_leaf_size_{0.10}; + double voxel_leaf_size_{0.05}; ros::Subscriber depth_cloud_sub_; }; @@ -345,6 +349,19 @@ public: depth_camera_data_topic_, std::string("/camera/depth/data")); private_nh.param("enable_depth_clearing_filter", enable_clearing_filter_, enable_clearing_filter_); + private_nh.param( + "depth_clearing_processing_rate", processing_rate_hz_, processing_rate_hz_); + private_nh.param( + "feeder_performance_metrics_enabled", + performance_metrics_enabled_, + performance_metrics_enabled_); + private_nh.param( + "feeder_performance_metrics_period", + performance_metrics_period_, + performance_metrics_period_); + performance_metrics_period_ = std::max(1.0, performance_metrics_period_); + processing_rate_limiter_.setRate(processing_rate_hz_); + metrics_window_start_ = ros::WallTime::now(); camera_info_sub_ = private_nh.subscribe(camera_info_topic_, 1, &DepthCameraDataFeeder::cameraInfoCallback, this); @@ -443,6 +460,15 @@ private: return; } + ++received_frames_; + if (!processing_rate_limiter_.shouldProcess()) + { + ++skipped_frames_; + reportPerformanceIfDue(); + return; + } + + const auto processing_start = std::chrono::steady_clock::now(); robot_sensor_msgs::DepthCameraData::Ptr depth_camera_data(new robot_sensor_msgs::DepthCameraData()); depth_camera_data->depth = depth_image_proc::toRobotImage(*msg); @@ -464,6 +490,7 @@ private: robot_sensor_msgs::DepthCameraData::ConstPtr const_depth_camera_data = depth_camera_data; feedVoxelLayers(local_costmap_, const_depth_camera_data, depth_camera_data_topic_); + recordProcessedFrame(processing_start); ROS_INFO_THROTTLE(5.0, "Fed DepthCameraData to robot_costmap_2d: %ux%u encoding=%s frame=%s topic=%s", msg->width, @@ -471,6 +498,48 @@ private: msg->encoding.c_str(), depth_camera_data->header.frame_id.c_str(), depth_camera_data_topic_.c_str()); + reportPerformanceIfDue(); + } + + void recordProcessedFrame(const std::chrono::steady_clock::time_point processing_start) + { + const double elapsed_ms = + std::chrono::duration( + std::chrono::steady_clock::now() - processing_start).count(); + ++processed_frames_; + processing_time_ms_ += elapsed_ms; + max_processing_time_ms_ = std::max(max_processing_time_ms_, elapsed_ms); + } + + void reportPerformanceIfDue() + { + if (!performance_metrics_enabled_) + return; + + const ros::WallTime now = ros::WallTime::now(); + const double window_seconds = (now - metrics_window_start_).toSec(); + if (window_seconds < performance_metrics_period_) + return; + + const double input_hz = static_cast(received_frames_) / window_seconds; + const double processed_hz = static_cast(processed_frames_) / window_seconds; + const double average_ms = + processed_frames_ > 0 ? processing_time_ms_ / static_cast(processed_frames_) : 0.0; + ROS_INFO( + "Depth clearing performance: input=%.1f Hz processed=%.1f Hz " + "skipped=%llu avg=%.2f ms max=%.2f ms", + input_hz, + processed_hz, + static_cast(skipped_frames_), + average_ms, + max_processing_time_ms_); + + metrics_window_start_ = now; + received_frames_ = 0; + processed_frames_ = 0; + skipped_frames_ = 0; + processing_time_ms_ = 0.0; + max_processing_time_ms_ = 0.0; } robot_costmap_2d::Costmap2DROBOT& local_costmap_; @@ -478,6 +547,16 @@ private: std::string camera_info_topic_; std::string depth_camera_data_topic_; bool enable_clearing_filter_{true}; + double processing_rate_hz_{15.0}; + depth_image_proc::ProcessingRateLimiter processing_rate_limiter_; + bool performance_metrics_enabled_{true}; + double performance_metrics_period_{5.0}; + ros::WallTime metrics_window_start_; + std::uint64_t received_frames_{0}; + std::uint64_t processed_frames_{0}; + std::uint64_t skipped_frames_{0}; + double processing_time_ms_{0.0}; + double max_processing_time_ms_{0.0}; depth_image_proc::DepthFilterConfig clearing_filter_config_; depth_image_proc::DepthFrameFilter clearing_frame_filter_; robot_sensor_msgs::CameraInfo camera_info_;