这篇教程来详细介绍一下如何编写出一个控制无人机的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. */

6. 

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>

12. 

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. }

24. 

25. int main(int argc, char **argv)
26. {

27. ros::init(argc, argv, "takeoff_land_node");

28. ros::NodeHandle nh;

29. 

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);

40. 

41. //the setpoint publishing rate MUST be faster than 2Hz

42. ros::Rate rate(20.0);

43. 

44. // wait for FCU connection

45. while(ros::ok() && !current_state.connected){

46. ros::spinOnce();

47. rate.sleep();

48. }

49. 

50. geometry_msgs::PoseStamped pose;

51. pose.pose.position.x = 0;

52. pose.pose.position.y = 0;

53. pose.pose.position.z = 10;

54. 

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. }

61. 

62. mavros_msgs::SetMode offb_set_mode;

63. offb_set_mode.request.custom_mode = "OFFBOARD";

64. 

65. mavros_msgs::CommandBool arm_cmd;

66. arm_cmd.request.value = true;

67. 

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. }

89. 

90. local_pos_pub.publish(pose);

91. 

92. ros::spinOnce();

93. rate.sleep();

94. if(flag == false){

95. break;

96. }

97. }

98. 

99. 

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";

3. 

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. }

19. 

20. local_pos_pub.publish(pose);

21. 

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