创建包
mkdir -p ~/catkin_ws/src cd ~/catkin_ws/src catkin_create_pkg offb roscpp mavros cd ~/catkin_ws/src/offb/src vim offb_node.cpp
源码可参考pixhawk官网
/** * @file offb_node.cpp * @brief offboard example node, written with mavros version 0.14.2, px4 flight * stack and tested in Gazebo SITL */ #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.subscribe<mavros_msgs::State> ("mavros/state", 10, state_cb); ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped> ("mavros/setpoint_position/local", 10); ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool> ("mavros/cmd/arming"); ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode> ("mavros/set_mode"); //the setpoint publishing rate MUST be faster than 2Hz ros::Rate rate(20.0); // wait for FCU connection while(ros::ok() && current_state.connected) { ros::spinOnce(); rate.sleep(); } geometry_msgs::PoseStamped pose; pose.pose.position.x = 0; pose.pose.position.y = 0; pose.pose.position.z = 2; //send a few setpoints before starting for(int i = 100; ros::ok() && i > 0; --i) { local_pos_pub.publish(pose); ros::spinOnce(); rate.sleep(); } 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()) { 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 { 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; }
修改CMakeLists.txt
vi src/offb/CMakeLists.txt
## Declare a C++ executable ## With catkin_make all packages are built within a single CMake context ## The recommended prefix ensures that target names across packages don't collide # add_executable(${PROJECT_NAME}_node src/offb_node.cpp) add_executable(offb_node src/offb_node.cpp) # 添加到提示位置
编译
catkin build
运行
可以控制飞机
rosrun offb offb_node