diff --git a/src/common/launch_system/launch/start_all.launch.py b/src/common/launch_system/launch/start_all.launch.py index 0fac479..ba90262 100644 --- a/src/common/launch_system/launch/start_all.launch.py +++ b/src/common/launch_system/launch/start_all.launch.py @@ -4,6 +4,7 @@ from launch.actions import TimerAction, ExecuteProcess from ament_index_python.packages import get_package_share_directory import os + def generate_launch_description(): rslidar_sdk_dir = get_package_share_directory("rslidar_sdk") pointcloud_merger_dir = get_package_share_directory("rslidar_pointcloud_merger") @@ -25,7 +26,7 @@ def generate_launch_description(): output="screen", prefix="taskset -c 0", ) - ] + ], ) remote_ctrl = TimerAction( @@ -37,7 +38,7 @@ def generate_launch_description(): output="screen", prefix="taskset -c 2", ) - ] + ], ) mc = TimerAction( @@ -49,7 +50,7 @@ def generate_launch_description(): output="screen", prefix="taskset -c 2", ) - ] + ], ) ctrl_arbiter = TimerAction( @@ -61,7 +62,7 @@ def generate_launch_description(): output="screen", prefix="taskset -c 1", ) - ] + ], ) pl = TimerAction( @@ -73,7 +74,7 @@ def generate_launch_description(): output="screen", prefix="taskset -c 3", ) - ] + ], ) mqtt_report = TimerAction( @@ -85,7 +86,7 @@ def generate_launch_description(): output="screen", prefix="taskset -c 7", ) - ] + ], ) lidar_driver = TimerAction( @@ -96,18 +97,23 @@ def generate_launch_description(): output="screen", prefix="taskset -c 4,5", ) - ] + ], ) pointcloud_merger = TimerAction( period=3.1, actions=[ ExecuteProcess( - cmd=["ros2", "launch", "rslidar_pointcloud_merger", "merge_two_lidars.launch.py"], + cmd=[ + "ros2", + "launch", + "rslidar_pointcloud_merger", + "merge_two_lidars.launch.py", + ], output="screen", - prefix="taskset -c 6", + # prefix="taskset -c 6", ) - ] + ], ) rtk = TimerAction( @@ -119,7 +125,7 @@ def generate_launch_description(): output="screen", prefix="taskset -c 7", ) - ] + ], ) route = TimerAction( @@ -131,7 +137,7 @@ def generate_launch_description(): output="screen", prefix="taskset -c 7", ) - ] + ], ) sub = TimerAction( @@ -143,7 +149,7 @@ def generate_launch_description(): output="screen", prefix="taskset -c 7", ) - ] + ], ) task_manager = TimerAction( @@ -155,7 +161,7 @@ def generate_launch_description(): output="screen", prefix="taskset -c 7", ) - ] + ], ) fu = TimerAction( @@ -166,22 +172,24 @@ def generate_launch_description(): output="screen", prefix="taskset -c 7", ) - ] + ], ) - return LaunchDescription([ - vehicle_params, - can_radio_ctrl, - remote_ctrl, - mc, - ctrl_arbiter, - pl, - mqtt_report, - lidar_driver, - pointcloud_merger, - rtk, - route, - sub, - task_manager, - fu, - ]) \ No newline at end of file + return LaunchDescription( + [ + vehicle_params, + can_radio_ctrl, + remote_ctrl, + mc, + ctrl_arbiter, + pl, + mqtt_report, + lidar_driver, + pointcloud_merger, + rtk, + route, + sub, + task_manager, + fu, + ] + ) diff --git a/src/perception/rslidar_pointcloud_merger/src/merge_two_lidars.cpp b/src/perception/rslidar_pointcloud_merger/src/merge_two_lidars.cpp index e9bc802..2692128 100644 --- a/src/perception/rslidar_pointcloud_merger/src/merge_two_lidars.cpp +++ b/src/perception/rslidar_pointcloud_merger/src/merge_two_lidars.cpp @@ -61,12 +61,13 @@ #include /* strerror */ #endif -#include "logger/logger.h" #include #include #include #include +#include "logger/logger.h" + using sensor_msgs::msg::PointCloud2; /* ================================================================ @@ -715,9 +716,13 @@ class LidarMerger : public rclcpp::Node if (filter_brush_) { // 右扫刷 - if (fx >= brush_r_min_x_ && fx <= brush_r_max_x_ && fy >= brush_r_min_y_ && fy <= brush_r_max_y_ && fz >= brush_r_min_z_ && fz <= brush_r_max_z_) continue; + if (fx >= brush_r_min_x_ && fx <= brush_r_max_x_ && fy >= brush_r_min_y_ && fy <= brush_r_max_y_ && + fz >= brush_r_min_z_ && fz <= brush_r_max_z_) + continue; // 左扫刷 - if (fx >= brush_l_min_x_ && fx <= brush_l_max_x_ && fy >= brush_l_min_y_ && fy <= brush_l_max_y_ && fz >= brush_l_min_z_ && fz <= brush_l_max_z_) continue; + if (fx >= brush_l_min_x_ && fx <= brush_l_max_x_ && fy >= brush_l_min_y_ && fy <= brush_l_max_y_ && + fz >= brush_l_min_z_ && fz <= brush_l_max_z_) + continue; } } @@ -1142,9 +1147,8 @@ class LidarMerger : public rclcpp::Node last_stats_time_ = t; int f = front_rx_.load(), r = rear_rx_.load(), m = merged_count_.load(); double merge_rate = (f > 0) ? m * 100.0 / std::min(f, r) : 0.0; - LOG_INFO("Stats | rx(F=%d,R=%d) merged=%d(%.0f%%) | queue(raw_F=%zu,raw_R=%zu,proc=%zu) | sor_method=%d", f, - r, m, merge_rate, raw_queue_front_.size(), raw_queue_rear_.size(), processed_queue_.size(), - sor_method_); + LOG_INFO("Stats | rx(F=%d,R=%d) merged=%d(%.0f%%) | queue(raw_F=%zu,raw_R=%zu,proc=%zu) | sor_method=%d", f, r, + m, merge_rate, raw_queue_front_.size(), raw_queue_rear_.size(), processed_queue_.size(), sor_method_); } /* ============================================================