GAZEBO仿真+offboard模式无人机控制

&黄昏的乐师

已于 2024-05-27 17:16:22 修改

阅读量639

点赞数 4

文章标签：无人机

于 2023-08-31 21:54:45 首次发布

本文链接：https://blog.csdn.net/2301_76165902/article/details/132594935

版权

offboard代码部分

官方提供源码

https://docs.px4.io/main/zh/ros/mavros_offboard_cpp.html

配置步骤

先建立工作空间,功能包,然后编写offboard_node文件

mkdir H_ws/src
cd H_ws/src
catkin_create_pkg offboard_nodes rospy roscpp std_msgs
cd offboard_nodes/src
gedit offboard_node.cpp

官方代码演示

#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初始化，最后一个参数为节点名称
ros::NodeHandle nh;

//订阅。<>里面为模板参数，传入的是订阅的消息体类型，（）里面传入三个参数，分别是该消息体的位置、缓存大小（通常为10）、回调函数
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);//控制话题,用于发布位置信息
 
//启动服务1，设置客户端（Client）名称为arming_client，客户端的类型为ros::ServiceClient，用于请求解锁无人机
ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>("mavros/cmd/arming");

//启动服务2，设置客户端（Client）名称为set_mode_client，客户端的类型为ros::ServiceClient，用于请求切换无人机模式
ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>("mavros/set_mode");
 
//发布频率
ros::Rate rate(20.0);
 
// 等待飞控连接mavros，current_state是我们订阅的mavros的状态，连接成功在跳出循环
while(ros::ok() && !current_state.connected){
    ros::spinOnce();
    rate.sleep();
}
//先实例化一个geometry_msgs::PoseStamped类型的对象，并对其赋值，最后将其发布出去
geometry_msgs::PoseStamped pose;
pose.pose.position.x = 0;
pose.pose.position.y = 0;
pose.pose.position.z = 2; //控制飞到z=2米处
 
//启动之前发送一些设定值
for(int i = 100; ros::ok() && i > 0; --i){
       local_pos_pub.publish(pose);
       ros::spinOnce();
       rate.sleep();
}
//建立一个类型为SetMode的服务端offb_set_mode，并将其中的模式mode设为"OFFBOARD"，作用便是用于后面的客户端与服务端之间的通信（服务）
mavros_msgs::SetMode offb_set_mode;
offb_set_mode.request.custom_mode = "OFFBOARD";
 
//建立一个类型为CommandBool的服务端arm_cmd，并将其中的是否解锁设为"true"，作用便是用于后面的客户端与服务端之间的通信（服务）
mavros_msgs::CommandBool arm_cmd;
arm_cmd.request.value = true;
 
//更新时间
ros::Time last_request = ros::Time::now();
 
while(ros::ok())//进入大循环
{
    //首先判断当前模式是否为offboard模式，如果不是，则客户端set_mode_client向服务端offb_set_mode发起请求call
    if( current_state.mode != "OFFBOARD" && (ros::Time::now() - last_request > ros::Duration(5.0)))
    {
       	if( set_mode_client.call(offb_set_mode) && offb_set_mode.response.mode_sent)
        {
            ROS_INFO("Offboard enabled");//打开模式后打印信息
        }
        last_request = ros::Time::now();
    }
    else //else指已经为offboard模式，然后进去判断是否解锁，如果没有解锁，则客户端arming_client向服务端arm_cmd发起请求call
    {
        if( !current_state.armed && (ros::Time::now() - last_request > ros::Duration(5.0)))
        {
            if( arming_client.call(arm_cmd) && arm_cmd.response.success)
            {
                ROS_INFO("UAV is arming");//解锁后打印信息
            }
            last_request = ros::Time::now();
        }
    }
 
    local_pos_pub.publish(pose); //发布位置信息，所以综上飞机只有先打开offboard模式然后解锁才能飞起来
 
    ros::spinOnce();
    rate.sleep();
}
 
return 0;

}

保存后关闭,打开CMakeLists.txt,添加

add_executable(offboard_node src/offboard_node.cpp)
target_link_libraries(offboard_node
  ${catkin_LIBRARIES}
)

我习惯在最末尾添加,也可以在build内直接复制添加

退回H_ws目录,开始编译

catkin_make

开启gazebo

另起终端输入

roslaunch px4 mavros_posix_sitl.launch #仿真通信一体化,可以直接建立通信

打开地面站QGC会显示已经连接上飞行器

在H_ws目录下输入

source devel/setup.bash #为该终端添加环境变量
rosrun offboard_nodes offboard_node #运行ros节点

等待无人机切换offboard模式,自动解锁

飞机起飞到2米高处

自己写轨迹

新创建一个offboard文件 offboard_node1.cpp

#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;
}
// 订阅的无人机当前位置数据
geometry_msgs::PoseStamped local_pos;
void local_pos_cb(const geometry_msgs::PoseStamped::ConstPtr &msg)
{
	local_pos = *msg;
}


int main(int argc, char **argv)
{
ros::init(argc, argv, "offb_node"); //ros初始化，最后一个参数为节点名称
ros::NodeHandle nh;

//订阅。<>里面为模板参数，传入的是订阅的消息体类型，（）里面传入三个参数，分别是该消息体的位置、缓存大小（通常为10）、回调函数
ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>("mavros/state", 10, state_cb);
ros::Subscriber local_pos_sub = nh.subscribe<geometry_msgs::PoseStamped>("mavros/local_position/pose", 10, local_pos_cb);//订阅位置信息
ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>("mavros/setpoint_position/local", 10);//控制话题,用于发布位置信息
 
//启动服务1，设置客户端（Client）名称为arming_client，客户端的类型为ros::ServiceClient，用于请求解锁无人机
ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>("mavros/cmd/arming");

//启动服务2，设置客户端（Client）名称为set_mode_client，客户端的类型为ros::ServiceClient，用于请求切换无人机模式
ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>("mavros/set_mode");
 
//发布频率
ros::Rate rate(20.0);
 
// 等待飞控连接mavros，current_state是我们订阅的mavros的状态，连接成功在跳出循环
while(ros::ok() && !current_state.connected){
    ros::spinOnce();
    rate.sleep();
}
//先实例化一个geometry_msgs::PoseStamped类型的对象，并对其赋值，最后将其发布出去
geometry_msgs::PoseStamped pose;
pose.pose.position.x = 0;
pose.pose.position.y = 0;
pose.pose.position.z = 0;
 
//启动之前发送一些设定值
for(int i = 100; ros::ok() && i > 0; --i){
       local_pos_pub.publish(pose);
       ros::spinOnce();
       rate.sleep();
}
//建立一个类型为SetMode的服务端offb_set_mode，并将其中的模式mode设为"OFFBOARD"，作用便是用于后面的客户端与服务端之间的通信（服务）
mavros_msgs::SetMode offb_set_mode;
offb_set_mode.request.custom_mode = "OFFBOARD";

//设定无人机保护模式 POSTION
mavros_msgs::SetMode offb_setPS_mode;
offb_setPS_mode.request.custom_mode = "POSCTL";

//建立一个类型为CommandBool的服务端arm_cmd，并将其中的是否解锁设为"true"，作用便是用于后面的客户端与服务端之间的通信（服务）
mavros_msgs::CommandBool arm_cmd;
arm_cmd.request.value = true;
 
//更新时间
ros::Time last_request = ros::Time::now();

int step = 0;
int sametimes = 0;

while(ros::ok())//进入大循环
{
  if (current_state.mode != "OFFBOARD" && (ros::Time::now() - last_request > ros::Duration(5.0)))
	{
		if (set_mode_client.call(offb_setPS_mode) && offb_setPS_mode.response.mode_sent)
		{
			ROS_INFO("POSTION PROTECTED");
		}
		last_request = ros::Time::now();
	}
	else
	{
        if( !current_state.armed && (ros::Time::now() - last_request > ros::Duration(5.0)))
        {
            if( arming_client.call(arm_cmd) && arm_cmd.response.success)
            {
                ROS_INFO("UAV is arming");//解锁后打印信息
            }
            last_request = ros::Time::now();
        }
	else
	{
		switch(step)
		{
			case 0:
				pose.pose.position.x = 0;
				pose.pose.position.y = 0;
				pose.pose.position.z = 2;
				if (local_pos.pose.position.z > 1.9 && local_pos.pose.position.z < 2.1)
                  		{
     		  	                if (sametimes > 20)
              				{
         		                   step = 1;
                        		}
                        		else
                           		sametimes++;
                    			}
                    			else sametimes = 0;
	 	                break;
			case 1:
				pose.pose.position.x = 5;
				pose.pose.position.y = 0;
				pose.pose.position.z = 2;
				if (local_pos.pose.position.x > 4.9 && local_pos.pose.position.x < 5.1)
                  		{
     		  	                if (sametimes > 20)
              				{
         		                   step = 2;
                        		}
                        		else
                           		sametimes++;
                    			}
                    			else sametimes = 0;
	 	                break;
			case 2:
				pose.pose.position.x = 5;
				pose.pose.position.y = 5;
				pose.pose.position.z = 2;
				if (local_pos.pose.position.y > 4.9 && local_pos.pose.position.y < 5.1)
                  		{
     		  	                if (sametimes > 20)
              				{
         		                   step = 3;
                        		}
                        		else
                           		sametimes++;
                    			}
                    			else sametimes = 0;
	 	                break;
			case 3:
				pose.pose.position.x = 0;
				pose.pose.position.y = 5;
				pose.pose.position.z = 2;
				if (local_pos.pose.position.x > -0.1 && local_pos.pose.position.x < 0.1)
                  		{
     		  	                if (sametimes > 20)
              				{
         		                   step = 4;
                        		}
                        		else
                           		sametimes++;
                    			}
                    			else sametimes = 0;
	 	                break;
			case 4:
				pose.pose.position.x = 0;
				pose.pose.position.y = 0;
				pose.pose.position.z = 2;
				if (local_pos.pose.position.x > -0.1 && local_pos.pose.position.x < 0.1 && local_pos.pose.position.y > -0.1 && local_pos.pose.position.y < 0.1)
                  		{
     		  	                if (sametimes > 20)
              				{
         		                   step = 5;
                        		}
                        		else
                           		sametimes++;
                    			}
                    			else sametimes = 0;
	 	                break;
			case 5:
				offb_set_mode.request.custom_mode = "AUTO.LAND";
				if (current_state.mode != "AUTO.LAND" && (ros::Time::now() - last_request > ros::Duration(5.0)))
				{
					if (set_mode_client.call(offb_set_mode) && offb_set_mode.response.mode_sent)
					{
						ROS_INFO("AUTO.LAND enabled");
					}
					last_request = ros::Time::now();
					}
				break;
				default:
					break;
		}
	}
    }
 
    local_pos_pub.publish(pose); //发布位置信息，所以综上飞机只有先打开offboard模式然后解锁才能飞起来
 
    ros::spinOnce();
    rate.sleep();
}
 
return 0;

}

再次编写CMakeList.txt文件

添加

add_executable(offboard_node1 src/offboard_node1.cpp)
target_link_libraries(offboard_node1
  ${catkin_LIBRARIES}
)

编译

之后运行offboard节点

飞机会飞一个正方形

(地面站轨迹有延迟)

添加部分

1.本次代码添加了位置信息订阅

mavros/local_position/lose话题

2.添加悬停模式保护

//设定无人机保护模式 POSTION
mavros_msgs::SetMode offb_setPS_mode;
offb_setPS_mode.request.custom_mode = "POSCTL";

if (set_mode_client.call(offb_setPS_mode) && offb_setPS_mode.response.mode_sent)
		{
			ROS_INFO("POSTION PROTECTED");
		}
		last_request = ros::Time::now();

飞机会自动开启悬停模式,需要我们自己手动开启offboard模式开启控制(在地面站切换即可)

该修改只适合在仿真中进行,不适合在现实中进行,因为如果无人机一旦着陆,会自动切换为悬停模式,飞机会再次起飞

如有需要,可以将

offb_setPS_mode.request.custom_mode = "POSCTL";

该行代码的"POSCTL"随意编写.使得终端报错

该报错对无人机没有影响,直接切换offboard模式即可起飞

3.降落模式

在仿真中,无人机降落后会自动上锁,所以悬停保护也不会有什么作用,但是在现实中降落后是需要手动上锁的,不然飞机直接切换悬停模式让电机有怠速,会再次起飞.

所以建议执行上一点的修改.

4.添加模式

注意

所有代码都是在仿真gazebo中进行,不宜直接应用到现实中,现实的情况和仿真差距还是很大的.

如需在现实中调试飞行器,为防止意外发生,请卸下螺旋桨,不然就打开控制器强制上锁,因为源代码会自动请求解锁飞行器.

参考资料:

https://zhuanlan.zhihu.com/p/440996013?utm_id=0

感谢!

&黄昏的乐师

关注

4
点赞
踩
22

收藏

觉得还不错? 一键收藏
0
评论
GAZEBO仿真+offboard模式无人机控制

官方提供源码。
复制链接

扫一扫