lyq/sweeper_200
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

去掉 rslidar_pointcloud_merger taskset，让代码内部的线程绑定生效

Browse Source
This commit is contained in:
Alvin-lyq 2026-04-22 13:12:44 +08:00
parent 2473e3cc0e
commit 0c73b91e1e
2 changed files with 49 additions and 37 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

70
src/common/launch_system/launch/start_all.launch.py
View File

@ -4,6 +4,7 @@ from launch.actions import TimerAction, ExecuteProcess
from ament_index_python.packages import get_package_share_directory from ament_index_python.packages import get_package_share_directory
import os import os
def generate_launch_description(): def generate_launch_description():
rslidar_sdk_dir = get_package_share_directory("rslidar_sdk") rslidar_sdk_dir = get_package_share_directory("rslidar_sdk")
pointcloud_merger_dir = get_package_share_directory("rslidar_pointcloud_merger") pointcloud_merger_dir = get_package_share_directory("rslidar_pointcloud_merger")
@ -25,7 +26,7 @@ def generate_launch_description():
output="screen", output="screen",
prefix="taskset -c 0", prefix="taskset -c 0",
) )
] ],
) )
remote_ctrl = TimerAction( remote_ctrl = TimerAction(
@ -37,7 +38,7 @@ def generate_launch_description():
output="screen", output="screen",
prefix="taskset -c 2", prefix="taskset -c 2",
) )
] ],
) )
mc = TimerAction( mc = TimerAction(
@ -49,7 +50,7 @@ def generate_launch_description():
output="screen", output="screen",
prefix="taskset -c 2", prefix="taskset -c 2",
) )
] ],
) )
ctrl_arbiter = TimerAction( ctrl_arbiter = TimerAction(
@ -61,7 +62,7 @@ def generate_launch_description():
output="screen", output="screen",
prefix="taskset -c 1", prefix="taskset -c 1",
) )
] ],
) )
pl = TimerAction( pl = TimerAction(
@ -73,7 +74,7 @@ def generate_launch_description():
output="screen", output="screen",
prefix="taskset -c 3", prefix="taskset -c 3",
) )
] ],
) )
mqtt_report = TimerAction( mqtt_report = TimerAction(
@ -85,7 +86,7 @@ def generate_launch_description():
output="screen", output="screen",
prefix="taskset -c 7", prefix="taskset -c 7",
) )
] ],
) )
lidar_driver = TimerAction( lidar_driver = TimerAction(
@ -96,18 +97,23 @@ def generate_launch_description():
output="screen", output="screen",
prefix="taskset -c 4,5", prefix="taskset -c 4,5",
) )
] ],
) )
pointcloud_merger = TimerAction( pointcloud_merger = TimerAction(
period=3.1, period=3.1,
actions=[ actions=[
ExecuteProcess( 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", output="screen",
prefix="taskset -c 6", # prefix="taskset -c 6",
) )
] ],
) )
rtk = TimerAction( rtk = TimerAction(
@ -119,7 +125,7 @@ def generate_launch_description():
output="screen", output="screen",
prefix="taskset -c 7", prefix="taskset -c 7",
) )
] ],
) )
route = TimerAction( route = TimerAction(
@ -131,7 +137,7 @@ def generate_launch_description():
output="screen", output="screen",
prefix="taskset -c 7", prefix="taskset -c 7",
) )
] ],
) )
sub = TimerAction( sub = TimerAction(
@ -143,7 +149,7 @@ def generate_launch_description():
output="screen", output="screen",
prefix="taskset -c 7", prefix="taskset -c 7",
) )
] ],
) )
task_manager = TimerAction( task_manager = TimerAction(
@ -155,7 +161,7 @@ def generate_launch_description():
output="screen", output="screen",
prefix="taskset -c 7", prefix="taskset -c 7",
) )
] ],
) )
fu = TimerAction( fu = TimerAction(
@ -166,22 +172,24 @@ def generate_launch_description():
output="screen", output="screen",
prefix="taskset -c 7", prefix="taskset -c 7",
) )
] ],
) )
return LaunchDescription([ return LaunchDescription(
vehicle_params, [
can_radio_ctrl, vehicle_params,
remote_ctrl, can_radio_ctrl,
mc, remote_ctrl,
ctrl_arbiter, mc,
pl, ctrl_arbiter,
mqtt_report, pl,
lidar_driver, mqtt_report,
pointcloud_merger, lidar_driver,
rtk, pointcloud_merger,
route, rtk,
sub, route,
task_manager, sub,
fu, task_manager,
]) fu,
]
)

16
src/perception/rslidar_pointcloud_merger/src/merge_two_lidars.cpp
View File

@ -61,12 +61,13 @@
#include <cstring> /* strerror */ #include <cstring> /* strerror */
#endif #endif
#include "logger/logger.h"
#include <rclcpp/rclcpp.hpp> #include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp> #include <sensor_msgs/msg/point_cloud2.hpp>
#include <std_msgs/msg/int32_multi_array.hpp> #include <std_msgs/msg/int32_multi_array.hpp>
#include <std_msgs/msg/multi_array_dimension.hpp> #include <std_msgs/msg/multi_array_dimension.hpp>
#include "logger/logger.h"
using sensor_msgs::msg::PointCloud2; using sensor_msgs::msg::PointCloud2;
/* ================================================================ /* ================================================================
@ -715,9 +716,13 @@ class LidarMerger : public rclcpp::Node
if (filter_brush_) 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; last_stats_time_ = t;
int f = front_rx_.load(), r = rear_rx_.load(), m = merged_count_.load(); 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; 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, 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,
r, m, merge_rate, raw_queue_front_.size(), raw_queue_rear_.size(), processed_queue_.size(), m, merge_rate, raw_queue_front_.size(), raw_queue_rear_.size(), processed_queue_.size(), sor_method_);
sor_method_);
} }
/* ============================================================ /* ============================================================