0. 飞行效果
飞行效果
1.前期准备
实验环境:Ubuntu20.04,ROS noetic
1.1 完成PX4的安装和编译
1.2 完成mavros 的安装
1.3 完成QGC的安装
上述内容网上资料很多,这里就不赘述了。
2.代码思路
代码思路参考PX4官网的Offboard control 的思路,重点在于如何将键盘上的内容转化为无人机的飞行信息。
代码1:无人机节点控制
#include <ros/ros.h>
#include <iostream>
#include <eigen3/Eigen/Dense>
#include <geometry_msgs/PoseStamped.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>
#include <std_msgs/String.h>
using namespace Eigen;
using namespace std;
//定义话题名称
ros::Publisher local_pos_pub;
ros::Subscriber key_sub;
// 计算无人机与目标位置的距离
Eigen::Vector3d target_position;
bool position_updated = false;
void callback(const std_msgs::String::ConstPtr &msg)
{
ROS_INFO("Received keyboard input: %s", msg->data.c_str());
position_updated = true;
// 根据接收到的字符执行不同操作
//w,a,s,d控制前后左右,p,l控制高度
if (msg->data == "w")
{
target_position = target_position + Eigen::Vector3d(1.0, 0, 0);
}
else if (msg->data == "s")
{
target_position = target_position + Eigen::Vector3d(-1, 0, 0);
}
else if (msg->data == "a")
{
target_position = target_position + Eigen::Vector3d(0, 1, 0);
}
else if (msg->data == "d")
{
target_position = target_position + Eigen::Vector3d(0, -1, 0);
}
else if (msg->data == "p")
{
target_position = target_position + Eigen::Vector3d(0, 0, 1);
}
else if (msg->data == "l")
{
target_position = target_position + Eigen::Vector3d(0, 0, -1);
}
else
{
ROS_INFO("input must be 'w,a,s,d,p,l'");
position_updated = false;
}
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "offb_node");
ros::NodeHandle nh;
ros::Subscriber key_sub = nh.subscribe("keyboard_input", 10, callback);
ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>("mavros/setpoint_position/local", 10);
// the setpoint publishing rate MUST be faster than 2Hz
ros::Rate rate(20.0);
// wait for FCU connection
target_position << 0.0, 0.0, 1.0;
while (ros::ok())
{
geometry_msgs::PoseStamped pose;
pose.pose.position.x = target_position[0];
pose.pose.position.y = target_position[1];
pose.pose.position.z = target_position[2];
if ( position_updated )
{
ROS_INFO("Target position %f,%f,%f", target_position[0], target_position[1], target_position[2]);
position_updated = false;
}
local_pos_pub.publish(pose);
ros::spinOnce();
rate.sleep();
}
return 0;
}
本代码比较简单,两个话题分别对应键盘的输入和无人机位置的输出。具体内容可参考代码里的注释,这里不赘述。
值得注意的一点是,这里并没有像官网代码那里一样在代码里切换Offboard模式和Arm,因为在实际实验中,这种代码是非常危险的,容易导致无人机无法切换到其他模式,因此这里使用地面站QGC进行切换飞行模式
代码2: 键盘输入代码
#include <ros/ros.h>
#include <std_msgs/String.h>
#include <termios.h>
#include <unistd.h>
#include <fcntl.h>
class KeyboardInput
{
public:
KeyboardInput()
{
pub_ = nh_.advertise<std_msgs::String>("keyboard_input", 10);
tcgetattr(STDIN_FILENO, &initial_settings_);
termios new_settings = initial_settings_;
new_settings.c_lflag &= ~(ICANON | ECHO);
tcsetattr(STDIN_FILENO, TCSANOW, &new_settings);
fcntl(STDIN_FILENO, F_SETFL, O_NONBLOCK);
}
~KeyboardInput()
{
tcsetattr(STDIN_FILENO, TCSANOW, &initial_settings_);
}
void run()
{
ros::Rate rate(10);
while (ros::ok())
{
char c;
if (read(STDIN_FILENO, &c, 1) == 1)
{
std_msgs::String msg;
msg.data = std::string(1, c);
pub_.publish(msg);
}
ros::spinOnce();
rate.sleep();
}
}
private:
ros::NodeHandle nh_;
ros::Publisher pub_;
termios initial_settings_;
};
int main(int argc, char **argv)
{
ros::init(argc, argv, "keyboard_input");
KeyboardInput keyboard_input;
keyboard_input.run();
return 0;
}
这里直接让GPT写的代码,内容很好理解,只有一个发布话题将键盘的内容转为ROS信息发布出去即可。
3.实验流程
3.1 进行PX4编译
打开终端,根据自己路径和PX4版本执行:
make px4_sitl gazebo
操作结果为gazebo中出现无人机模型
3.2 打开Mavros进行通讯
另外打开终端,执行
roslaunch mavros px4.launch fcu_url:="udp://:14540@127.0.0.1:14557"
可以通过指令
rostopic list
验证通信是否成果,成果的话可以看到很多和无人机有关的话题。
3.3 打卡QGC,连接无人机
这里没啥说的,一般都是直接就连上了
不过大家可以在这个时候试着切换Offboard模式,这个时候是无法切换到offboard模式的,因为px4对切换至offboard模式做出了限制,即必须有控制执行发送一段时间之后才可以切换到offboard模式。
3.4运行offboard程序
打开终端,根据自己编译的位置执行:
rosrun offboard offboard_node
此时无人机应该没有任何反应,因为这个时候还未处于Offboard模式
3.5 通过QGC切换飞行模式并解锁
此时可以看到无人机首先飞到了(0,0,1)的位置
3.5执行键盘输入指令
根据自己编译的位置,执行
rosrun keyboard_control keyboard_input
之后在终端里输入wasdpl即可控制无人机
无人机按照指令飞到其位置
4.后续计划
这里采用的是直接计算无人机的位置点来完成其位置的改变,后续计划通过利用速度、加速度来控制无人机,来达到更加真实的效果。
——2024.08.01