#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 <cmath>
// 无人机状态和位置变量
mavros_msgs::State current_state_typhoon0, current_state_typhoon1;
mavros_msgs::State current_state_iris0, current_state_iris1;
geometry_msgs::PoseStamped pos_typhoon0, pos_typhoon1;
geometry_msgs::PoseStamped pos_iris0, pos_iris1;
// 状态回调函数
void state_cb_typhoon0(const mavros_msgs::State::ConstPtr& msg) { current_state_typhoon0 = *msg; }
void state_cb_typhoon1(const mavros_msgs::State::ConstPtr& msg) { current_state_typhoon1 = *msg; }
void state_cb_iris0(const mavros_msgs::State::ConstPtr& msg) { current_state_iris0 = *msg; }
void state_cb_iris1(const mavros_msgs::State::ConstPtr& msg) { current_state_iris1 = *msg; }
// 位置回调函数
void pos_cb_typhoon0(const geometry_msgs::PoseStamped::ConstPtr& msg) { pos_typhoon0 = *msg; }
void pos_cb_typhoon1(const geometry_msgs::PoseStamped::ConstPtr& msg) { pos_typhoon1 = *msg; }
void pos_cb_iris0(const geometry_msgs::PoseStamped::ConstPtr& msg) { pos_iris0 = *msg; }
void pos_cb_iris1(const geometry_msgs::PoseStamped::ConstPtr& msg) { pos_iris1 = *msg; }
// 检查是否到达目标位置
bool checkPositionReached(const geometry_msgs::PoseStamped& current_pose, float target_x, float target_y, float target_z, float tolerance = 2.0) {
float dx = current_pose.pose.position.x - target_x;
float dy = current_pose.pose.position.y - target_y;
float dz = current_pose.pose.position.z - target_z;
float distance = sqrt(dx*dx + dy*dy + dz*dz);
return distance < tolerance;
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "multi_drone_offboard");
// typhoon_h480_0
ros::NodeHandle nh_typhoon0;
ros::Subscriber state_sub_typhoon0 = nh_typhoon0.subscribe<mavros_msgs::State>
("typhoon_h480_0/mavros/state", 10, state_cb_typhoon0);
ros::Subscriber local_pos_sub_typhoon0 = nh_typhoon0.subscribe<geometry_msgs::PoseStamped>
("/typhoon_h480_0/mavros/local_position/pose", 10, pos_cb_typhoon0);
ros::Publisher local_pos_pub_typhoon0 = nh_typhoon0.advertise<geometry_msgs::PoseStamped>
("/typhoon_h480_0/mavros/setpoint_position/local", 10);
ros::ServiceClient arming_client_typhoon0 = nh_typhoon0.serviceClient<mavros_msgs::CommandBool>
("/typhoon_h480_0/mavros/cmd/arming");
ros::ServiceClient set_mode_client_typhoon0 = nh_typhoon0.serviceClient<mavros_msgs::SetMode>
("/typhoon_h480_0/mavros/set_mode");
// typhoon_h480_1
ros::NodeHandle nh_typhoon1;
ros::Subscriber state_sub_typhoon1 = nh_typhoon1.subscribe<mavros_msgs::State>
("typhoon_h480_1/mavros/state", 10, state_cb_typhoon1);
ros::Subscriber local_pos_sub_typhoon1 = nh_typhoon1.subscribe<geometry_msgs::PoseStamped>
("/typhoon_h480_1/mavros/local_position/pose", 10, pos_cb_typhoon1);
ros::Publisher local_pos_pub_typhoon1 = nh_typhoon1.advertise<geometry_msgs::PoseStamped>
("/typhoon_h480_1/mavros/setpoint_position/local", 10);
ros::ServiceClient arming_client_typhoon1 = nh_typhoon1.serviceClient<mavros_msgs::CommandBool>
("/typhoon_h480_1/mavros/cmd/arming");
ros::ServiceClient set_mode_client_typhoon1 = nh_typhoon1.serviceClient<mavros_msgs::SetMode>
("/typhoon_h480_1/mavros/set_mode");
// iris_0
ros::NodeHandle nh_iris0;
ros::Subscriber state_sub_iris0 = nh_iris0.subscribe<mavros_msgs::State>
("iris_0/mavros/state", 10, state_cb_iris0);
ros::Subscriber local_pos_sub_iris0 = nh_iris0.subscribe<geometry_msgs::PoseStamped>
("/iris_0/mavros/local_position/pose", 10, pos_cb_iris0);
ros::Publisher local_pos_pub_iris0 = nh_iris0.advertise<geometry_msgs::PoseStamped>
("/iris_0/mavros/setpoint_position/local", 10);
ros::ServiceClient arming_client_iris0 = nh_iris0.serviceClient<mavros_msgs::CommandBool>
("/iris_0/mavros/cmd/arming");
ros::ServiceClient set_mode_client_iris0 = nh_iris0.serviceClient<mavros_msgs::SetMode>
("/iris_0/mavros/set_mode");
// iris_1
ros::NodeHandle nh_iris1;
ros::Subscriber state_sub_iris1 = nh_iris1.subscribe<mavros_msgs::State>
("iris_1/mavros/state", 10, state_cb_iris1);
ros::Subscriber local_pos_sub_iris1 = nh_iris1.subscribe<geometry_msgs::PoseStamped>
("/iris_1/mavros/local_position/pose", 10, pos_cb_iris1);
ros::Publisher local_pos_pub_iris1 = nh_iris1.advertise<geometry_msgs::PoseStamped>
("/iris_1/mavros/setpoint_position/local", 10);
ros::ServiceClient arming_client_iris1 = nh_iris1.serviceClient<mavros_msgs::CommandBool>
("/iris_1/mavros/cmd/arming");
ros::ServiceClient set_mode_client_iris1 = nh_iris1.serviceClient<mavros_msgs::SetMode>
("/iris_1/mavros/set_mode");
ROS_INFO("Establishing Connection with all drones........");
ros::Rate rate(20.0);
// 等待所有无人机连接
while(ros::ok() &&
(!current_state_typhoon0.connected || !current_state_typhoon1.connected ||
!current_state_iris0.connected || !current_state_iris1.connected)) {
ros::spinOnce();
rate.sleep();
}
ROS_INFO("All drones connected!");
// 目标点设置
float final_target_x = 120.0;
float final_target_y = -150.0;
float final_target_z = 10.0;
// iris0 提前降落点
float early_landing_iris0_x = 80.0;
float early_landing_iris0_y = -100.0;
float final_target_x_iris = 100.0;
float final_target_y_iris = -170.0;
// 创建位置消息
geometry_msgs::PoseStamped pose_typhoon0, pose_typhoon1;
geometry_msgs::PoseStamped pose_iris0, pose_iris1;
// 初始化位置设定点(起飞高度)
float takeoff_height = 10.0;
pose_typhoon0.pose.position.x = pos_typhoon0.pose.position.x;
pose_typhoon0.pose.position.y = pos_typhoon0.pose.position.y;
pose_typhoon0.pose.position.z = takeoff_height;
pose_typhoon1.pose.position.x = pos_typhoon1.pose.position.x;
pose_typhoon1.pose.position.y = pos_typhoon1.pose.position.y;
pose_typhoon1.pose.position.z = takeoff_height;
pose_iris0.pose.position.x = pos_iris0.pose.position.x;
pose_iris0.pose.position.y = pos_iris0.pose.position.y;
pose_iris0.pose.position.z = takeoff_height;
pose_iris1.pose.position.x = pos_iris1.pose.position.x;
pose_iris1.pose.position.y = pos_iris1.pose.position.y;
pose_iris1.pose.position.z = takeoff_height;
ROS_INFO("Sending initial setpoints...");
// 发送初始设定点
for(int i = 100; ros::ok() && i > 0; --i) {
local_pos_pub_typhoon0.publish(pose_typhoon0);
local_pos_pub_typhoon1.publish(pose_typhoon1);
local_pos_pub_iris0.publish(pose_iris0);
local_pos_pub_iris1.publish(pose_iris1);
ros::spinOnce();
rate.sleep();
}
// 设置offboard模式和arming服务
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();
// 任务状态标志
bool typhoon0_takeoff_complete = false;
bool typhoon1_takeoff_complete = false;
bool iris0_takeoff_complete = false;
bool iris1_takeoff_complete = false;
bool typhoon0_mission_complete = false;
bool typhoon1_mission_complete = false;
bool iris0_mission_complete = false;
bool iris1_mission_complete = false;
ROS_INFO("Starting mission...");
while(ros::ok()) {
// 设置offboard模式
if((current_state_typhoon0.mode != "OFFBOARD" || current_state_typhoon1.mode != "OFFBOARD" ||
current_state_iris0.mode != "OFFBOARD" || current_state_iris1.mode != "OFFBOARD") &&
(ros::Time::now() - last_request > ros::Duration(5.0))) {
if(set_mode_client_typhoon0.call(offb_set_mode) && set_mode_client_typhoon1.call(offb_set_mode) &&
set_mode_client_iris0.call(offb_set_mode) && set_mode_client_iris1.call(offb_set_mode) &&
offb_set_mode.response.mode_sent) {
ROS_INFO("Offboard enabled for all drones");
}
last_request = ros::Time::now();
} else {
// 解锁无人机
if((!current_state_typhoon0.armed || !current_state_typhoon1.armed ||
!current_state_iris0.armed || !current_state_iris1.armed) &&
(ros::Time::now() - last_request > ros::Duration(5.0))) {
if(arming_client_typhoon0.call(arm_cmd) && arming_client_typhoon1.call(arm_cmd) &&
arming_client_iris0.call(arm_cmd) && arming_client_iris1.call(arm_cmd) &&
arm_cmd.response.success) {
ROS_INFO("All vehicles armed");
}
last_request = ros::Time::now();
}
}
// 第一阶段:起飞到安全高度
if(!typhoon0_takeoff_complete) {
if(checkPositionReached(pos_typhoon0, pose_typhoon0.pose.position.x, pose_typhoon0.pose.position.y, takeoff_height, 1.0)) {
typhoon0_takeoff_complete = true;
ROS_INFO("typhoon_h480_0 takeoff complete");
}
}
if(!typhoon1_takeoff_complete) {
if(checkPositionReached(pos_typhoon1, pose_typhoon1.pose.position.x, pose_typhoon1.pose.position.y, takeoff_height, 1.0)) {
typhoon1_takeoff_complete = true;
ROS_INFO("typhoon_h480_1 takeoff complete");
}
}
if(!iris0_takeoff_complete) {
if(checkPositionReached(pos_iris0, pose_iris0.pose.position.x, pose_iris0.pose.position.y, takeoff_height, 1.0)) {
iris0_takeoff_complete = true;
ROS_INFO("iris_0 takeoff complete");
}
}
if(!iris1_takeoff_complete) {
if(checkPositionReached(pos_iris1, pose_iris1.pose.position.x, pose_iris1.pose.position.y, takeoff_height, 1.0)) {
iris1_takeoff_complete = true;
ROS_INFO("iris_1 takeoff complete");
}
}
// 第二阶段:飞往目标点
if(typhoon0_takeoff_complete && !typhoon0_mission_complete) {
pose_typhoon0.pose.position.x = final_target_x;
pose_typhoon0.pose.position.y = final_target_y;
pose_typhoon0.pose.position.z = final_target_z;
if(checkPositionReached(pos_typhoon0, final_target_x, final_target_y, final_target_z, 2.0)) {
ROS_INFO("typhoon_h480_0 reached final target, starting landing");
typhoon0_mission_complete = true;
}
}
if(typhoon1_takeoff_complete && !typhoon1_mission_complete) {
pose_typhoon1.pose.position.x = final_target_x;
pose_typhoon1.pose.position.y = final_target_y;
pose_typhoon1.pose.position.z = final_target_z;
if(checkPositionReached(pos_typhoon1, final_target_x, final_target_y, final_target_z, 2.0)) {
ROS_INFO("typhoon_h480_1 reached final target, starting landing");
typhoon1_mission_complete = true;
}
}
// iris_0 提前在x=50,y=-85处降落
if(iris0_takeoff_complete && !iris0_mission_complete) {
pose_iris0.pose.position.x = early_landing_iris0_x;
pose_iris0.pose.position.y = early_landing_iris0_y;
pose_iris0.pose.position.z = final_target_z;
if(checkPositionReached(pos_iris0, early_landing_iris0_x, early_landing_iris0_y, final_target_z, 2.0)) {
ROS_INFO("iris_0 reached early landing point, starting descent");
// 开始下降
pose_iris0.pose.position.z = 0.5; // 下降到地面
if(checkPositionReached(pos_iris0, early_landing_iris0_x, early_landing_iris0_y, 0.5, 0.5)) {
iris0_mission_complete = true;
ROS_INFO("iris_0 landed successfully");
}
}
}
// iris_1 飞往最终目标
if(iris1_takeoff_complete && !iris1_mission_complete) {
pose_iris1.pose.position.x = final_target_x_iris;
pose_iris1.pose.position.y = final_target_y_iris;
pose_iris1.pose.position.z = final_target_z;
if(checkPositionReached(pos_iris1, final_target_x_iris, final_target_y_iris, final_target_z, 2.0)) {
ROS_INFO("iris_1 reached final target, starting landing");
iris1_mission_complete = true;
}
}
// 第三阶段:降落
if(typhoon0_mission_complete) {
pose_typhoon0.pose.position.z = 0.5; // 下降到地面
}
if(typhoon1_mission_complete) {
pose_typhoon1.pose.position.z = 0.5; // 下降到地面
}
if(iris1_mission_complete) {
pose_iris1.pose.position.z = 0.5; // 下降到地面
}
// 发布所有位置设定点
local_pos_pub_typhoon0.publish(pose_typhoon0);
local_pos_pub_typhoon1.publish(pose_typhoon1);
local_pos_pub_iris0.publish(pose_iris0);
local_pos_pub_iris1.publish(pose_iris1);
ros::spinOnce();
rate.sleep();
}
return 0;
}帮我检查是否有问题,这里typhoon无人机比较特殊,希望你仔细检查,但不要吹毛求疵。因为我在运行cpp文件时遇到这种问题“dell@node01:~/catkin_ws$ rosrun offboard offboard_sin_node
[ INFO] [1762609701.415953640]: Establishing Connection with all drones........
[ INFO] [1762609701.697483242, 226.728000000]: All drones connected!
[ INFO] [1762609701.697523195, 226.728000000]: Sending initial setpoints...
[ INFO] [1762609706.699555750, 231.728000000]: Starting mission...
[ INFO] [1762609709.563074280, 234.578000000]: typhoon_h480_1 takeoff complete
[ INFO] [1762609709.963499081, 234.978000000]: typhoon_h480_0 takeoff complete
[ INFO] [1762609711.769315981, 236.782000000]: Offboard enabled for all drones
[ INFO] [1762609716.822062578, 241.832000000]: Offboard enabled for all drones
[ INFO] [1762609721.872602370, 246.880000000]: Offboard enabled for all drones
[ INFO] [1762609726.928338924, 251.934000000]: Offboard enabled for all drones”一直是Offboard enabled for all drones