<think>这是一个使用ROS（Robot Operating System）和MAVROS（MAVLink to ROS bridge）控制无人机（如PX4飞控）的节点程序。代码的主要功能是让无人机起飞到指定高度，然后悬停，并能够响应降落指令和动态目标点更新。下面我将详细解释代码的各个部分。
### 1. 头文件
```cpp
#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <mavros_msgs/State.h>
#include <mavros_msgs/PositionTarget.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <std_msgs/Bool.h>
#include <cmath>
```
- 这些头文件引入了所需的ROS消息类型和服务类型，用于与飞控通信。
- `geometry_msgs/PoseStamped.h`：带时间戳的位置和姿态消息。
- `mavros_msgs/State.h`：飞控状态（如模式、是否解锁等）。
- `mavros_msgs/PositionTarget.h`：用于设置位置、速度或加速度目标。
- `mavros_msgs/CommandBool.h`：用于解锁/锁定飞控的服务。
- `mavros_msgs/SetMode.h`：用于设置飞行模式的服务。
- `std_msgs/Bool.h`：布尔类型消息，用于降落指令。
- `cmath`：数学函数库，例如`std::sqrt`。
### 2. 全局变量
```cpp
ros::Publisher pos_raw_pub;
mavros_msgs::State current_state;
geometry_msgs::PoseStamped current_pose;
bool land_command_received = false;
geometry_msgs::Point target_position;
```
- `pos_raw_pub`：发布`mavros_msgs::PositionTarget`消息的发布器，用于发送控制指令。
- `current_state`：保存当前飞控状态（如模式、是否解锁）。
- `current_pose`：保存当前无人机的位置和姿态。
- `land_command_received`：标记是否收到降落指令。
- `target_position`：存储目标位置（来自`target_point`话题）。
### 3. 回调函数
```cpp
void state_cb(const mavros_msgs::State::ConstPtr& msg) {
current_state = *msg;
}
```
- 飞控状态回调，更新`current_state`。
```cpp
void pose_cb(const geometry_msgs::PoseStamped::ConstPtr& msg) {
current_pose = *msg;
}
```
- 当前位置回调，更新`current_pose`。
```cpp
void landCommandCallback(const std_msgs::Bool::ConstPtr& msg) {
land_command_received = msg->data;
}
```
- 降落指令回调，当收到`land_cmd`话题的消息时，设置`land_command_received`。
```cpp
void targetPointCallback(const geometry_msgs::Point::ConstPtr& msg) {
target_position = *msg; // 更新目标点
}
```
- 目标点回调，更新`target_position`。
### 4. 辅助函数
```cpp
mavros_msgs::PositionTarget makePositionTarget(...) { ... }
```
- 用于构造`PositionTarget`消息，通过参数设置位置、速度、偏航角等，并指定忽略哪些字段（通过`type_mask`）。
```cpp
bool reached_target(double x, double y, double z, double threshold) {
double dx = current_pose.pose.position.x - x;
double dy = current_pose.pose.position.y - y;
double dz = current_pose.pose.position.z - z;
return (std::sqrt(dx*dx + dy*dy + dz*dz) < threshold);
}
```
- 判断当前位置是否到达目标位置（欧氏距离小于阈值）。
### 5. main函数
#### 初始化
```cpp
ros::init(argc, argv, "offboard_node");
ros::NodeHandle nh;
```
- 初始化ROS节点，节点名为`offboard_node`。
#### 订阅器和发布器
```cpp
ros::Subscriber state_sub = nh.subscribe("mavros/state", 10, state_cb);
ros::Subscriber pose_sub = nh.subscribe("mavros/local_position/pose", 100, pose_cb);
ros::Subscriber land_cmd_sub = nh.subscribe("land_cmd", 1, landCommandCallback);
ros::Subscriber target_cmd_sub = nh.subscribe("target_point", 1, targetPointCallback);
pos_raw_pub = nh.advertise<mavros_msgs::PositionTarget>("mavros/setpoint_raw/local", 100);
```
- 订阅飞控状态、本地位置、降落指令和目标点话题。
- 发布`PositionTarget`消息到飞控。
#### 服务客户端
```cpp
ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>("mavros/cmd/arming");
ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>("mavros/set_mode");
```
- 用于解锁和设置模式的服务客户端。
#### 等待连接
```cpp
while (ros::ok() && !current_state.connected) {
ros::spinOnce();
rate.sleep();
}
```
- 等待飞控连接。
#### 起飞前发送设定点
```cpp
for (int i = 0; i < 100; ++i) {
// 发送位置目标（高度1米，忽略速度、加速度等）
pos_raw_pub.publish(...);
ros::spinOnce();
rate.sleep();
}
```
- 在进入OFFBOARD模式前，需要持续发送设定点（至少几Hz），否则飞控会拒绝进入OFFBOARD模式。
#### 设置OFFBOARD模式和解锁
```cpp
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() && !current_state.armed) {
// 尝试设置OFFBOARD模式
if (当前不是OFFBOARD模式且超过1秒) {
set_mode_client.call(offb_set_mode);
last_request = now;
}
// 尝试解锁
else if (未解锁且超过1秒) {
arming_client.call(arm_cmd);
last_request = now;
}
// 发送起飞指令（向上速度控制）
pos_raw_pub.publish(...);
...
}
```
- 循环尝试设置OFFBOARD模式和解锁，同时持续发送控制指令（向上速度控制，使无人机起飞）。
#### 起飞到目标高度
```cpp
double target_x = 0.0, target_y = 0.0, target_z = 1.0;
while (!reached_target(target_x, target_y, target_z, 0.1)) {
// 发送位置目标（高度1米，并设置向上的速度0.3 m/s）
pos_raw_pub.publish(...);
...
}
```
- 控制无人机飞到目标位置（0,0,1），使用位置控制，但设置了一个向上的速度（0.3 m/s）以帮助上升。
#### 悬停和响应降落指令
```cpp
bool has_landed = false;
while (ros::ok() && !has_landed) {
// 如果已经到达目标位置，则悬停；否则向目标位置移动（设置速度2.0 m/s？）
if (reached_target(target_position, 0.1)) {
// 发送悬停指令（位置目标，忽略速度）
...
} else {
// 发送位置目标，并设置向目标位置的速度（这里x方向速度2.0？）
// 注意：这里设置的type_mask忽略了速度，所以实际不会使用速度控制，而是位置控制？
// 但代码中设置了IGNORE_VX等，所以实际上发布的是位置目标，速度被忽略。
...
}
// 检查降落指令
if (land_command_received) {
// 设置AUTO.LAND模式
mavros_msgs::SetMode land_mode;
land_mode.request.custom_mode = "AUTO.LAND";
set_mode_client.call(land_mode);
// 等待降落完成（检测是否解锁）
while (ros::ok() && current_state.armed) {
// 等待直到解锁（即降落完成）
// 设置超时，如果超时则尝试手动解锁
...
}
has_landed = true;
}
ros::spinOnce();
rate_1.sleep();
}
```
- 在悬停循环中，不断检查是否到达目标位置（动态更新的`target_position`），如果到达则悬停，否则向目标位置移动（注意：代码中设置的速度被忽略，因为type_mask中忽略了速度，所以实际上使用的是位置控制）。
- 当收到降落指令时，切换到AUTO.LAND模式，并等待无人机降落并解锁。
### 代码中的问题
1. **速度控制与位置控制混淆**：在悬停循环中，当未到达目标位置时，发布的位置目标消息中设置了`velocity.x=2.0`，但由于`type_mask`中忽略了速度（`IGNORE_VX`等），所以这个速度值实际上不会被飞控使用。因此，无人机仍然会使用位置控制模式飞向目标位置，而不是速度控制。这可能导致无人机移动过快而飞过目标点。
2. **目标点更新逻辑**：目标点通过`target_point`话题更新，但在悬停循环中，如果目标点更新到一个较远的位置，无人机可能会突然改变目标位置，导致飞行路径不连续。
3. **悬停循环中的控制频率**：主悬停循环使用`rate_1(50.0)`，但之前使用`rate(100.0)`，这可能导致控制频率不一致。
4. **起飞阶段的速度控制**：在起飞阶段，代码中发送了一个向上的速度（0.3 m/s），但同时也设置了目标位置（0,0,1）。由于`type_mask`忽略了速度，所以实际上飞控使用的是位置控制，速度设置无效。因此，无人机将直接飞向目标位置，而不会以0.3m/s的速度上升。
5. **降落模式切换后**：在切换到AUTO.LAND模式后，代码停止发送位置目标，这是正确的，因为AUTO.LAND模式不需要外部控制指令。
### 改进建议
1. 明确控制方式：如果要使用位置控制，则不要设置速度；如果要使用速度控制，则忽略位置。在`makePositionTarget`函数中，通过`type_mask`明确指定。
2. 在悬停循环中，如果目标点更新，应该使用位置控制平滑飞向目标点，或者使用速度控制（需要根据当前位置和目标位置计算速度向量）。
3. 统一控制频率，避免不同频率导致的问题。
### 总结
该代码实现了一个基本的无人机控制节点，能够起飞到指定高度，悬停，响应外部目标点更新和降落指令。但需要注意控制方式（位置/速度）的明确，以及目标点更新的处理。
§§相关问题§§:
1. 在ROS中，如何正确使用MAVROS控制无人机起飞并悬停？
2. 在OFFBOARD模式下，位置控制和速度控制有什么区别？如何选择？
3. 如何实现无人机平滑移动到动态更新的目标点？
4. 在自动降落过程中，为什么需要停止发送控制指令？</think>### 代码功能概述
这是一个基于ROS和MAVROS的无人机控制节点，主要实现以下功能：
1. 连接飞控并进入OFFBOARD模式
2. 自动起飞到指定高度
3. 悬停并响应动态目标点更新
4. 接收降落指令并执行自动降落
5. 实现位置控制逻辑
### 代码详细解释
#### 1. 头文件和全局变量
```cpp
#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <mavros_msgs/State.h>
#include <mavros_msgs/PositionTarget.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <std_msgs/Bool.h>
#include <cmath>
// 全局变量
ros::Publisher pos_raw_pub; // 控制指令发布器
mavros_msgs::State current_state; // 当前飞控状态
geometry_msgs::PoseStamped current_pose; // 当前无人机位置
bool land_command_received = false; // 降落指令标志
geometry_msgs::Point target_position; // 目标位置
```
#### 2. 回调函数
```cpp
// 飞控状态回调
void state_cb(const mavros_msgs::State::ConstPtr& msg) {
current_state = *msg;
}
// 当前位置回调
void pose_cb(const geometry_msgs::PoseStamped::ConstPtr& msg) {
current_pose = *msg;
}
// 降落指令回调
void landCommandCallback(const std_msgs::Bool::ConstPtr& msg) {
land_command_received = msg->data;
}
// 目标点更新回调
void targetPointCallback(const geometry_msgs::Point::ConstPtr& msg) {
target_position = *msg;
}
```
#### 3. 控制消息构造函数
```cpp
// 构造PositionTarget控制消息
mavros_msgs::PositionTarget makePositionTarget(
float x, float y, float z,
float vx, float vy, float vz,
float yaw,
uint16_t type_mask)
{
mavros_msgs::PositionTarget target;
target.header.stamp = ros::Time::now();
target.coordinate_frame = mavros_msgs::PositionTarget::FRAME_LOCAL_NED; // 使用局部NED坐标系
target.type_mask = type_mask; // 指定忽略哪些控制字段
target.position.x = x;
target.position.y = y;
target.position.z = z;
target.velocity.x = vx;
target.velocity.y = vy;
target.velocity.z = vz;
target.yaw = yaw; // 偏航角（弧度）
return target;
}
```
#### 4. 位置判断函数
```cpp
// 判断是否到达目标位置
bool reached_target(double x, double y, double z, double threshold) {
double dx = current_pose.pose.position.x - x;
double dy = current_pose.pose.position.y - y;
double dz = current_pose.pose.position.z - z;
return (std::sqrt(dx*dx + dy*dy + dz*dz) < threshold); // 欧氏距离判断
}
```
#### 5. 主函数逻辑
```cpp
int main(int argc, char **argv) {
// ROS初始化
ros::init(argc, argv, "offboard_node");
ros::NodeHandle nh;
// 订阅器初始化
ros::Subscriber state_sub = nh.subscribe("mavros/state", 10, state_cb);
ros::Subscriber pose_sub = nh.subscribe("mavros/local_position/pose", 100, pose_cb);
ros::Subscriber land_cmd_sub = nh.subscribe("land_cmd", 1, landCommandCallback);
ros::Subscriber target_cmd_sub = nh.subscribe("target_point", 1, targetPointCallback);
// 发布器初始化
pos_raw_pub = nh.advertise<mavros_msgs::PositionTarget>("mavros/setpoint_raw/local", 100);
// 服务客户端初始化
ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>("mavros/cmd/arming");
ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>("mavros/set_mode");
// 控制频率设置
ros::Rate rate(100.0); // 100Hz主循环
ros::Rate rate_1(50.0); // 50Hz悬停循环
// 等待飞控连接
while (ros::ok() && !current_state.connected) {
ros::spinOnce();
rate.sleep();
}
// 起飞前发送设定点（惯例要求）
for (int i = 0; i < 100; ++i) {
auto target = makePositionTarget(0, 0, 1.0, 0, 0, 0, 0,
mavros_msgs::PositionTarget::IGNORE_VX |
mavros_msgs::PositionTarget::IGNORE_VY |
mavros_msgs::PositionTarget::IGNORE_VZ |
mavros_msgs::PositionTarget::IGNORE_AFX |
mavros_msgs::PositionTarget::IGNORE_AFY |
mavros_msgs::PositionTarget::IGNORE_AFZ |
mavros_msgs::PositionTarget::IGNORE_YAW_RATE);
pos_raw_pub.publish(target);
ros::spinOnce();
rate.sleep();
}
// 设置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();
// 进入OFFBOARD模式并解锁
while (ros::ok() && !current_state.armed) {
// 尝试设置OFFBOARD模式
if (current_state.mode != "OFFBOARD" &&
(ros::Time::now() - last_request > ros::Duration(1.0))) {
if (set_mode_client.call(offb_set_mode) && offb_set_mode.response.mode_sent) {
ROS_INFO("Offboard mode enabled");
}
last_request = ros::Time::now();
}
// 尝试解锁
else if (!current_state.armed &&
(ros::Time::now() - last_request > ros::Duration(1.0))) {
if (arming_client.call(arm_cmd) && arm_cmd.response.success) {
ROS_INFO("Vehicle armed");
}
last_request = ros::Time::now();
}
// 发送起飞指令（向上速度控制）
auto takeoff_target = makePositionTarget(0, 0, 1.0, 0, 0, 0, 0,
mavros_msgs::PositionTarget::IGNORE_VX |
mavros_msgs::PositionTarget::IGNORE_VY |
mavros_msgs::PositionTarget::IGNORE_AFX |
mavros_msgs::PositionTarget::IGNORE_AFY |
mavros_msgs::PositionTarget::IGNORE_AFZ |
mavros_msgs::PositionTarget::IGNORE_YAW_RATE);
pos_raw_pub.publish(takeoff_target);
ros::spinOnce();
rate.sleep();
}
// === 位置控制到目标点 ===
double target_x = 0.0, target_y = 0.0, target_z = 1.0;
target_position.x = target_x;
target_position.y = target_y;
target_position.z = target_z;
while (ros::ok() && !reached_target(target_x, target_y, target_z, 0.1)) {
auto move_target = makePositionTarget(target_x, target_y, target_z, 0, 0, 0.3, 0,
mavros_msgs::PositionTarget::IGNORE_VX |
mavros_msgs::PositionTarget::IGNORE_VY |
mavros_msgs::PositionTarget::IGNORE_AFX |
mavros_msgs::PositionTarget::IGNORE_AFY |
mavros_msgs::PositionTarget::IGNORE_AFZ |
mavros_msgs::PositionTarget::IGNORE_YAW_RATE);
pos_raw_pub.publish(move_target);
ros::spinOnce();
rate.sleep();
}
// === 悬停和响应指令的主循环 ===
bool has_landed = false;
while (ros::ok() && !has_landed) {
// 根据目标点位置决策控制方式
if(reached_target(target_position.x, target_position.y, target_position.z, 0.1)) {
// 悬停控制（位置保持）
auto hover_target = makePositionTarget(
target_position.x, target_position.y, target_position.z,
0, 0, 0, 0,
mavros_msgs::PositionTarget::IGNORE_VX |
mavros_msgs::PositionTarget::IGNORE_VY |
mavros_msgs::PositionTarget::IGNORE_VZ |
mavros_msgs::PositionTarget::IGNORE_AFX |
mavros_msgs::PositionTarget::IGNORE_AFY |
mavros_msgs::PositionTarget::IGNORE_AFZ |
mavros_msgs::PositionTarget::IGNORE_YAW_RATE);
pos_raw_pub.publish(hover_target);
} else {
// 向目标点移动（速度控制）
auto move_target = makePositionTarget(
target_position.x, target_position.y, target_position.z,
2.0, 0, 0, 0, // X方向速度2m/s
mavros_msgs::PositionTarget::IGNORE_VX | // 注意：这里实际是位置控制
mavros_msgs::PositionTarget::IGNORE_VY |
mavros_msgs::PositionTarget::IGNORE_VZ |
mavros_msgs::PositionTarget::IGNORE_AFX |
mavros_msgs::PositionTarget::IGNORE_AFY |
mavros_msgs::PositionTarget::IGNORE_AFZ |
mavros_msgs::PositionTarget::IGNORE_YAW_RATE);
pos_raw_pub.publish(move_target);
}
// 处理降落指令
if (land_command_received) {
mavros_msgs::SetMode land_mode;
land_mode.request.custom_mode = "AUTO.LAND";
if (set_mode_client.call(land_mode) && land_mode.response.mode_sent) {
ROS_INFO("AUTO.LAND mode set");
}
// 等待降落完成
ros::Time land_start = ros::Time::now();
while (ros::ok()) {
ros::spinOnce();
// 检测是否已解锁（降落完成）
if (!current_state.armed) {
ROS_INFO("Landing complete. Vehicle disarmed.");
break;
}
// 超时处理（20秒）
if ((ros::Time::now() - land_start).toSec() > 20.0) {
ROS_WARN("Landing timeout. Attempting manual disarm...");
mavros_msgs::CommandBool disarm_cmd;
disarm_cmd.request.value = false;
if (arming_client.call(disarm_cmd) && disarm_cmd.response.success) {
ROS_INFO("Manual disarm succeeded.");
} else {
ROS_WARN("Manual disarm failed.");
}
break;
}
rate.sleep();
}
has_landed = true;
}
ros::spinOnce();
rate_1.sleep(); // 50Hz循环
}
return 0;
}
```
### 关键功能说明
1. **初始化与连接**：
- 等待飞控连接（`current_state.connected`）
- 起飞前发送100个设定点（MAVROS要求）
2. **模式设置与解锁**：
- 设置OFFBOARD模式（`set_mode_client.call(offb_set_mode)`）
- 解锁飞控（`arming_client.call(arm_cmd)`）
- 持续发送控制指令保持连接
3. **起飞与位置控制**：
- 飞到初始目标点(0,0,1)
- 使用`reached_target()`判断位置误差
- 控制类型：位置+速度混合（Z轴有0.3m/s上升速度）
4. **悬停与目标跟踪**：
- 动态响应`target_point`话题更新
- 根据目标距离选择悬停或移动
- 两种控制策略：
* 到达目标：纯位置控制
* 未到达：X方向速度控制（实际是位置控制）
5. **降落处理**：
- 响应`land_cmd`话题指令
- 切换到AUTO.LAND模式
- 等待降落完成（检测armed状态）
- 20秒超时后强制解锁
### 代码优化建议
1. **控制逻辑改进**：
```cpp
// 改进的位置控制逻辑（示例）
double kp = 0.5; // 比例系数
double dx = target_position.x - current_pose.pose.position.x;
double dy = target_position.y - current_pose.pose.position.y;
double dz = target_position.z - current_pose.pose.position.z;
double vx = kp * dx;
double vy = kp * dy;
double vz = kp * dz;
// 构造速度控制指令
auto move_target = makePositionTarget(
0, 0, 0, // 位置忽略
vx, vy, vz, 0,
mavros_msgs::PositionTarget::IGNORE_PX |
mavros_msgs::PositionTarget::IGNORE_PY |
mavros_msgs::PositionTarget::IGNORE_PZ |
...);
```
2. **安全增强**：
- 添加紧急停止机制
- 实现返航(RTL)功能
- 添加位置超限检查
3. **参数可配置化**：
```cpp
// 从参数服务器获取
double takeoff_height;
nh.param("takeoff_height", takeoff_height, 1.0);
```
4. **状态机实现**：
```cpp
enum State { DISARMED, TAKEOFF, HOVER, MOVING, LANDING };
State current_state = DISARMED;
```
扫一扫
1万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
