多架无人机解锁并设置为offboard模式

写了c++代码实现：多架无人机解锁并设置为offboard模式。但代码运行后，无人机不能变成offboard模式，但可以解锁。
代码如下：

#include <ros/ros.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>

// 定义一个长度为10的无人机状态数组
mavros_msgs::State current_state[10];

// 回调函数，用于更新当前无人机状态数组中的状态
void state_cb(const mavros_msgs::State::ConstPtr &msg, int index)
{
    current_state[index] = *msg;
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "formation_offb_node");
    ros::NodeHandle nh;

    // 从命令行参数中读取无人机数量
    int N = std::stoi(argv[1]);

    // // 订阅无人机状态、解锁和切换模式的服务客户端
    // ros::Subscriber state_sub[N];
    // ros::ServiceClient arming_client[N];
    // ros::ServiceClient set_mode_client[N];
    // for (int i = 0; i < N; i++)
    // {
    //     // 订阅状态话题
    //     state_sub[i] = nh.subscribe<mavros_msgs::State>("iris_" + std::to_string(i) + "/mavros/state", 10, boost::bind(state_cb, _1, i));
    //     // 解锁客户端
    //     arming_client[i] = nh.serviceClient<mavros_msgs::CommandBool>("iris_" + std::to_string(i) + "/mavros/cmd/arming");
    //     // 切换模式客户端
    //     set_mode_client[i] = nh.serviceClient<mavros_msgs::SetMode>("iris_" + std::to_string(i) + "/mavros/set_mode");
    // }
    // 订阅无人机状态、解锁和切换模式的服务客户端
    std::vector<ros::Subscriber> state_sub;
    std::vector<ros::ServiceClient> arming_client;
    std::vector<ros::ServiceClient> set_mode_client;
    for (int i = 0; i < N; i++)
    {
        // 订阅状态话题
        state_sub.push_back(nh.subscribe<mavros_msgs::State>("iris_" + std::to_string(i) + "/mavros/state", 10, boost::bind(state_cb, _1, i)));
        // 解锁客户端
        arming_client.push_back(nh.serviceClient<mavros_msgs::CommandBool>("iris_" + std::to_string(i) + "/mavros/cmd/arming"));
        // 切换模式客户端
        set_mode_client.push_back(nh.serviceClient<mavros_msgs::SetMode>("iris_" + std::to_string(i) + "/mavros/set_mode"));
    }

    // 设置发送速率
    ros::Rate rate(20.0);

    // 设置切换到OFFBOARD模式的请求
    mavros_msgs::SetMode offb_set_mode;
    offb_set_mode.request.custom_mode = "OFFBOARD";

    // 设置解锁的请求
    mavros_msgs::CommandBool arm_cmd;
    arm_cmd.request.value = true;

    // 循环检查每个无人机的状态并进行解锁和切换模式的操作
    ros::Time last_request = ros::Time::now();
    while (ros::ok())
    {
          for (int i = 0; i < N; i++)
      {
          set_mode_client[i].call(offb_set_mode);
          arming_client[i].call(arm_cmd);
      }
        ros::spinOnce();
        rate.sleep();
    }

    return 0;
}

请问该如何修改代码，实现功能。

#include <ros/ros.h>  
#include <mavros_msgs/CommandBool.h>  
#include <mavros_msgs/SetMode.h>  
#include <mavros_msgs/State.h>  
   
// 定义一个长度为10的无人机状态数组  
mavros_msgs::State current_state[10];  
   
// 回调函数，用于更新当前无人机状态数组中的状态  
void state_cb(const mavros_msgs::State::ConstPtr &msg, int index)  
{  
    current_state[index] = *msg;  
}  
   
int main(int argc, char **argv)  
{  
    ros::init(argc, argv, "formation_offb_node");  
    ros::NodeHandle nh;  
   
    // 从命令行参数中读取无人机数量  
    int N = std::stoi(argv[1]);  
   
    // // 订阅无人机状态、解锁和切换模式的服务客户端  
    // ros::Subscriber state_sub[N];  
    // ros::ServiceClient arming_client[N];  
    // ros::ServiceClient set_mode_client[N];  
    // for (int i = 0; i < N; i++)  
    // {  
    //     // 订阅状态话题  
    //     state_sub[i] = nh.subscribe<mavros_msgs::State>("iris_" + std::to_string(i) + "/mavros/state", 10, boost::bind(state_cb, _1, i));  
    //     // 解锁客户端  
    //     arming_client[i] = nh.serviceClient<mavros_msgs::CommandBool>("iris_" + std::to_string(i) + "/mavros/cmd/arming");  
    //     // 切换模式客户端  
    //