MAVROS多机仿真

1

添加多机仿真的脚本的环境变量 我这里的Firmware在home目录下，添加如下到.bashrc

source ~/Firmware/Tools/setup_gazebo.bash ~/Firmware/ ~/Firmware/build/px4_sitl_default export ROS_PACKAGE_PATH=$RO{S}_{P}ACKAG{E}_{P}ATH:/FirmwareexportRO{S}_{P}ACKAG{E}_{P}ATH=$ROS_PACKAGE_PATH:~/Firmware/Tools/sitl_gazebo

修改launch文件multi_uav_mavros_sitl.launch 格式如下

<?xml version="1.0"?>

编写多机控制节点，控制不同的无人机发布不同的话题就行了 /**

@file offb_node.cpp@brief Offboard control example node, written with MAVROS version 0.19.x, PX4 Pro FlightStack and tested in Gazebo SITL */

#include <ros/ros.h> #include <geometry_msgs/PoseStamped.h> #include <mavros_msgs/CommandBool.h> #include <mavros_msgs/SetMode.h> #include <mavros_msgs/State.h>

mavros_msgs::State current_state; void state_cb(const mavros_msgs::State::ConstPtr& msg){ current_state = *msg; }

int main(int argc, char **argv) { ros::init(argc, argv, “offb_node”); ros::NodeHandle nh;

ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State> ("mavros/state", 10, state_cb); ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped> ("mavros/setpoint_position/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::Publisher local_pos_pub1 = nh.advertise<geometry_msgs::PoseStamped> ("uav1/mavros/setpoint_position/local", 10); ros::ServiceClient arming_client1 = nh.serviceClient<mavros_msgs::CommandBool> ("uav1/mavros/cmd/arming"); ros::ServiceClient set_mode_client1 = nh.serviceClient<mavros_msgs::SetMode> ("uav1/mavros/set_mode"); ros::Publisher local_pos_pub2 = nh.advertise<geometry_msgs::PoseStamped> ("uav2/mavros/setpoint_position/local", 10); ros::ServiceClient arming_client2 = nh.serviceClient<mavros_msgs::CommandBool> ("uav2/mavros/cmd/arming"); ros::ServiceClient set_mode_client2 = nh.serviceClient<mavros_msgs::SetMode> ("uav2/mavros/set_mode"); ros::Publisher local_pos_pub3 = nh.advertise<geometry_msgs::PoseStamped> ("uav3/mavros/setpoint_position/local", 10); ros::ServiceClient arming_client3 = nh.serviceClient<mavros_msgs::CommandBool> ("uav3/mavros/cmd/arming"); ros::ServiceClient set_mode_client3 = nh.serviceClient<mavros_msgs::SetMode> ("uav3/mavros/set_mode"); ros::Publisher local_pos_pub4 = nh.advertise<geometry_msgs::PoseStamped> ("uav4/mavros/setpoint_position/local", 10); ros::ServiceClient arming_client4 = nh.serviceClient<mavros_msgs::CommandBool> ("uav4/mavros/cmd/arming"); ros::ServiceClient set_mode_client4= nh.serviceClient<mavros_msgs::SetMode> ("uav4/mavros/set_mode"); ros::Publisher local_pos_pub5 = nh.advertise<geometry_msgs::PoseStamped> ("uav5/mavros/setpoint_position/local", 10); ros::ServiceClient arming_client5 = nh.serviceClient<mavros_msgs::CommandBool> ("uav5/mavros/cmd/arming"); ros::ServiceClient set_mode_client5= nh.serviceClient<mavros_msgs::SetMode> ("uav5/mavros/set_mode"); ros::Publisher local_pos_pub6 = nh.advertise<geometry_msgs::PoseStamped> ("uav6/mavros/setpoint_position/local", 10); ros::ServiceClient arming_client6 = nh.serviceClient<mavros_msgs::CommandBool> ("uav6/mavros/cmd/arming"); ros::ServiceClient set_mode_client6= nh.serviceClient<mavros_msgs::SetMode> ("uav6/mavros/set_mode"); ros::Publisher local_pos_pub7 = nh.advertise<geometry_msgs::PoseStamped> ("uav7/mavros/setpoint_position/local", 10); ros::ServiceClient arming_client7 = nh.serviceClient<mavros_msgs::CommandBool> ("uav7/mavros/cmd/arming"); ros::ServiceClient set_mode_client7= nh.serviceClient<mavros_msgs::SetMode> ("uav7/mavros/set_mode"); ros::Publisher local_pos_pub8 = nh.advertise<geometry_msgs::PoseStamped> ("uav8/mavros/setpoint_position/local", 10); ros::ServiceClient arming_client8 = nh.serviceClient<mavros_msgs::CommandBool> ("uav8/mavros/cmd/arming"); ros::ServiceClient set_mode_client8= nh.serviceClient<mavros_msgs::SetMode> ("uav8/mavros/set_mode"); ros::Publisher local_pos_pub9 = nh.advertise<geometry_msgs::PoseStamped> ("uav9/mavros/setpoint_position/local", 10); ros::ServiceClient arming_client9 = nh.serviceClient<mavros_msgs::CommandBool> ("uav9/mavros/cmd/arming"); ros::ServiceClient set_mode_client9= nh.serviceClient<mavros_msgs::SetMode> ("uav9/mavros/set_mode"); //the setpoint publishing rate MUST be faster than 2Hz ros::Rate rate(20.0); geometry_msgs::PoseStamped pose,pose1,pose2,pose3,pose4,pose5,pose6,pose7,pose8,pose9; pose.pose.position.x = 0; pose.pose.position.y = 0; pose.pose.position.z = 6; pose1.pose.position.x = 0; pose1.pose.position.y = 0; pose1.pose.position.z = 6; pose2.pose.position.x = 0; pose2.pose.position.y = 0; pose2.pose.position.z = 6; pose3.pose.position.x = 0; pose3.pose.position.y = 0; pose3.pose.position.z = 6; pose4.pose.position.x =0; pose4.pose.position.y =0; pose4.pose.position.z =6; pose5.pose.position.x =0; pose5.pose.position.y =0; pose5.pose.position.z =6; pose6.pose.position.x =0; pose6.pose.position.y =0; pose6.pose.position.z =6; pose7.pose.position.x =0; pose7.pose.position.y =0; pose7.pose.position.z =6; pose8.pose.position.x =0; pose8.pose.position.y =0; pose8.pose.position.z =6; pose9.pose.position.x =0; pose9.pose.position.y =0; pose9.pose.position.z =6; mavros_msgs::SetMode offb_set_mode; offb_set_mode.request.custom_mode = "TAKEOFF"; mavros_msgs::CommandBool arm_cmd; arm_cmd.request.value = true; ros::Time last_request = ros::Time::now();

int flag=0; while(ros::ok()){ if(flag>30) { pose.pose.position.x = 0; pose.pose.position.y = 10; pose.pose.position.z = 6;

pose1.pose.position.x = 0; pose1.pose.position.y = 10; pose1.pose.position.z = 6; pose2.pose.position.x = 0; pose2.pose.position.y = 10; pose2.pose.position.z = 6; pose3.pose.position.x = 0; pose3.pose.position.y = 10; pose3.pose.position.z = 6; pose4.pose.position.x =0; pose4.pose.position.y =10; pose4.pose.position.z =6; pose5.pose.position.x =0; pose5.pose.position.y =10; pose5.pose.position.z =6; pose6.pose.position.x =0; pose6.pose.position.y =10; pose6.pose.position.z =6; pose7.pose.position.x =0; pose7.pose.position.y =10; pose7.pose.position.z =6; pose8.pose.position.x =0; pose8.pose.position.y =10; pose8.pose.position.z =6; pose9.pose.position.x =0; pose9.pose.position.y =10; pose9.pose.position.z =6;

} set_mode_client.call(offb_set_mode); arming_client.call(arm_cmd); local_pos_pub.publish(pose);

set_mode_client1.call(offb_set_mode); arming_client1.call(arm_cmd); local_pos_pub1.publish(pose1);

set_mode_client2.call(offb_set_mode); arming_client2.call(arm_cmd); local_pos_pub2.publish(pose2);

set_mode_client3.call(offb_set_mode); arming_client3.call(arm_cmd); local_pos_pub3.publish(pose3);

set_mode_client4.call(offb_set_mode); arming_client4.call(arm_cmd); local_pos_pub4.publish(pose4);

set_mode_client5.call(offb_set_mode); arming_client5.call(arm_cmd); local_pos_pub5.publish(pose5);

set_mode_client6.call(offb_set_mode); arming_client6.call(arm_cmd); local_pos_pub6.publish(pose6);

set_mode_client7.call(offb_set_mode); arming_client7.call(arm_cmd); local_pos_pub7.publish(pose7);

set_mode_client8.call(offb_set_mode); arming_client8.call(arm_cmd); local_pos_pub8.publish(pose8);

set_mode_client9.call(offb_set_mode); arming_client9.call(arm_cmd); local_pos_pub9.publish(pose9);

flag++; ros::spinOnce(); rate.sleep(); }

return 0;

}

然后先执行launch文件。再执行控制节点，就可以实现多机控制