PX4 Offboard Control Using MAVROS on ROS

本文详细介绍了如何在Ubuntu环境中配置并利用PX4飞控的offboard模式,配合外部设备控制无人机在2米高度自主飞行。步骤包括安装依赖、创建工作空间、编写并编译offboard_node.cpp,以及在实际环境中通过jmavsim进行软件在环仿真和地面站控制。

pixhawk4飞控作为一款开源且流行的飞控,在其硬件版本上可以支持APM固件与PX4原生固件。本文针对PX4原生固件进行介绍。PX4中的offboard模式能够接受来自外部的控制指令,搭配机载或支持MAVROS的协同计算机(如tx1,tx2,树莓派,dji妙算等等),可在PX4飞控平台上加入视觉处理或人工智能,以实现无人机自动控制功能。这里详细介绍一个offboard的例程,外部控制飞机自主飞行2m的高度。

首先需要在Ubuntu的环境下安装好ros,mavros以及px4原生固件

1.为外部控制例程建立一个工作空间   

mkdir -p px4_offboard_ws/src

2.新建一个用于offboard的功能包

cd px4_offboard_ws/src
catkin_create_pkg offboard roscpp std_msgs geometry_msgs mavros_msgs

3.建立好ros包过后,新建一个可cpp源文件用来作为这个功能包的执行文件

cd offboard/src
gedit offboard_node.cpp

4.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 **a
#include <ros/ros.h> #include <std_msgs/Bool.h> #include <geometry_msgs/PoseStamped.h> #include <mavros_msgs/CommandBool.h> #include <mavros_msgs/SetMode.h> #include <mavros_msgs/State.h> #include <mavros_msgs/PositionTarget.h> #include <cmath> #include <tf/transform_listener.h> #include <nav_msgs/Odometry.h> #include <string> #include <geometry_msgs/Twist.h> #include <mavros_msgs/CommandTOL.h> using namespace std; mavros_msgs::PositionTarget setpoint_raw; //定义变量,用于接收无人机的里程计信息 tf::Quaternion quat; double roll, pitch, yaw; float init_position_x_take_off =0; float init_position_y_take_off =0; float init_position_z_take_off =0; bool flag_init_position = false; nav_msgs::Odometry local_pos; void local_pos_cb(const nav_msgs::Odometry::ConstPtr& msg) { local_pos = *msg; if (flag_init_position==false && (local_pos.pose.pose.position.z!=0)) { init_position_x_take_off = local_pos.pose.pose.position.x; init_position_y_take_off = local_pos.pose.pose.position.y; init_position_z_take_off = local_pos.pose.pose.position.z; flag_init_position = true; } tf::quaternionMsgToTF(local_pos.pose.pose.orientation, quat); tf::Matrix3x3(quat).getRPY(roll, pitch, yaw); } mavros_msgs::State current_state; void state_cb(const mavros_msgs::State::ConstPtr& msg) { current_state = *msg; } int main(int argc, char **argv) { //初始化节点,名称为v2_ar_track_landing ros::init(argc, argv, "offboard_multi_position"); //创建句柄 ros::NodeHandle nh; //订阅无人机状态话题 ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>("mavros/state", 100, state_cb); //订阅无人机实时位置信息 ros::Subscriber local_pos_sub = nh.subscribe<nav_msgs::Odometry>("/mavros/local_position/odom", 100, local_pos_cb); //发布无人机多维控制话题 ros::Publisher mavros_setpoint_pos_pub = nh.advertise<mavros_msgs::PositionTarget>("/mavros/setpoint_raw/local", 100); //请求无人机解锁服务 ros::ServiceClient arming_cl = nh.serviceClient<mavros_msgs::CommandBool>("/mavros/cmd/arming"); //请求无人机设置飞行模式,本代码请求进入offboard ros::ServiceClient cl = nh.serviceClient<mavros_msgs::SetMode>("/mavros/set_mode"); ros::ServiceClient takeoff_cl = nh.serviceClient<mavros_msgs::CommandTOL>("/mavros/cmd/takeoff"); //循环频率 ros::Rate rate(20.0); ROS_INFO("Initializing..."); while (ros::ok() && !current_state.connected) { ros::spinOnce(); rate.sleep(); } ROS_INFO("Connected."); // 设置无人机模式为GUIDED mavros_msgs::SetMode srv_setMode; srv_setMode.request.base_mode = 0; srv_setMode.request.custom_mode = "GUIDED"; if (cl.call(srv_setMode)) { ROS_INFO("setmode send ok %d value:", srv_setMode.response.mode_sent); } else { ROS_ERROR("Failed SetMode"); return -1; } // 解锁无人机 mavros_msgs::CommandBool srv; srv.request.value = true; if (arming_cl.call(srv)) { ROS_INFO("ARM send ok %d", srv.response.success); } else { ROS_ERROR("Failed arming or disarming"); } // 发布起飞的指令,向上飞1米 mavros_msgs::CommandTOL srv_takeoff; srv_takeoff.request.altitude = 0; srv_takeoff.request.latitude = 0; srv_takeoff.request.longitude = 0; srv_takeoff.request.min_pitch = 0; srv_takeoff.request.yaw = 0; if (takeoff_cl.call(srv_takeoff)) { ROS_INFO("srv_takeoff send ok %d", srv_takeoff.response.success); } else { ROS_ERROR("Failed Takeoff"); } sleep(10); bool flag_arrive = false; while(ros::ok()) { setpoint_raw.type_mask = /*1 + 2 + 4 + 8 + 16 + 32*/ + 64 + 128 + 256 + 512 /*+ 1024 + 2048*/; setpoint_raw.coordinate_frame = 1; //setpoint_raw.velocity.x = 0.2; //setpoint_raw.velocity.y = 0.0; setpoint_raw.yaw = 3.14; //setpoint_raw.position.x =init_position_x_take_off + 2; //setpoint_raw.position.y =init_position_y_take_off + 0; setpoint_raw.position.z =init_position_z_take_off + 1; mavros_setpoint_pos_pub.publish(setpoint_raw); ros::spinOnce(); rate.sleep(); } return 0; }
最新发布
07-29
评论
成就一亿技术人!
拼手气红包6.0元
还能输入1000个字符
 
红包 添加红包
表情包 插入表情
 条评论被折叠 查看
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值