无人机：3DR Solo_（6）.3DRSolo高级飞行模式和设置

//相关头文件包含 #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; //创建订阅者，订阅话题mavros/state ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State> ("mavros/state", 10, state_cb); //创建一个发布者，发布"mavros/setpoint_position/local"话题 ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped> ("mavros/setpoint_position/local", 10); //创建一个客户端，该客户端用来请求PX4无人机的解锁 ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool> ("mavros/cmd/arming"); //创建一个客户端，该客户端用来请求进入offboard模式 ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode> ("mavros/set_mode"); //搭配rate.sleep()函数，实现20Hz频率的循环; ros::Rate rate(20.0); // 等待飞控连接 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(); } //设置客户端请求无人机进入“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()){ //if语句循环请求进入OFFBOARD模式，进入以后则会进入else语句请求对无人机解锁。 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(); } } //循环发布期望位置信息，20Hz频率 local_pos_pub.publish(pose); ros::spinOnce(); rate.sleep(); } return 0; }