/**
* @file uav_fsm_node.cpp
* @brief UAV Takeoff and U-Shaped Path Flight for Simulation
* @version 6.1
* @date 2023-10-15
*
* 针对仿真环境的优化:
* 1. 修复坐标系问题(使用ENU坐标系的数值表示)
* 2. 简化起飞控制逻辑
* 3. 增强错误检测和日志输出
* 4. 优化参数默认值
* 5. 确保控制指令正确发送
*/
#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/TwistStamped.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>
#include <mavros_msgs/CommandTOL.h>
#include <mavros_msgs/PositionTarget.h>
#include <mavros_msgs/TimesyncStatus.h>
#include <cmath>
#include <vector>
#include <algorithm>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <angles/angles.h>
// 全局变量
mavros_msgs::State current_state;
geometry_msgs::PoseStamped current_pose;
geometry_msgs::TwistStamped current_vel;
mavros_msgs::TimesyncStatus timesync_status;
bool pose_received = false;
bool vel_received = false;
bool timesync_received = false;
// 状态回调
void state_cb(const mavros_msgs::State::ConstPtr& msg) {
current_state = *msg;
}
// 位置回调
void pose_cb(const geometry_msgs::PoseStamped::ConstPtr& msg) {
current_pose = *msg;
pose_received = true;
}
// 速度回调
void vel_cb(const geometry_msgs::TwistStamped::ConstPtr& msg) {
current_vel = *msg;
vel_received = true;
}
// 时间同步回调
void timesync_cb(const mavros_msgs::TimesyncStatus::ConstPtr& msg) {
timesync_status = *msg;
timesync_received = true;
}
// 计算两点之间的距离
double distance(const geometry_msgs::Point& p1, const geometry_msgs::Point& p2) {
double dx = p1.x - p2.x;
double dy = p1.y - p2.y;
double dz = p1.z - p2.z;
return std::sqrt(dx*dx + dy*dy + dz*dz);
}
// 计算航点与位置之间的距离
double distance_to_waypoint(const geometry_msgs::Point& position, double wx, double wy, double wz) {
double dx = position.x - wx;
double dy = position.y - wy;
double dz = position.z - wz;
return std::sqrt(dx*dx + dy*dy + dz*dz);
}
// 检查是否到达目标点
bool reached_target(double target_x, double target_y, double target_z, double tolerance = 0.3) {
if (!pose_received) return false;
return distance_to_waypoint(current_pose.pose.position, target_x, target_y, target_z) < tolerance;
}
// 创建偏航角四元数
geometry_msgs::Quaternion create_quaternion_from_yaw(double yaw_rad) {
tf2::Quaternion q;
q.setRPY(0, 0, yaw_rad);
return tf2::toMsg(q);
}
// 获取当前偏航角
double get_current_yaw() {
if (!pose_received) return 0.0;
tf2::Quaternion q(
current_pose.pose.orientation.x,
current_pose.pose.orientation.y,
current_pose.pose.orientation.z,
current_pose.pose.orientation.w);
tf2::Matrix3x3 m(q);
double roll, pitch, yaw;
m.getRPY(roll, pitch, yaw);
return yaw;
}
int main(int argc, char **argv) {
ros::init(argc, argv, "uav_fsm_node");
ros::NodeHandle nh;
// 参数配置 - 仿真环境优化值
double flight_height = 2.0; // 飞行高度(米) - 降低高度
double u_width = 1.5; // U型宽度(米) - 缩小路径
double u_depth = 1.5; // U型深度(米) - 缩小路径
double takeoff_climb_rate = 0.8; // 起飞爬升速率(米/秒) - 降低爬升率
double max_vel = 1.0; // 最大速度(m/s) - 降低速度
double arming_tolerance = 1.0; // 解锁位置容差 - 增大容差
double state_timeout = 120.0; // 状态超时时间(秒) - 延长时间
nh.param("flight_height", flight_height, flight_height);
nh.param("u_width", u_width, u_width);
nh.param("u_depth", u_depth, u_depth);
nh.param("takeoff_climb_rate", takeoff_climb_rate, takeoff_climb_rate);
nh.param("max_vel", max_vel, max_vel);
nh.param("arming_tolerance", arming_tolerance, arming_tolerance);
nh.param("state_timeout", state_timeout, state_timeout);
ROS_INFO("UAV Simulation Flight Control v6.1");
ROS_INFO(" - Takeoff Height: %.1f m", flight_height);
ROS_INFO(" - Takeoff Climb Rate: %.1f m/s", takeoff_climb_rate);
ROS_INFO(" - U-Width: %.1f m", u_width);
ROS_INFO(" - U-Depth: %.1f m", u_depth);
ROS_INFO(" - Max Velocity: %.1f m/s", max_vel);
ROS_INFO(" - State Timeout: %.1f sec", state_timeout);
// 订阅者
ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>
("mavros/state", 10, state_cb);
ros::Subscriber pose_sub = nh.subscribe<geometry_msgs::PoseStamped>
("mavros/local_position/pose", 10, pose_cb);
ros::Subscriber vel_sub = nh.subscribe<geometry_msgs::TwistStamped>
("mavros/local_position/velocity_local", 10, vel_cb);
ros::Subscriber timesync_sub = nh.subscribe<mavros_msgs::TimesyncStatus>
("mavros/timesync_status", 10, timesync_cb);
// 发布者
ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>
("mavros/setpoint_position/local", 10);
ros::Publisher setpoint_raw_pub = nh.advertise<mavros_msgs::PositionTarget>
("mavros/setpoint_raw/local", 10);
// 服务客户端
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::ServiceClient land_client = nh.serviceClient<mavros_msgs::CommandTOL>
("mavros/cmd/land");
// 设置发布频率
double control_rate = 20.0;
ros::Rate rate(control_rate);
double dt_val = 1.0 / control_rate;
// 等待飞控连接
ROS_INFO("Waiting for FCU connection...");
int connection_attempts = 0;
while (ros::ok() && !current_state.connected) {
ros::spinOnce();
rate.sleep();
if (++connection_attempts % 50 == 0) {
ROS_WARN("Still waiting for FCU connection...");
}
}
ROS_INFO("FCU connected");
// 定义航点类型
enum MovementType {
FORWARD, // 机头朝向运动方向
SIDEWAYS // 侧向移动
};
struct Waypoint {
double x, y, z;
MovementType move_type; // 移动类型
double target_yaw; // 目标偏航角(用于侧向移动)
};
// U形路径航点序列(起飞后执行的部分)
std::vector<Waypoint> u_path = {
// 起点(起飞后的悬停点)
{0.0, 0.0, flight_height, FORWARD, 0.0},
// 向前飞行
{u_depth, 0.0, flight_height, FORWARD, 0.0},
// 向右移动
{u_depth, u_width, flight_height, FORWARD, M_PI/2},
// U型终点
{0.0, u_width, flight_height, FORWARD, M_PI},
// 降落准备点
{0.0, u_width, flight_height, FORWARD, M_PI}
};
// 创建目标位置消息(用于位置控制)
geometry_msgs::PoseStamped target_pose;
target_pose.header.frame_id = "map";
// 创建速度控制消息 - 使用ENU坐标系(仿真环境标准)
mavros_msgs::PositionTarget vel_cmd;
// 关键修复:使用数值代替常量
// FRAME_LOCAL_ENU = 4, FRAME_LOCAL_NED = 1
vel_cmd.coordinate_frame = 4; // MAV_FRAME_LOCAL_ENU (z-up)
ROS_INFO("Using ENU coordinate system (value 4) for simulation");
vel_cmd.type_mask =
0b0000011111000111 | // 忽略加速度和力
mavros_msgs::PositionTarget::IGNORE_YAW_RATE;
// 预先发送设定点 - 关键修复:确保在OFFBOARD模式下持续发送设定点
ROS_INFO("Sending initial setpoints...");
ros::Time last_setpoint_time = ros::Time::now();
for (int i = 0; ros::ok() && i < 100; i++) {
// 初始设定点为地面点(0,0,0)
target_pose.pose.position.x = 0.0;
target_pose.pose.position.y = 0.0;
target_pose.pose.position.z = 0.0;
target_pose.pose.orientation = create_quaternion_from_yaw(0.0);
target_pose.header.stamp = ros::Time::now();
local_pos_pub.publish(target_pose);
// 在初始设定点发送期间尝试进入OFFBOARD模式
if (i > 20) {
mavros_msgs::SetMode offb_set_mode;
offb_set_mode.request.custom_mode = "OFFBOARD";
if (set_mode_client.call(offb_set_mode) && offb_set_mode.response.mode_sent) {
ROS_INFO("OFFBOARD mode enabled during initialization");
}
}
ros::spinOnce();
rate.sleep();
}
// 准备服务请求
mavros_msgs::SetMode offb_set_mode;
offb_set_mode.request.custom_mode = "OFFBOARD";
mavros_msgs::CommandBool arm_cmd;
arm_cmd.request.value = true;
// 飞行状态
enum FlightState { PREFLIGHT, OFFBOARD, ARMING, TAKEOFF, FLYING, LANDING, COMPLETED };
FlightState flight_state = PREFLIGHT;
size_t current_waypoint = 0;
ros::Time last_request = ros::Time::now();
ros::Time state_start_time = ros::Time::now();
bool landing_initiated = false;
double target_altitude = flight_height;
double current_yaw = 0.0;
double target_yaw = 0.0;
bool position_landing = false;
int arming_attempts = 0;
int convergence_count = 0;
geometry_msgs::Point current_position;
double takeoff_start_z = 0.0;
double pos_error = 0.0;
// 时间同步计数器
int timesync_warning_count = 0;
const int MAX_TIMESYNC_WARNINGS = 10;
ROS_INFO("Starting UAV simulation mission...");
while (ros::ok()) {
// 获取当前状态
ros::spinOnce();
// 检查状态超时
if ((ros::Time::now() - state_start_time).toSec() > state_timeout) {
ROS_ERROR("State timeout in state %d! Resetting to PREFLIGHT.", flight_state);
flight_state = PREFLIGHT;
state_start_time = ros::Time::now();
}
// 记录当前位置
if (pose_received) {
current_position = current_pose.pose.position;
}
// 状态机处理
switch (flight_state) {
case PREFLIGHT:
ROS_INFO_THROTTLE(1.0, "PREFLIGHT: Waiting for OFFBOARD mode");
ROS_INFO_THROTTLE(1.0, "Current mode: %s, Armed: %d",
current_state.mode.c_str(), current_state.armed);
if (ros::Time::now() - last_request > ros::Duration(5.0)) {
if (set_mode_client.call(offb_set_mode)) {
if (offb_set_mode.response.mode_sent) {
ROS_INFO("OFFBOARD enabled");
flight_state = OFFBOARD;
convergence_count = 0;
state_start_time = ros::Time::now();
} else {
ROS_WARN("OFFBOARD enable failed");
}
} else {
ROS_WARN("OFFBOARD service call failed");
}
last_request = ros::Time::now();
}
break;
case OFFBOARD:
ROS_INFO_THROTTLE(1.0, "OFFBOARD: Waiting for position convergence");
// 保持在地面点(0,0,0)
target_pose.pose.position.x = 0.0;
target_pose.pose.position.y = 0.0;
target_pose.pose.position.z = 0.0;
target_pose.pose.orientation = create_quaternion_from_yaw(0.0);
target_pose.header.stamp = ros::Time::now();
local_pos_pub.publish(target_pose);
// 计算位置误差
if (pose_received) {
pos_error = distance_to_waypoint(
current_position,
0.0,
0.0,
0.0
);
// 位置收敛检查
if (pos_error < 0.5) {
convergence_count++;
} else {
convergence_count = std::max(0, convergence_count - 1);
}
}
if (current_state.mode != "OFFBOARD") {
ROS_WARN("OFFBOARD mode lost! Current mode: %s", current_state.mode.c_str());
flight_state = PREFLIGHT;
state_start_time = ros::Time::now();
} else if (ros::Time::now() - last_request > ros::Duration(5.0)) {
// 位置稳定检查
if (convergence_count > 10 && pos_error < arming_tolerance) {
if (arming_client.call(arm_cmd)) {
if (arm_cmd.response.success) {
ROS_INFO("Armed successfully");
flight_state = TAKEOFF;
if (pose_received) {
takeoff_start_z = current_pose.pose.position.z;
}
arming_attempts = 0;
state_start_time = ros::Time::now();
} else {
arming_attempts++;
ROS_WARN("Arming failed (attempt %d)!", arming_attempts);
if (arming_attempts > 3) {
ROS_ERROR("Too many arming failures. Aborting mission.");
ros::shutdown();
}
}
} else {
ROS_WARN("Arming service call failed");
}
} else {
ROS_WARN_THROTTLE(1.0, "Position not stable for arming: error=%.2fm, convergence=%d/%d",
pos_error, convergence_count, 10);
}
last_request = ros::Time::now();
}
break;
case TAKEOFF:
if (pose_received) {
double current_height = current_pose.pose.position.z;
double height_error = flight_height - current_height;
ROS_INFO_THROTTLE(0.5, "TAKEOFF: Climbing to %.1fm (Current: %.2fm, Error: %.2fm)",
flight_height, current_height, height_error);
// 获取当前偏航角
current_yaw = get_current_yaw();
// 创建速度控制指令
vel_cmd.velocity.x = 0.0;
vel_cmd.velocity.y = 0.0;
vel_cmd.velocity.z = takeoff_climb_rate; // 直接使用固定爬升速率
vel_cmd.yaw = current_yaw;
// 发布速度控制命令
vel_cmd.header.stamp = ros::Time::now();
setpoint_raw_pub.publish(vel_cmd);
// 检查是否达到起飞高度
if (current_height > flight_height - 0.1) {
ROS_INFO("Reached takeoff altitude: %.2f m", current_height);
flight_state = FLYING;
current_waypoint = 0;
ROS_INFO("Starting U-shaped path flight");
state_start_time = ros::Time::now();
}
}
break;
case FLYING:
if (pose_received) {
ROS_INFO_THROTTLE(1.0, "FLYING: Waypoint %zu/%zu (Current Z: %.2f)",
current_waypoint+1, u_path.size(), current_pose.pose.position.z);
// 获取当前偏航角
current_yaw = get_current_yaw();
// 根据移动类型计算目标偏航角
if (u_path[current_waypoint].move_type == FORWARD) {
if (current_waypoint < u_path.size() - 1) {
double dx = u_path[current_waypoint+1].x - u_path[current_waypoint].x;
double dy = u_path[current_waypoint+1].y - u_path[current_waypoint].y;
target_yaw = std::atan2(dy, dx);
} else {
target_yaw = u_path[current_waypoint].target_yaw;
}
} else {
target_yaw = u_path[current_waypoint].target_yaw;
}
// 平滑偏航角过渡
double yaw_error = angles::shortest_angular_distance(current_yaw, target_yaw);
double smoothed_yaw = current_yaw + 0.1 * yaw_error;
// 创建位置控制指令
target_pose.pose.position.x = u_path[current_waypoint].x;
target_pose.pose.position.y = u_path[current_waypoint].y;
target_pose.pose.position.z = u_path[current_waypoint].z;
target_pose.pose.orientation = create_quaternion_from_yaw(smoothed_yaw);
// 发布位置指令
target_pose.header.stamp = ros::Time::now();
local_pos_pub.publish(target_pose);
// 检查是否到达当前航点
if (reached_target(
u_path[current_waypoint].x,
u_path[current_waypoint].y,
u_path[current_waypoint].z
)) {
ROS_INFO("Reached waypoint %zu: (%.1f, %.1f, %.1f)",
current_waypoint,
u_path[current_waypoint].x,
u_path[current_waypoint].y,
u_path[current_waypoint].z);
if (current_waypoint < u_path.size() - 1) {
current_waypoint++;
ROS_INFO("Moving to waypoint %zu: (%.1f, %.1f, %.1f)",
current_waypoint,
u_path[current_waypoint].x,
u_path[current_waypoint].y,
u_path[current_waypoint].z);
state_start_time = ros::Time::now();
} else {
ROS_INFO("Final waypoint reached, initiating landing");
flight_state = LANDING;
state_start_time = ros::Time::now();
}
}
}
if (!current_state.armed) {
ROS_ERROR("Unexpected disarm during flight!");
flight_state = OFFBOARD;
state_start_time = ros::Time::now();
}
break;
case LANDING:
ROS_INFO_THROTTLE(1.0, "LANDING: Descending... (Current Z: %.2f)",
pose_received ? current_pose.pose.position.z : 0.0);
if (!landing_initiated) {
mavros_msgs::CommandTOL land_cmd_srv;
land_cmd_srv.request.yaw = u_path.back().target_yaw;
if (land_client.call(land_cmd_srv)) {
if (land_cmd_srv.response.success) {
ROS_INFO("Landing command accepted");
landing_initiated = true;
} else {
ROS_WARN("Land service failed, using position landing");
landing_initiated = true;
position_landing = true;
if (pose_received) {
target_altitude = current_pose.pose.position.z;
} else {
target_altitude = flight_height;
}
}
} else {
ROS_WARN("Land service call failed, using position landing");
landing_initiated = true;
position_landing = true;
if (pose_received) {
target_altitude = current_pose.pose.position.z;
} else {
target_altitude = flight_height;
}
}
}
if (position_landing && pose_received) {
// 设置降落位置(XY保持不变)
target_pose.pose.position.x = u_path.back().x;
target_pose.pose.position.y = u_path.back().y;
// 平滑降低高度
if (target_altitude > 0.1) {
target_altitude -= 0.05 * dt_val;
} else {
target_altitude = 0.0;
}
target_pose.pose.position.z = target_altitude;
// 设置偏航角
target_pose.pose.orientation = create_quaternion_from_yaw(u_path.back().target_yaw);
// 发布位置指令
target_pose.header.stamp = ros::Time::now();
local_pos_pub.publish(target_pose);
}
// 检查是否着陆
if (pose_received && current_pose.pose.position.z < 0.2) {
ROS_INFO("Landed successfully at (%.2f, %.2f, %.2f)",
current_pose.pose.position.x,
current_pose.pose.position.y,
current_pose.pose.position.z);
flight_state = COMPLETED;
}
break;
case COMPLETED:
ROS_INFO("Mission completed. Shutting down...");
ros::shutdown();
break;
}
rate.sleep();
}
return 0;
}
代码运行后无人机一直在地面附件徘徊,无法上升到设定的1m高度,帮我修改代码
最新发布