mavros offboard_node控制飞机

创建包

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