#!/bin/bash
set -e

source install/setup.bash

# ==============================================
# 1. 车辆参数节点（你之前漏掉的！必须第一个启动）
# ==============================================
taskset -c 0 ros2 run vehicle_params vehicle_params_node &
sleep 0.2

# ==============================================
# 2. 控制类节点（低负载，不影响雷达）
# ==============================================
taskset -c 0 ros2 run can_radio_ctrl can_radio_ctrl_node &
sleep 0.2

taskset -c 2 ros2 run remote_ctrl remote_ctrl_node &
sleep 0.2

taskset -c 1 ros2 run mc mc_node &
sleep 0.2

taskset -c 1 ros2 run ctrl_arbiter ctrl_arbiter_node &
sleep 0.2

taskset -c 2 ros2 run pl pl_node &
sleep 0.2

taskset -c 6 ros2 run mqtt_report mqtt_report_node &
sleep 0.3

# ==============================================
# 3. 雷达驱动 —— 专用核心 3、4（绝不被抢占）
# ==============================================
taskset -c 3,4 ros2 launch rslidar_sdk start_double.launch.py &
sleep 1.5

# ==============================================
# 4. 点云融合 —— 专用核心 5（完全隔离）
# ==============================================
taskset -c 5 ros2 launch rslidar_pointcloud_merger merge_two_lidars.launch.py &
sleep 0.3

# ==============================================
# 5. 定位 & 业务节点（不干扰雷达）
# ==============================================
taskset -c 6 ros2 run rtk rtk_node &
sleep 0.3

taskset -c 6 ros2 run route route_node &
sleep 0.3

taskset -c 6 ros2 run sub sub_node &
sleep 0.3

taskset -c 7 ros2 run task_manager task_manager_node &
sleep 0.3

taskset -c 7 ros2 launch fu fu.launch.py &

wait