translated_page: https://github.com/PX4/Devguide/blob/master/en/ros/mavros_offboard.md

translated_sha: 95b39d747851dd01c1fe5d36b24e59ec865e323e

MAVROS机外(offboard)控制例程

注意: 机外控制非常危险。如果在真机上操作,请确保可以在出错的时候切回手动控制。

下面的教程是一个基础的机外控制例子,通过MAVROS在Gazebo中应用于Iris四旋翼上。在教程最后,你应该会得到与下面视频相同的结果,即无人机缓慢起飞到高度2米。

代码

在ROS包中创建offb_node.cpp文件,并粘贴下面内容:

  1. /**
  2. * @file offb_node.cpp
  3. * @brief offboard example node, written with mavros version 0.14.2, px4 flight
  4. * stack and tested in Gazebo SITL
  5. */
  6. #include <ros/ros.h>
  7. #include <geometry_msgs/PoseStamped.h>
  8. #include <mavros_msgs/CommandBool.h>
  9. #include <mavros_msgs/SetMode.h>
  10. #include <mavros_msgs/State.h>
  11. mavros_msgs::State current_state;
  12. void state_cb(const mavros_msgs::State::ConstPtr& msg){
  13. current_state = *msg;
  14. }
  15. int main(int argc, char **argv)
  16. {
  17. ros::init(argc, argv, "offb_node");
  18. ros::NodeHandle nh;
  19. ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>
  20. ("mavros/state", 10, state_cb);
  21. ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>
  22. ("mavros/setpoint_position/local", 10);
  23. ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>
  24. ("mavros/cmd/arming");
  25. ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>
  26. ("mavros/set_mode");
  27. //the setpoint publishing rate MUST be faster than 2Hz
  28. ros::Rate rate(20.0);
  29. // wait for FCU connection
  30. while(ros::ok() && current_state.connected){
  31. ros::spinOnce();
  32. rate.sleep();
  33. }
  34. geometry_msgs::PoseStamped pose;
  35. pose.pose.position.x = 0;
  36. pose.pose.position.y = 0;
  37. pose.pose.position.z = 2;
  38. //send a few setpoints before starting
  39. for(int i = 100; ros::ok() && i > 0; --i){
  40. local_pos_pub.publish(pose);
  41. ros::spinOnce();
  42. rate.sleep();
  43. }
  44. mavros_msgs::SetMode offb_set_mode;
  45. offb_set_mode.request.custom_mode = "OFFBOARD";
  46. mavros_msgs::CommandBool arm_cmd;
  47. arm_cmd.request.value = true;
  48. ros::Time last_request = ros::Time::now();
  49. while(ros::ok()){
  50. if( current_state.mode != "OFFBOARD" &&
  51. (ros::Time::now() - last_request > ros::Duration(5.0))){
  52. if( set_mode_client.call(offb_set_mode) &&
  53. offb_set_mode.response.mode_sent){
  54. ROS_INFO("Offboard enabled");
  55. }
  56. last_request = ros::Time::now();
  57. } else {
  58. if( !current_state.armed &&
  59. (ros::Time::now() - last_request > ros::Duration(5.0))){
  60. if( arming_client.call(arm_cmd) &&
  61. arm_cmd.response.success){
  62. ROS_INFO("Vehicle armed");
  63. }
  64. last_request = ros::Time::now();
  65. }
  66. }
  67. local_pos_pub.publish(pose);
  68. ros::spinOnce();
  69. rate.sleep();
  70. }
  71. return 0;
  72. }

提示: 本过程需要对ROS有一定的了解。
创建工作空间后需要source devel/setup.bash,否则会出现找不到package的情况,要想保证工作空间已配置正确需确保ROS_PACKAGE_PATH环境变量包含你的工作空间目录,采用echo $ROS_PACKAGE_PATH命令查看是否包含了你创建的package的路径,此操作也可以通过直接在.bashrc文件最后添加路径的方式解决。

代码解释

  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包中提供的服务(service)和话题(topic)所需的一切自定义消息。所有服务和话题以及相应的消息类型可参照文档mavros wiki

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

我们创建一个简单的回调函数来保存飞控的当前状态。我们可以用它检查连接状态,解锁状态以及外部控制标志。

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

我们实例化一个用来发布被控制的本地位置的发布器,以及适当的客户端来请求解锁和模式更改。注意,对你自己的系统,”mavros”前缀部分会有所不同,它依赖于对应节点的launch文件中定义的名字。

  1. //the setpoint publishing rate MUST be faster than 2Hz
  2. ros::Rate rate(20.0);

px4飞行栈的两个机外(offboard)控制指令之间有500ms的时限。如果超过了时限,commander指令将会切换回进入机外控制模式前的上一个模式。这正是为什么发布频率必须高于2Hz的原因,并且还要考虑可能的延迟。这也是我们推荐从位置控制(POSCTL)模式进入机外控制模式的原因。这样一来,如果飞机意外脱离了机外控制模式,飞机将会停在当前轨道并悬停。

  1. // wait for FCU connection
  2. while(ros::ok() && current_state.connected){
  3. ros::spinOnce();
  4. rate.sleep();
  5. }

在发布任何东西之前,我们需要等待MAVROS和飞控建立连接。一旦接收到心跳消息heartbeat message,该循环就会立即退出。以上代码是以一定频率(20Hz)来执行ROS消息回调函数,即ros::spinOnce().

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

即使px4飞行栈在航空NED坐标系中运行,MAVROS仍然会将这些坐标转换到标准的ENU坐标系,反之亦然。这是我们将Z设置为+2的原因。

  1. //send a few setpoints before starting
  2. for(int i = 100; ros::ok() && i > 0; --i){
  3. local_pos_pub.publish(pose);
  4. ros::spinOnce();
  5. rate.sleep();
  6. }

在进入机外控制模式之前,就必须开始发送设定值(这里是指pose),否则模式切换会被拒绝。这里的100是一个随意选取的值。

  1. mavros_msgs::SetMode offb_set_mode;
  2. offb_set_mode.request.custom_mode = "OFFBOARD";

设置自定义模式为OFFBOARD。PX4飞行栈所支持的飞行模式可参考这里

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

剩下的代码比较好理解。我们试图在解锁旋翼允许它起飞后,将它切换至机外控制模式。为了避免大量请求堵塞飞控,我们设置服务调用间隔时间为5秒。在同一个循环里,继续以合适的频率持续发布设定的pose。

提示: 为了便于说明,该代码已被简化。在较大的系统中,往往会创建一个新的线程用来周期性地发布设定值。