1. 环境准备与基础概念在开始编写Offboard模式控制节点之前我们需要确保仿真环境已经正确搭建。这里假设你已经完成了PX4固件的下载和编译并且熟悉ROS的基本操作。如果还没有完成这些准备工作可以参考PX4官方文档进行环境配置。1.1 安装必要依赖首先需要安装MAVROS包这是ROS和PX4飞控通信的桥梁。在Ubuntu系统中可以通过以下命令安装sudo apt-get install ros-你的ROS版本-mavros ros-你的ROS版本-mavros-extras然后安装地理围栏数据这对于某些地理围栏功能是必需的wget https://raw.githubusercontent.com/mavlink/mavros/master/mavros/scripts/install_geographiclib_datasets.sh chmod x install_geographiclib_datasets.sh sudo ./install_geographiclib_datasets.sh1.2 理解Offboard模式Offboard模式是PX4飞控提供的一种外部控制模式允许外部系统如ROS节点直接发送控制指令给飞控。与Position模式不同Offboard模式下飞控完全依赖外部系统提供的控制指令不会自主进行任何控制决策。这种模式非常适合需要精确控制的场景比如定点悬停、轨迹跟踪等。在实际应用中Offboard模式需要特别注意以下几点必须持续发送控制指令如果超过一定时间默认0.5秒没有收到新指令飞控会进入保护状态指令发送频率建议在10-50Hz之间需要先发送若干指令通常10-20个再切换模式确保飞控已经准备好接收外部指令2. 启动仿真环境2.1 启动Gazebo仿真进入PX4固件目录使用以下命令启动Gazebo仿真cd ~/PX4-Autopilot make px4_sitl_default gazebo这个命令会启动一个默认的无人机模型在Gazebo仿真环境中。第一次运行可能需要下载一些模型文件请确保网络连接正常。2.2 启动MAVROS节点新开一个终端启动MAVROS节点连接仿真飞控roslaunch mavros px4.launch fcu_url:udp://:14540127.0.0.1:14557这个命令建立了ROS和PX4飞控之间的通信链路。可以通过rostopic list命令查看当前可用的ROS话题应该能看到一系列以/mavros/开头的话题。3. 编写Offboard控制节点3.1 创建ROS包首先创建一个新的ROS包来存放我们的控制节点cd ~/catkin_ws/src catkin_create_pkg offboard_node roscpp mavros geometry_msgs cd ~/catkin_ws catkin_make3.2 编写控制节点代码创建一个新的C文件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::NodeHandle nh; // 订阅飞控状态 ros::Subscriber state_sub nh.subscribemavros_msgs::State (mavros/state, 10, state_cb); // 发布位置指令 ros::Publisher local_pos_pub nh.advertisegeometry_msgs::PoseStamped (mavros/setpoint_position/local, 10); // 服务客户端解锁无人机 ros::ServiceClient arming_client nh.serviceClientmavros_msgs::CommandBool (mavros/cmd/arming); // 服务客户端设置飞行模式 ros::ServiceClient set_mode_client nh.serviceClientmavros_msgs::SetMode (mavros/set_mode); // 设置循环频率为20Hz ros::Rate rate(20.0); // 等待飞控连接 while(ros::ok() !current_state.connected){ ros::spinOnce(); rate.sleep(); } // 设置目标位置x0, y0, z2米 geometry_msgs::PoseStamped pose; pose.pose.position.x 0; pose.pose.position.y 0; pose.pose.position.z 2; // 先发送一些指令确保飞控准备好接收 for(int i 100; ros::ok() i 0; --i){ local_pos_pub.publish(pose); ros::spinOnce(); rate.sleep(); } // 设置Offboard模式 mavros_msgs::SetMode offb_set_mode; offb_set_mode.request.custom_mode OFFBOARD; // 解锁指令 mavros_msgs::CommandBool arm_cmd; arm_cmd.request.value true; ros::Time last_request ros::Time::now(); while(ros::ok()){ // 每5秒尝试一次切换到Offboard模式 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 { // 如果已经在Offboard模式但未解锁尝试解锁 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(Vehicle armed); } last_request ros::Time::now(); } } // 持续发布位置指令 local_pos_pub.publish(pose); ros::spinOnce(); rate.sleep(); } return 0; }3.3 代码解析这段代码实现了以下功能初始化ROS节点和必要的发布者、订阅者、服务客户端等待飞控连接预先发送100个位置指令确保飞控准备好接收外部指令尝试切换到Offboard模式在Offboard模式下解锁无人机持续发布位置指令使无人机悬停在指定位置关键点说明state_cb回调函数用于获取飞控当前状态local_pos_pub用于发布位置指令arming_client用于发送解锁指令set_mode_client用于设置飞行模式20Hz的循环频率保证了指令的持续发送4. 测试与调试4.1 编译运行节点在CMakeLists.txt中添加以下内容add_executable(offboard_node src/offboard_node.cpp) target_link_libraries(offboard_node ${catkin_LIBRARIES})然后编译运行cd ~/catkin_ws catkin_make source devel/setup.bash rosrun offboard_node offboard_node4.2 常见问题排查飞控不切换模式确保已经发送了足够多的预设指令代码中发送了100个检查/mavros/state话题确认飞控已连接确保没有其他节点在发送冲突的指令无人机不悬停检查Gazebo中的无人机是否正常加载查看/mavros/setpoint_position/local话题是否有数据发布检查飞控日志确认是否收到指令指令延迟降低循环频率如从20Hz降到10Hz检查系统负载关闭不必要的程序4.3 进阶调试技巧可以使用RViz可视化无人机位置rosrun rviz rviz在RViz中添加TF和Marker显示可以直观看到无人机的位置和姿态。也可以使用rostopic echo命令查看具体话题内容rostopic echo /mavros/state rostopic echo /mavros/local_position/pose在实际项目中我经常遇到的一个问题是模式切换失败。后来发现是因为没有持续发送指令导致的。PX4要求进入Offboard模式后必须持续接收指令否则会自动退出该模式。这个细节在官方文档中有说明但很容易被忽略。