菜单

二次开发(改)

下载

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

详细介绍参考：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/General_Module/sunray_uav_control/include/ExternalPosition中的程序实现方式

自定义任务控制无人机

在Sunray的控制模块中通过订阅两个话题实现对无人机状态和任务的控制

sunray/setup

消息定义：Sunray/General_Module/sunray_common/sunray_msgs/msg/UAVSetup.msg
消息说明：该消息用于对控制模块中的模式设置、解锁、上锁、停桨等操作，详细功能请阅读消息定义

  • 消息定义
复制代码 
## 自定义消息：无人机设置指令

## 消息header
std_msgs/Header header

## 无人机Setup类型（可用于模拟遥控器）
uint8 cmd

## 无人机Setup类型枚举
uint8 DISARM = 0                ## 无人机上锁
uint8 ARM = 1                   ## 解锁无人机
uint8 SET_PX4_MODE = 2          ## 设置PX4模式，需要配合px4_mode进行设置
uint8 REBOOT_PX4 = 3            ## 重启PX4飞控
uint8 SET_CONTROL_MODE = 4      ## 设置无人机控制模式，需要配合control_mode进行设置
uint8 EMERGENCY_KILL = 5        ## 无人机紧急上锁

## PX4模式名查询:http://wiki.ros.org/mavros/CustomModes
## 常用模式名:OFFBOARD,AUTO.LAND,AUTO.RTL,POSCTL
string px4_mode

## 参见uav_control.h中无人机控制模式枚举：INIT,RC_CONTROL,CMD_CONTROL,LAND_CONTROL
string control_mode

## 无人机control_mode类型枚举
uint8 INIT = 0                      ## 初始模式
uint8 RC_CONTROL = 1                ## 遥控器控制模式
uint8 CMD_CONTROL = 2               ## 外部指令控制模式
uint8 LAND_CONTROL = 3              ## 降落模式
uint8 WITHOUT_CONTROL = 4           ## 无控制模式
  • 例：执行解锁
复制代码 
rostopic pub /uav1/sunray/setup sunray_msgs/UAVSetup "header:
seq: 0
stamp:
   secs: 0
   nsecs: 0
   frame_id: ''
cmd: 1
px4_mode: ''
control_mode: ''" -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_mode: 'CMD_CONTROL'" -1

  • sunray/uav_control_cmd

消息定义：Sunray/General_Module/sunray_common/sunray_msgs/msg/UAVControlCMD.msg
消息说明：该消息用于对无人机发布控制方式与对应控制项的值，详细功能请阅读消息定义

  • 消息定义
复制代码 
## 自定义消息：无人机控制指令

## 消息header
std_msgs/Header header

## 控制命令
uint8 cmd

## 注意：以下控制类型随PX4版本同、机型不同等，可能存在差异，请根据实际使用情况调整
## 控制命令枚举

## 控制命令枚举 - 基础移动模式
uint8 XyzPos = 1                # 移动指令：惯性系定点控制 保持当前偏航角，需要配合desired_pos使用
uint8 XyzPosYaw = 4             # 移动指令：惯性系定点控制 带偏航角, 需要配合desired_pos和desired_yaw使用
uint8 XyzPosYawrate = 5         # 移动指令：惯性系定点控制 带偏航角速率, 需要配合desired_pos和desired_yaw_rate使用

uint8 XyzVel = 2                # 移动指令：惯性系XYZ速度控制 保持当前偏航角, 需要配合desired_vel使用
uint8 XyzVelYaw = 6             # 移动指令：惯性系XYZ速度控制 带偏航角, 需要配合desired_vel和desired_yaw使用
uint8 XyzVelYawrate = 7         # 移动指令：惯性系XYZ速度控制 带偏航角速率, 需要配合desired_vel和desired_yaw_rate使用

uint8 XyVelZPos = 3             # 移动指令：惯性系XY速度 Z位置 保持当前偏航角, 需要配合desired_vel、desired_pos[2]使用
uint8 XyVelZPosYaw = 8          # 移动指令：惯性系XY速度 Z位置 带偏航角，需要配合desired_vel[0]、desired_vel[1]、desired_pos[2]和desired_yaw使用
uint8 XyVelZPosYawrate = 9      # 移动指令：惯性系XY速度 Z位置 带偏航角速率，需要配合desired_vel[0]、desired_vel[1]、desired_pos[2]和desired_yaw_rate使用

uint8 XyzPosVelYaw = 10         # 移动指令：XYZ位置速度 带偏航角，需要配合desired_pos、desired_vel和desired_yaw使用
uint8 XyzPosVelYawrate = 11     # 移动指令：XYZ位置速度 带偏航角速率，需要配合desired_pos、desired_vel和desired_yaw_rate使用

uint8 PosVelAccYaw = 12         # 移动指令：XYZ位置速度加速度 带偏航角，需要配合desired_pos、desired_vel、desired_acc和desired_yaw使用
uint8 PosVelAccYawrate = 13     # 移动指令：XYZ位置速度加速度 带偏航角速率，需要配合desired_pos、desired_vel、desired_acc和desired_yaw_rate使用

uint8 XyzPosYawBody = 14        # 移动指令：机体坐标系XYZ位置 带偏航角，需要配合desired_pos和desired_yaw使用
uint8 XyzVelYawBody = 15        # 移动指令：机体坐标系XYZ速度 带偏航角，需要配合desired_vel和desired_yaw使用
uint8 XyVelZPosYawBody = 16     # 移动指令：机体坐标系XY速度 Z位置 带偏航角，需要配合desired_vel[0]、desired_vel[1]、desired_pos[2]和desired_yaw使用
uint8 GlobalPos = 17            # 移动指令：全局坐标 经纬度海拔，需要配合latitude、longitude、altitude使用

## 控制命令枚举 - 使用自定义位置控制器进行控制（默认发送期望姿态至PX4飞控）
uint8 CTRL_XyzPos = 50          # 移动指令：惯性系定点控制 带偏航角, 需要配合desired_pos和desired_yaw使用
uint8 CTRL_Traj = 51            # 移动指令：轨迹追踪 需要配合desired_pos、desired_vel、desired_acc和desired_yaw使用

## 控制命令枚举 - 特殊指令（高级模式）
uint8 Point = 30                # 特殊指令：路径规划的目标点
uint8 Takeoff = 100             # 特殊指令：起飞，home点上方悬停
uint8 Land = 101                # 特殊指令：原地降落
uint8 Hover = 102               # 特殊指令：当前位置悬停
uint8 Waypoint = 103            # 特殊指令：航点
uint8 Return = 104              # 特殊指令：返航

## 控制命令期望值
float32[3] desired_pos          ## 期望位置，单位：[m]
float32[3] desired_vel          ## 期望速度，单位：[m/s]
float32[3] desired_acc          ## 期望加速度，单位：[m/s^2]
float32[3] desired_jerk         ## 期望加加速度，单位：[m/s^2]
float32[3] desired_att          ## 期望姿态，单位：[rad]
float32 desired_thrust          ## 期望推力，[0-1]
float32 desired_yaw             ## 期望偏航角，单位：[rad]
float32 desired_yaw_rate        ## 期望偏航角角速率，单位：[rad/s]
float32 latitude                ## 无人机期望经度                 
float32 longitude               ## 无人机期望纬度
float32 altitude                ## 无人机期望高度
  • 例：发布位置控制
复制代码 
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);

// 解锁
cout << "arm" << endl;
ros::Duration(1.0).sleep();
setup.cmd = sunray_msgs::UAVSetup::ARM;
uav_setup_pub.publish(setup);
ros::Duration(1.5).sleep();

// 切换到指令控制模式
cout << "switch CMD_CONTROL" << endl;
setup.cmd = sunray_msgs::UAVSetup::SET_CONTROL_MODE;
setup.control_mode = "CMD_CONTROL";
uav_setup_pub.publish(setup);
ros::Duration(1.0).sleep();

// 起飞
cout << "takeoff" << endl;
uav_cmd.cmd = sunray_msgs::UAVControlCMD::Takeoff;
control_cmd_pub.publish(uav_cmd);
// 等待起飞
ros::Duration(10).sleep();

// 悬停
cout << "hover" << endl;
uav_cmd.cmd = sunray_msgs::UAVControlCMD::Hover;
control_cmd_pub.publish(uav_cmd);
// 悬停5秒
ros::Duration(5).sleep();

// 降落
cout << "land" << endl;
uav_cmd.cmd = sunray_msgs::UAVControlCMD::Land;
control_cmd_pub.publish(uav_cmd);
ros::Duration(0.5).sleep();
  • 位置控制
复制代码 
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);

// 定义正方形的顶点,并导入容器储存
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
};

// 解锁
cout << "arm" << endl;
setup.cmd = sunray_msgs::UAVSetup::ARM;**
uav_setup_pub.publish(setup);
ros::Duration(1.5).sleep();

// 切换到指令控制模式
cout << "switch CMD_CONTROL" << endl;
setup.cmd = sunray_msgs::UAVSetup::SET_CONTROL_MODE;
setup.control_mode = "CMD_CONTROL";
uav_setup_pub.publish(setup);
ros::Duration(1.0).sleep();

// 起飞
cout << "takeoff" << endl;
uav_cmd.cmd = sunray_msgs::UAVControlCMD::Takeoff;
control_cmd_pub.publish(uav_cmd);
ros::Duration(10).sleep();

// 悬停
cout << "hover" << endl;
uav_cmd.cmd = sunray_msgs::UAVControlCMD::Hover;
control_cmd_pub.publish(uav_cmd);
ros::Duration(2).sleep();

for (const auto &vertex : vertices)
{
    // 输出目标点坐标
    // 在每个顶点发送控制指令直到无人机到达该点
    cout << "go to point: (" << std::get<0>(vertex) << " " << std::get<1>(vertex) << " " << std::get<2>(vertex) << ")" << endl;
    uav_cmd.header.stamp = ros::Time::now();
    uav_cmd.cmd = 1;
    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);
    // 等待无人机到达位置
    ros::Duration(5).sleep();
}

// 降落
cout << "land" << endl;
uav_cmd.cmd = sunray_msgs::UAVControlCMD::Land;
control_cmd_pub.publish(uav_cmd);
ros::Duration(0.5).sleep();
上一个
单二维码与自动跟随(改)
下一个
Sunray地面站
最近修改: 2025-09-04