px4下基于ros和gazebo的多无人机编队仿真,offboard模式
单机的offboard仿真见https://blog.csdn.net/weixin_43409270/article/details/114585397
多机仿真
1、修改launch文件
在/PX4-Autopilot/launch目录下,找到multi_uav_mavros_sitl.launch文件,添加无人机和配置通信端口
可以直接复制文件中已有的配置文件,比如下面照片中是multi_uav_mavros_sitl.launch文件中已有的UAV0无人机的配置代码,我们可以直接复制,然后修改fcu_url, mavlink_udp_port和mavlink_tcp_port(在uav0的基础上加ID号即可)
比如,我们要添加第四架无人机(uav3),则udp端口为14540+3=14543:
<!-- UAV3 -->
<group ns="uav3">
<!-- MAVROS and vehicle configs -->
<arg name="ID" value="3"/>
<arg name="fcu_url" default="udp://:14543@localhost:14553"/>
<!-- PX4 SITL and vehicle spawn -->
<include file="$(find px4)/launch/single_vehicle_spawn.launch">
<arg name="x" value="3"/>
<arg name="y" value="0"/>
<arg name="z" value="0"/>
<arg name="R" value="0"/>
<arg name="P" value="0"/>
<arg name="Y" value="0"/>
<arg name="vehicle" value="$(arg vehicle)"/>
<arg name="mavlink_udp_port" value="14563"/>
<arg name="mavlink_tcp_port" value="4563"/>
<arg name="ID" value="$(arg ID)"/>
<arg name="gst_udp_port" value="$(eval 5600 + arg('ID'))"/>
<arg name="video_uri" value="$(eval 5600 + arg('ID'))"/>
<arg name="mavlink_cam_udp_port" value="$(eval 14530 + arg('ID'))"/>
</include>
<!-- MAVROS -->
<include file="$(find mavros)/launch/px4.launch">
<arg name="fcu_url" value="$(arg fcu_url)"/>
<arg name="gcs_url" value=""/>
<arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
<arg name="tgt_component" value="1"/>
</include>
</group>
最多只能有10架无人机。
注意仿真的坐标系有点乱~~,gazebo中有一个坐标系,在上面配置文件中的 的坐标(x,y,z)是设置无人机在gazebo中初始生成的位置,然后控制代码offboard中发布的每架无人机的pose的坐标系的原点默认就是每架无人机的初始位置(x,y,z),所以多机控制时坐标要转化一下。
2、修改offboard代码
每架无人机都需要设置ros接受发送消息,通过mavros前添加前缀,例如/uav0/mavros/…具体如下:
//uav0
ros::NodeHandle nh0;
ros::Subscriber state_sub0 = nh0.subscribe<mavros_msgs::State>
("uav0/mavros/state", 10, state_cb);
ros::Publisher local_pos_pub0 = nh0.advertise<geometry_msgs::PoseStamped>
("/uav0/mavros/setpoint_position/local", 10);
ros::ServiceClient arming_client0 = nh0.serviceClient<mavros_msgs::CommandBool>
("/uav0/mavros/cmd/arming");
ros::ServiceClient set_mode_client0 = nh0.serviceClient<mavros_msgs::SetMode>
("/uav0/mavros/set_mode");
//uav1
ros::NodeHandle nh1;
ros::Subscriber state_sub1 = nh1.subscribe<mavros_msgs::State>
("uav1/mavros/state", 10, state_cb);
ros::Publisher local_pos_pub1 = nh1.advertise<geometry_msgs::PoseStamped>
("/uav1/mavros/setpoint_position/local", 10);
ros::ServiceClient arming_client1 = nh1.serviceClient<mavros_msgs::CommandBool>
("/uav1/mavros/cmd/arming");
ros::ServiceClient set_mode_client1 = nh1.serviceClient<mavros_msgs::SetMode>
("/uav1/mavros/set_mode");
同理下面Offboard enabled和Vehicle armed也都要分别设置:
set_mode_client0.call(offb_set_mode) &&
set_mode_client1.call(offb_set_mode) &&
set_mode_client2.call(offb_set_mode) &&
set_mode_client3.call(offb_set_mode)
arming_client0.call(arm_cmd) &&
arming_client1.call(arm_cmd) &&
arming_client2.call(arm_cmd) &&
arming_client3.call(arm_cmd)
3、启动仿真
首先启动gazebo模型:
cd PX4-Autopilot
git submodule update --init --recursive
DONT_RUN=1 make px4_sitl_default gazebo
source Tools/setup_gazebo.bash $(pwd) $(pwd)/build/px4_sitl_default
export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:$(pwd):$(pwd)/Tools/sitl_gazebo
roslaunch px4 multi_uav_mavros_sitl.launch
其中4,5行的source和export在每次开新终端launch前都需要输入
然后运行offboard节点:
rosrun offboard offboard_node
下面附上自己的4架无人机画圆的offboard控制代码:
/**
* @file offb_node.cpp
* @brief Offboard control example node, written with MAVROS version 0.19.x, PX4 Pro Flight
* Stack 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");
//uav0
ros::NodeHandle nh0;
ros::Subscriber state_sub0 = nh0.subscribe<mavros_msgs::State>
("uav0/mavros/state", 10, state_cb);
ros::Publisher local_pos_pub0 = nh0.advertise<geometry_msgs::PoseStamped>
("/uav0/mavros/setpoint_position/local", 10);
ros::ServiceClient arming_client0 = nh0.serviceClient<mavros_msgs::CommandBool>
("/uav0/mavros/cmd/arming");
ros::ServiceClient set_mode_client0 = nh0.serviceClient<mavros_msgs::SetMode>
("/uav0/mavros/set_mode");
//uav1
ros::NodeHandle nh1;
ros::Subscriber state_sub1 = nh1.subscribe<mavros_msgs::State>
("uav1/mavros/state", 10, state_cb);
ros::Publisher local_pos_pub1 = nh1.advertise<geometry_msgs::PoseStamped>
("/uav1/mavros/setpoint_position/local", 10);
ros::ServiceClient arming_client1 = nh1.serviceClient<mavros_msgs::CommandBool>
("/uav1/mavros/cmd/arming");
ros::ServiceClient set_mode_client1 = nh1.serviceClient<mavros_msgs::SetMode>
("/uav1/mavros/set_mode");
//uav2
ros::NodeHandle nh2;
ros::Subscriber state_sub2 = nh2.subscribe<mavros_msgs::State>
("uav2/mavros/state", 10, state_cb);
ros::Publisher local_pos_pub2 = nh2.advertise<geometry_msgs::PoseStamped>
("/uav2/mavros/setpoint_position/local", 10);
ros::ServiceClient arming_client2 = nh2.serviceClient<mavros_msgs::CommandBool>
("/uav2/mavros/cmd/arming");
ros::ServiceClient set_mode_client2 = nh2.serviceClient<mavros_msgs::SetMode>
("/uav2/mavros/set_mode");
//uav3
ros::NodeHandle nh3;
ros::Subscriber state_sub3 = nh3.subscribe<mavros_msgs::State>
("uav3/mavros/state", 10, state_cb);
ros::Publisher local_pos_pub3 = nh3.advertise<geometry_msgs::PoseStamped>
("/uav3/mavros/setpoint_position/local", 10);
ros::ServiceClient arming_client3 = nh3.serviceClient<mavros_msgs::CommandBool>
("/uav3/mavros/cmd/arming");
ros::ServiceClient set_mode_client3 = nh3.serviceClient<mavros_msgs::SetMode>
("/uav3/mavros/set_mode");
//the setpoint publishing rate MUST be faster than 2Hz
ros::Rate rate(20.0);
// wait for FCU connection
while(ros::ok() && !current_state.connected){
ros::spinOnce();
rate.sleep();
}
// x_num, y_num refer to the desired uav input position
//all the uavs have the same height : z
float x0 = 0.0, y0 = 0.0;
float x1 = 0.0, y1 = 0.0;
float x2 = 0.0, y2 = 0.0;
float x3 = 0.0, y3 = 0.0;
float z = 2.0;
float w = 0.0;
const float pi = 3.1415926;
geometry_msgs::PoseStamped pose0;
geometry_msgs::PoseStamped pose1;
geometry_msgs::PoseStamped pose2;
geometry_msgs::PoseStamped pose3;
pose0.pose.position.x = x0;
pose0.pose.position.y = y0;
pose0.pose.position.z = z;
pose1.pose.position.x = x1;
pose1.pose.position.y = y1;
pose1.pose.position.z = z;
pose2.pose.position.x = x2;
pose2.pose.position.y = y2;
pose2.pose.position.z = z;
pose3.pose.position.x = x3;
pose3.pose.position.y = y3;
pose3.pose.position.z = z;
//uavs have the initial position offset because of the .launch config file
float x0_offset = -3;
float x1_offset = -1;
float x2_offset = 1;
float x3_offset = 3;
//define uavs' coordinates in the formation coordinate system
float x0_f = -0.5, y0_f = 0.5;
float x1_f = 0.5, y1_f = 0.5;
float x2_f = -0.5, y2_f = -0.5;
float x3_f = 0.5, y3_f = -0.5;
float xx, yy;
//send a few setpoints before starting
for(int i = 100; ros::ok() && i > 0; --i){
local_pos_pub0.publish(pose0);
local_pos_pub1.publish(pose1);
local_pos_pub2.publish(pose2);
local_pos_pub3.publish(pose3);
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;
ros::Time last_request = ros::Time::now();
while(ros::ok()){
if( current_state.mode != "OFFBOARD" &&
(ros::Time::now() - last_request > ros::Duration(5.0))){
if( set_mode_client0.call(offb_set_mode) &&
set_mode_client1.call(offb_set_mode) &&
set_mode_client2.call(offb_set_mode) &&
set_mode_client3.call(offb_set_mode) &&
offb_set_mode.response.mode_sent){
ROS_INFO("Offboard enabled : 4");
}
last_request = ros::Time::now();
} else {
if( !current_state.armed &&
(ros::Time::now() - last_request > ros::Duration(5.0))){
if( arming_client0.call(arm_cmd) &&
arming_client1.call(arm_cmd) &&
arming_client2.call(arm_cmd) &&
arming_client3.call(arm_cmd) &&
arm_cmd.response.success){
ROS_INFO("Vehicle armed : 4");
}
last_request = ros::Time::now();
}
}
local_pos_pub0.publish(pose0);
local_pos_pub1.publish(pose1);
local_pos_pub2.publish(pose2);
local_pos_pub3.publish(pose3);
w = w + 2*pi/(30/(1/20.0));
if(w > 2*pi){
w = w - 2*pi;
}
xx = 4.75*cos(w);
yy = 4.75*sin(w);
x0 = x0_f*cos(w) - y0_f*sin(w) + xx - x0_offset;
y0 = y0_f*cos(w) + x0_f*sin(w) + yy;
pose0.pose.position.x = x0;
pose0.pose.position.y = y0;
x1 = x1_f*cos(w) - y1_f*sin(w) + xx - x1_offset;
y1 = y1_f*cos(w) + x1_f*sin(w) + yy;
pose1.pose.position.x = x1;
pose1.pose.position.y = y1;
x2 = x2_f*cos(w) - y2_f*sin(w) + xx - x2_offset;
y2 = y2_f*cos(w) + x2_f*sin(w) + yy;
pose2.pose.position.x = x2;
pose2.pose.position.y = y2;
x3 = x3_f*cos(w) - y3_f*sin(w) + xx - x3_offset;
y3 = y3_f*cos(w) + x3_f*sin(w) + yy;
pose3.pose.position.x = x3;
pose3.pose.position.y = y3;
ros::spinOnce();
rate.sleep();
}
return 0;
}
本文内容由网友自发贡献,版权归原作者所有,本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容,请联系:hwhale#tublm.com(使用前将#替换为@)