菜单

二次开发

下载

本篇内容：实现基于Sunray无人机开发平台的二次开发

相关介绍

添加自定义外部定位

如果自定义的外部定位已经符合ENU系即前为X 左为Y 上为Z 的状态那么需要以下操作

如果消息类型为nav_msgs/Odometry 则在external_fusion.launch中修改external_source=0，并将position_topic修改为自定义定位定位话题名

如果消息类型为geometry_msgs/PoseStamped 则在external_fusion.launch中修改external_source=1，并将position_topic修改为自定义定位定位话题名

如果自定义的外部定位坐标系或消息类型不符合则需要自行修改或者在Sunray中添加对应的处理方式

自定义任务控制无人机

在Sunray的控制模块中通过订阅两个话题实现对无人机状态和任务的控制
sunray/setup
消息定义：Sunray/General_Module/sunray_common/sunray_msgs/msg/UAVSetup.msg
消息说明：该消息用于对控制模块中的模式设置、解锁、上锁、停桨等操作，详细功能请阅读消息定义

例：执行解锁
复制代码
rostopic pub /uav1/sunray/setup sunray_msgs/UAVSetup "header:
seq: 0
stamp:
   secs: 0
   nsecs: 0
   frame_id: ''
cmd: 1
px4_mode: ''
control_state: ''" -1
例：切换至CMD_CONTROL
复制代码
rostopic pub /uav1/sunray/setup sunray_msgs/UAVSetup "header:
seq: 0
stamp:
   secs: 0
   nsecs: 0
   frame_id: ''
cmd: 4
px4_mode: ''
control_state: 'CMD_CONTROL'" -1
sunray/uav_control_cmd
消息定义：Sunray/General_Module/sunray_common/sunray_msgs/msg/UAVControlCMD.msg
消息说明：该消息用于对无人机发布控制方式与对应控制项的值，详细功能请阅读消息定义

例：发布位置控制
复制代码
rostopic pub /uav1/sunray/uav_control_cmd sunray_msgs/UAVControlCMD "header:
   seq: 0
   stamp: {secs: 0, nsecs: 0}
   frame_id: ''
cmd: 1
desired_pos: [1.0, 1.0, 1.0]
desired_vel: [0.0, 0.0, 0.0]
desired_acc: [0.0, 0.0, 0.0]
desired_att: [0.0, 0.0, 0.0]
desired_thrust: 0.0
desired_yaw: 0.0
desired_yaw_rate: 0.0
latitude: 0.0
longitude: 0.0
altitude: 0.0"  -1

控制例程

更多二次开发教程可阅读Sunray/General_Module/sunray_tutorial中的程序

起飞降落悬停

复制代码

sunray_msgs::UAVControlCMD uav_cmd;
sunray_msgs::UAVSetup setup;

// 无人机控制指令
ros::Publisher control_cmd_pub = nh.advertise<sunray_msgs::UAVControlCMD>(topic_prefix + "/sunray/uav_control_cmd", 1);
// 无人机设置指令
ros::Publisher uav_setup_pub = nh.advertise<sunray_msgs::UAVSetup>(topic_prefix + "/sunray/setup", 1);


// 切换到指令控制模式(同时，PX4模式将切换至OFFBOARD模式)
while (ros::ok() && uav_state.control_mode != sunray_msgs::UAVSetup::CMD_CONTROL)
    {

        uav_setup.cmd = sunray_msgs::UAVSetup::SET_CONTROL_MODE;
        uav_setup.control_mode = "CMD_CONTROL";
        uav_setup_pub.publish(uav_setup);
        ros::Duration(1.0).sleep();
        ros::spinOnce();
    }

// 解锁
while (ros::ok() && !uav_state.armed)
    {
        uav_setup.cmd = sunray_msgs::UAVSetup::ARM;
        uav_setup_pub.publish(uav_setup);
        ros::Duration(1.0).sleep();
        ros::spinOnce();
    }
    
// 起飞
while (ros::ok() && abs(uav_state.position[2] - uav_state.home_pos[2] - uav_state.takeoff_height) > 0.2)
    {
        uav_cmd.cmd = sunray_msgs::UAVControlCMD::Takeoff;
        control_cmd_pub.publish(uav_cmd);  
        ros::Duration(4.0).sleep();
        ros::spinOnce();
    }
    
// 以上: 无人机已成功起飞，进入自由任务模式

ros::Duration(5).sleep();
// 悬停
uav_cmd.cmd = sunray_msgs::UAVControlCMD::Hover;
control_cmd_pub.publish(uav_cmd);
ros::Duration(5).sleep();
ros::spinOnce();

// 降落
while (ros::ok() && uav_state.control_mode != sunray_msgs::UAVSetup::LAND_CONTROL && uav_state.landed_state != 1)
    {
        uav_cmd.cmd = sunray_msgs::UAVControlCMD::Land;
        control_cmd_pub.publish(uav_cmd);
        ros::Duration(4.0).sleep();
        ros::spinOnce();
    }
    // 等待降落
while (ros::ok() && uav_state.landed_state != 1)
    {
        ros::Duration(1.0).sleep();
        ros::spinOnce();
    }
return 0;
}

位置控制

复制代码

sunray_msgs::UAVControlCMD uav_cmd;
sunray_msgs::UAVSetup setup;

// 无人机控制指令
ros::Publisher control_cmd_pub = nh.advertise<sunray_msgs::UAVControlCMD>(topic_prefix + "/sunray/uav_control_cmd", 1);
// 无人机设置指令
ros::Publisher uav_setup_pub = nh.advertise<sunray_msgs::UAVSetup>(topic_prefix + "/sunray/setup", 1);

// 切换到指令控制模式(同时，PX4模式将切换至OFFBOARD模式)
while (ros::ok() && uav_state.control_mode != sunray_msgs::UAVSetup::CMD_CONTROL)
    {
        uav_setup.cmd = sunray_msgs::UAVSetup::SET_CONTROL_MODE;
        uav_setup.control_mode = "CMD_CONTROL";
        uav_setup_pub.publish(uav_setup);
        ros::Duration(1.0).sleep();
        ros::spinOnce();
    }

// 解锁
while (ros::ok() && !uav_state.armed)
    {
        uav_setup.cmd = sunray_msgs::UAVSetup::ARM;
        uav_setup_pub.publish(uav_setup);
        ros::Duration(1.0).sleep();
        ros::spinOnce();
    }

// 起飞
while (ros::ok() && abs(uav_state.position[2] - uav_state.home_pos[2] - uav_state.takeoff_height) > 0.2)
    {
        uav_cmd.cmd = sunray_msgs::UAVControlCMD::Takeoff;
        control_cmd_pub.publish(uav_cmd);
        ros::Duration(4.0).sleep();
        ros::spinOnce();
    }

    // 以上: 无人机已成功起飞，进入自由任务模式

    ros::Duration(5).sleep();
// 悬停
uav_cmd.cmd = sunray_msgs::UAVControlCMD::Hover;
control_cmd_pub.publish(uav_cmd);
ros::Duration(5).sleep();
ros::spinOnce();

// 定义正方形的顶点,并导入容器储存
std::vector<std::tuple<double, double, double>> vertices = {
        std::make_tuple(0.9, -0.9, 0.8),  // Start point
        std::make_tuple(0.9, 0.9, 0.8),   // Right point
        std::make_tuple(-0.9, 0.9, 0.8),  // Top-right point
        std::make_tuple(-0.9, -0.9, 0.8), // Top-left point
        std::make_tuple(0.9, -0.9, 0.8),  // Start point
        std::make_tuple(0, 0, 0.8)        // Back to start point
    };

for (const auto &vertex : vertices)
    {
        // 输出目标点坐标
        // 在每个顶点发送控制指令直到无人机到达该点
        cout << "go to point: (" << std::get<0>(vertex) << " " << std::get<1>(vertex) << " " << std::get<2>(vertex) << ")" << endl;
        while (ros::ok())
        {
            // 如果停止将直接降落
            if (stop_flag)
            {
                Logger::print_color(int(LogColor::green), node_name, ": Land UAV now.");
                uav_cmd.cmd = sunray_msgs::UAVControlCMD::Land;
                control_cmd_pub.publish(uav_cmd);
                ros::Duration(0.5).sleep();
                break;
            }
            uav_cmd.header.stamp = ros::Time::now();
            uav_cmd.cmd = sunray_msgs::UAVControlCMD::XyzPos;
            uav_cmd.desired_pos[0] = std::get<0>(vertex);
            uav_cmd.desired_pos[1] = std::get<1>(vertex);
            uav_cmd.desired_pos[2] = std::get<2>(vertex);
            control_cmd_pub.publish(uav_cmd);

            if (fabs(uav_state.position[0] - std::get<0>(vertex)) < 0.15 &&
                fabs(uav_state.position[1] - std::get<1>(vertex)) < 0.15 &&
                fabs(uav_state.position[2] - std::get<2>(vertex)) < 0.15)
            {
                // 停下1秒等待无人机速度降下来
                ros::Duration(1.0).sleep();
                break;
            }
            ros::spinOnce();
            rate.sleep();
        }
    }

// 降落
while (ros::ok() && uav_state.control_mode != sunray_msgs::UAVSetup::LAND_CONTROL && uav_state.landed_state != 1)
    {
        uav_cmd.cmd = sunray_msgs::UAVControlCMD::Land;
        control_cmd_pub.publish(uav_cmd);
        ros::Duration(4.0).sleep();
        ros::spinOnce();
    }
    // 等待降落
    while (ros::ok() && uav_state.landed_state != 1)
    {
        ros::Duration(1.0).sleep();
        ros::spinOnce();
    }
    
return 0;
}

上一个

fuel未知环境探索模块

下一个

无人车控制模块

最近修改: 2025-06-16

大纲

分享文章