ROSワークスペースを構築し、PX4とMAVROSを統合したシミュレーション環境を準備します。

ワークスペースの初期化

mkdir -p ~/px4_control_ws/src
cd ~/px4_control_ws
catkin_make
source devel/setup.bash
echo "source ~/px4_control_ws/devel/setup.bash" >> ~/.bashrc
source ~/.bashrc

次に、オフボード制御ノードを含むパッケージ（例：`drone_control_pkg`）をsrc/内に配置します。

シミュレーションの起動

GazeboとPX4 SITLを連携させるため、以下のコマンドで環境を立ち上げます：

roslaunch px4 gazebo_sitl_default.launch

別ターミナルからQGroundControl（QGC）を起動し、UDPポート14540で接続を確立します。

基本的なオフボード飛行制御（C++）

以下は、ドローンを高度2mまで垂直離陸させる最小限のROSノード実装です。状態監視・モード切替・アーム処理を含み、安全なセットポイント送信周期（20Hz）を保証します。

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

mavros_msgs::State drone_status;

void status_callback(const mavros_msgs::State::ConstPtr& msg) {
    drone_status = *msg;
}

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

    const std::string state_topic = "/mavros/state";
    const std::string setpoint_topic = "/mavros/setpoint_position/local";
    const std::string arming_service = "/mavros/cmd/arming";
    const std::string mode_service = "/mavros/set_mode";

    ros::Subscriber status_sub = node_handle.subscribe(state_topic, 10, status_callback);
    ros::Publisher position_pub = node_handle.advertise<geometry_msgs::PoseStamped>(setpoint_topic, 10);
    ros::ServiceClient arming_client = node_handle.serviceClient<mavros_msgs::CommandBool>(arming_service);
    ros::ServiceClient mode_client = node_handle.serviceClient<mavros_msgs::SetMode>(mode_service);

    ros::Rate loop_rate(20.0); // 必須：2Hz以上

    // FCU接続待機
    while (ros::ok() && !drone_status.connected) {
        ros::spinOnce();
        loop_rate.sleep();
    }

    geometry_msgs::PoseStamped target_pose;
    target_pose.pose.position.x = 0.0;
    target_pose.pose.position.y = 0.0;
    target_pose.pose.position.z = 2.0;

    // 初期セットポイントを100回送信（安定化用）
    for (int i = 0; i < 100 && ros::ok(); ++i) {
        position_pub.publish(target_pose);
        ros::spinOnce();
        loop_rate.sleep();
    }

    mavros_msgs::SetMode offboard_req;
    offboard_req.request.custom_mode = "OFFBOARD";

    mavros_msgs::CommandBool arm_req;
    arm_req.request.value = true;

    ros::Time last_cmd_time = ros::Time::now();

    while (ros::ok()) {
        if (drone_status.mode != "OFFBOARD" && (ros::Time::now() - last_cmd_time > ros::Duration(5.0))) {
            if (mode_client.call(offboard_req) && offboard_req.response.mode_sent) {
                ROS_INFO("OFFBOARD mode activated");
            }
            last_cmd_time = ros::Time::now();
        }

        if (!drone_status.armed && (ros::Time::now() - last_cmd_time > ros::Duration(5.0))) {
            if (arming_client.call(arm_req) && arm_req.response.success) {
                ROS_INFO("Drone armed successfully");
            }
            last_cmd_time = ros::Time::now();
        }

        position_pub.publish(target_pose);
        ros::spinOnce();
        loop_rate.sleep();
    }

    return 0;
}

巡回飛行の実装（矩形軌道）

次のコードは、原点→(2,2,1.5)→(0,2,1.5)→(0,0,1.5)の4点を順に経由する簡易ループ制御を実現します。各目標点へ到達後、5秒間停留します。

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

mavros_msgs::State current_status;

void status_update(const mavros_msgs::State::ConstPtr& msg) {
    current_status = *msg;
}

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

    ros::Subscriber sub = nh.subscribe("/mavros/state", 10, status_update);
    ros::Publisher pub = nh.advertise<geometry_msgs::PoseStamped>("/mavros/setpoint_position/local", 10);
    ros::ServiceClient arming = nh.serviceClient<mavros_msgs::CommandBool>("/mavros/cmd/arming");
    ros::ServiceClient mode_ctl = nh.serviceClient<mavros_msgs::SetMode>("/mavros/set_mode");

    ros::Rate rate(20);

    // 接続確認
    while (ros::ok() && !current_status.connected) {
        ros::spinOnce();
        rate.sleep();
    }

    // アーム＆OFFBOARD切り替え（最大3回リトライ）
    int retries = 0;
    while (ros::ok() && retries < 3) {
        if (!current_status.armed) {
            mavros_msgs::CommandBool req;
            req.request.value = true;
            if (arming.call(req) && req.response.success) {
                ROS_INFO("Armed");
                break;
            }
        }
        retries++;
        ros::Duration(1.0).sleep();
    }

    mavros_msgs::SetMode mode_req;
    mode_req.request.custom_mode = "OFFBOARD";
    mode_ctl.call(mode_req);

    // ナビゲーションポイント列
    std::vector<std::vector<double>> waypoints = {
        {0.0, 0.0, 1.5},
        {2.0, 2.0, 1.5},
        {0.0, 2.0, 1.5},
        {0.0, 0.0, 1.5}
    };

    geometry_msgs::PoseStamped cmd;
    for (const auto& wp : waypoints) {
        cmd.pose.position.x = wp[0];
        cmd.pose.position.y = wp[1];
        cmd.pose.position.z = wp[2];

        ros::Time start = ros::Time::now();
        while (ros::ok() && (ros::Time::now() - start < ros::Duration(5.0))) {
            pub.publish(cmd);
            ros::spinOnce();
            rate.sleep();
        }
        ROS_INFO_STREAM("Reached waypoint: [" << wp[0] << ", " << wp[1] << ", " << wp[2] << "]");
    }

    return 0;
}