ROS編寫無人機控制程序包

這篇教程來詳細介紹一下如何編寫出一個控制無人機的ROS程序包

編寫ROS程序包推薦roboware-studio，這款軟件是在開源的vscode基礎上二次開發，用於ROS程序包的開發，非常好用

1. roboware-studio的安裝，roboware-studio軟件包的下載地址，官網已經進不去了，直接到github上下載deb包就好，下載時注意軟件包有32位和64位之分，根據自己系統的類型進行下載，可以雙擊安裝或者使用命令：

1. sudo dpkg -i xxxxxxx.deb

2. 這款集成開發環境很好用，減少了很多手工操作，但是還是建議先弄明白每個文件都是什么作用，先手工編寫幾次軟件包后再考慮使用這種自動化的IDE

 開始編寫ROS程序包

前面的建立程序包的操作這里不贅述了，參考這篇博客，直接介紹如何編寫src文件夾下的.cpp文件，這里以一個控制無人機起飛和降落的程序為例進行講解
示例程序如下：

1. /**
2. * @file offb_node.cpp
3. * @brief Offboard control example node, written with MAVROS version 0.19.x, PX4 Pro Flight
4. * Stack and tested in Gazebo SITL
5. */
7. #include <ros/ros.h>
8. #include <geometry_msgs/PoseStamped.h>
9. #include <mavros_msgs/CommandBool.h>
10. #include <mavros_msgs/SetMode.h>
11. #include <mavros_msgs/State.h>
13. mavros_msgs::State current_state;
14. void state_cb(const mavros_msgs::State::ConstPtr& msg){
15. current_state = *msg;
16. }
17. geometry_msgs::PoseStamped aim_pos;
18. bool flag = true;
19. void arrive_pos(const geometry_msgs::PoseStamped::ConstPtr& msg){
20. if((*msg).pose.position.z > aim_pos.pose.position.z * 0.95){
21. flag = false;
22. }
23. }
25. int main(int argc, char **argv)
26. {
27. ros::init(argc, argv, "takeoff_land_node");
28. ros::NodeHandle nh;
30. ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>
31. ("mavros/state", 10, state_cb);
32. ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>
33. ("mavros/setpoint_position/local", 10);
34. ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>
35. ("mavros/cmd/arming");
36. ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>
37. ("mavros/set_mode");
38. ros::Subscriber local_pos_sub = nh.subscribe<geometry_msgs::PoseStamped>
39. ("mavros/local_position/pose",10,arrive_pos);
41. //the setpoint publishing rate MUST be faster than 2Hz
42. ros::Rate rate(20.0);
44. // wait for FCU connection
45. while(ros::ok() && !current_state.connected){
46. ros::spinOnce();
47. rate.sleep();
48. }
50. geometry_msgs::PoseStamped pose;
51. pose.pose.position.x = 0;
52. pose.pose.position.y = 0;
53. pose.pose.position.z = 10;
55. //send a few setpoints before starting
56. for(int i = 100; ros::ok() && i > 0; --i){
57. local_pos_pub.publish(pose);
58. ros::spinOnce();
59. rate.sleep();
60. }
62. mavros_msgs::SetMode offb_set_mode;
63. offb_set_mode.request.custom_mode = "OFFBOARD";
65. mavros_msgs::CommandBool arm_cmd;
66. arm_cmd.request.value = true;
68. ros::Time last_request = ros::Time::now();
69. flag = true;
70. aim_pos = pose;
71. while(ros::ok()){
72. if( current_state.mode != "OFFBOARD" &&
73. (ros::Time::now() - last_request > ros::Duration(5.0))){
74. if( set_mode_client.call(offb_set_mode) &&
75. offb_set_mode.response.mode_sent){
76. ROS_INFO("Offboard enabled");
77. }
78. last_request = ros::Time::now();
79. } else {
80. if( !current_state.armed &&
81. (ros::Time::now() - last_request > ros::Duration(5.0))){
82. if( arming_client.call(arm_cmd) &&
83. arm_cmd.response.success){
84. ROS_INFO("Vehicle armed");
85. }
86. last_request = ros::Time::now();
87. }
88. }
90. local_pos_pub.publish(pose);
92. ros::spinOnce();
93. rate.sleep();
94. if(flag == false){
95. break;
96. }
97. }
100. mavros_msgs::SetMode land_set_mode;
101. land_set_mode.request.custom_mode = "AUTO.LAND";
102. if(set_mode_client.call(land_set_mode) && land_set_mode.response.mode_sent){
103. ROS_INFO("land enabled");
104. }
105. return 0;
106. }

 

首先解釋一下文件頭

1. #include <ros/ros.h>
2. #include <geometry_msgs/PoseStamped.h>
3. #include <mavros_msgs/CommandBool.h>
4. #include <mavros_msgs/SetMode.h>
5. #include <mavros_msgs/State.h>

這里包含了五個頭文件，mavros_msgs軟件包包含操作MAVROS軟件包提供的服務和主題所需的所有自定義消息每個頭文件都有作用，具體的作用我們到初始化訂閱器和發布器的時候解釋

 

1. mavros_msgs::State current_state;
2. void state_cb(const mavros_msgs::State::ConstPtr& msg){
3. current_state = *msg;
4. }

這里首先定義了一個全局變量current_state用來記錄無人機的狀態，然后創建了一個回調函數，這個回調函數會實時改變全局變量current_state的值

 

1. geometry_msgs::PoseStamped aim_pos;
2. bool flag = true;
3. void arrive_pos(const geometry_msgs::PoseStamped::ConstPtr& msg){
4. if((*msg).pose.position.z > aim_pos.pose.position.z * 0.95){
5. flag = false;
6. }
7. }

這里定義了一個全局變量aim_pos用來記錄無人機運動的目標位置，回調函數會判斷是否到達目標位置，當到達目標位置后，標志變量flag置為false，表示沒有運動命令正在執行，可以執行下一個命令

 

1. ros::init(argc, argv, "takeoff_land_node");
2. ros::NodeHandle nh;

初始化ROS，參數是節點的名稱，名稱不能包含/等符號，然后為這個進程的節點創建一個句柄，第一個創建的NodeHandle會為節點進行初始化，最后一個銷毀的NodeHandle則會釋放該節點的所占用的所有資源。

 

1. ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>
2. ("mavros/state", 10, state_cb);
3. ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>
4. ("mavros/setpoint_position/local", 10);
5. ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>
6. ("mavros/cmd/arming");
7. ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>
8. ("mavros/set_mode");
9. ros::Subscriber local_pos_sub = nh.subscribe<geometry_msgs::PoseStamped>
10. ("mavros/local_position/pose",10,arrive_pos);

這里初始化了四個對象，有訂閱器、發布器、兩個客戶端，如果對訂閱器和發布器之類的不熟悉，建議重新看一下，了解這些對象都有什么作用。

 

1. ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>
2. ("mavros/state", 10, state_cb);

這句代碼實例化一個訂閱器，<mavros_msgs::State>這里是對類模板的實例化，參數是消息類型（使用這個消息類型需要添加頭文件），類模板是C++的相關知識點，不在這里做說明。("mavros/state", 10, state_cb)第一個參數是一個訂閱器的topic，state_sub可以從這個topic上獲得無人機的狀態信息，第二個參數是發布序列的大小，如果我們發布的消息的頻率太高，緩沖區中的消息在大於10個時就會開始丟棄先前發布的消息。
這句代碼告訴master要訂閱”mavros/state”話題上的消息，當有消息發布到這個話題時，ROS就會調用state_cb

 

1. ros::Rate rate(20.0);

ros::Rate 對象可以允許你指定自循環的頻率。它會追蹤記錄自上一次調用 Rate::sleep() 后時間的流逝，並休眠直到一個頻率周期的時間，在這個程序中我們讓它以10Hz的頻率運行，而且無人機進入offboard模式后必須以大於2HZ的頻率發布消息，否則無人機會退出offboard模式

 

1. while(ros::ok() && !current_state.connected){
2. ros::spinOnce();
3. rate.sleep();
4. }

在發布任何內容之前，我們等待在MAVROS和自動駕駛儀之間建立連接，收到心跳消息后，此循環應立即退出

 

1. geometry_msgs::PoseStamped pose;
2. pose.pose.position.x = 0;
3. pose.pose.position.y = 0;
4. pose.pose.position.z = 10;

即使PX4在航空航天NED坐標系中運行，MAVROS也會將這些坐標轉換為標准ENU框架，這里就直接使用坐標就行，

 

1. for(int i = 100; ros::ok() && i > 0; --i){
2. local_pos_pub.publish(pose);
3. ros::spinOnce();
4. rate.sleep();
5. }

在進入offboard模式之前，必須已經開始流式傳輸設定值，否則，模式切換將被拒絕，在這里，100就是隨便設置的，稍微大一些就行

 

1. mavros_msgs::SetMode offb_set_mode;
2. offb_set_mode.request.custom_mode = "OFFBOARD";
4. mavros_msgs::CommandBool arm_cmd;
5. arm_cmd.request.value = true;

這兩段放一起解釋，第一段設置一個offboard模式的消息，供設置模式的客戶端調用時做參數，第二段是設置一個命令的消息，供設置arm的客戶端調用時做參數。后續代碼中會對這兩個變量的使用進行說明。

 

1. ros::Time last_request = ros::Time::now();
2. flag = true;
3. aim_pos = pose;

這段就是記錄當前時間，設置flag值為true,表示接下來要執行無人機飛行命令，記錄一下目標位置，這樣當目標位置到達后，會結束當前執行的飛行命令

 

1. while(ros::ok()){
2. if( current_state.mode != "OFFBOARD" &&
3. (ros::Time::now() - last_request > ros::Duration(5.0))){
4. if( set_mode_client.call(offb_set_mode) &&
5. offb_set_mode.response.mode_sent){
6. ROS_INFO("Offboard enabled");
7. }
8. last_request = ros::Time::now();
9. } else {
10. if( !current_state.armed &&
11. (ros::Time::now() - last_request > ros::Duration(5.0))){
12. if( arming_client.call(arm_cmd) &&
13. arm_cmd.response.success){
14. ROS_INFO("Vehicle armed");
15. }
16. last_request = ros::Time::now();
17. }
18. }
20. local_pos_pub.publish(pose);
22. ros::spinOnce();
23. rate.sleep();
24. if(flag == false){
25. break;
26. }
27. }

這段代碼比較好理解了，首先檢測當前模式是否是offboard，不是則置為offboard，如果當前無人機沒有armed，則設置為armed，然后向無人機的位置設置節點發布位置消息，最后檢測一下是否到達目標位置，到達目標位置后退出循環

 

1. mavros_msgs::SetMode land_set_mode;
2. land_set_mode.request.custom_mode = "AUTO.LAND";
3. if(set_mode_client.call(land_set_mode) && land_set_mode.response.mode_sent){
4. ROS_INFO("land enabled");
5. }

當無人機飛到指定位置后，想無人機發布位置信息的循環會結束，這里設置無人機的模式為land模式，無人機會自動降落，降落到當前位置的下方。
到這里代碼部分就結束了

 

ROS程序包的編譯和運行

編寫完程序包后，先進入catkin工作目錄使用catkin_make對程序包進行編譯

 

1. cd /catkin_ws
2. catkin_make

這時候編寫的ROS程序就可以執行了，不過執行之前先啟動一下無人機仿真環境，選擇一個啟動腳本執行

 

1. roslaunch px4 XXXXXXXX.launch

重新打開一個終端執行：

 

1. source /catkin_ws/devel/setup.bash
2. rosrun 程序包名 節點名

這時候正常就可以看見無人機緩慢移動到10米的高空后又緩緩下落，至此對ROS程序包編寫編譯執行過程的介紹結束。歡迎批評指正。

摘自網路

https://blog.csdn.net/weixin_44479297/article/details/105086582

https://blog.csdn.net/weixin_44479297/article/details/105063476

https://blog.csdn.net/weixin_44479297/article/details/105053555