T265定位的px4无人机通过offboard控制

先来说说硬件

主控：香橙派5
飞控：nxtpx4-v2
定位源：T265

这个网上有很多教程，我看了看还是决定记录一下，兴许能够帮助到大家。

方案采用的是VIO来进行t265到mavros的转换

这个源码已经消失了，我从gitee上找到的。
源码链接VIO: 将VIO里程计转换到PX4的mavros/vision/pose话题也可以看看其他的源码，毕竟能用就行

首先从飞控开始，nxtpx4-v2我找了半天，没有找到选择定位源的选项（但是可以正常使用），如果用的其他飞控可以看看这个参数EKF2_AID_MASK

设置成视觉定位和定yaw角

接下来配置香橙派5
1.ros

首先是ros，我还是推荐鱼香ros大佬的一键安装
 

wget http://fishros.com/install -O fishros && . fishros

安装完ros1之后我们来配置realsense

2.安装realsense

这里由于t265版本新的不支持，我们选择2.50版本

这里我找的是gitee的代码这里贴个链接Librealsense: librealsense 2.50.0驱动安装，ros1中librealsense 2.50.0与realsense-ros 2.3.2匹配 (ros2中librealsense 2.50.0是realsense-ros 3.2.3)； 安装顺序为librealsense 2.50.0、 ddynamic_reconfigure、realsense-ros 2.3.2

并且下方的readme写的很清晰了可以跟着去安装，遇到错误就百度

我用的是noetic，记住安装依赖要对应版本

再贴几个链接Jetson nano+T265+PX4实现室内定点飞行_t265安装位置修改-CSDN博客我觉得这位佬写的还不错

安完之后插上测试一下能够正常启动

roslaunch realsense2_camera rs_t265.launch

3.安装mavros

sudo apt install ros-noetic-mavros ros-noetic-mavros-extras

怕有人不懂，mavros是为了建立ros和px4连接的一个包启动这个机载电脑就可以连接到px4底层飞控 

4.vio

然后去下载vio源码VIO: 将VIO里程计转换到PX4的mavros/vision/pose话题

当时我用vision_to_mavros报错了懒得解决就没用这个，使用的vio

可以跟着下方的readme去部署

接下来根据摄像头的位置去修改参数可以参考px4官方的文档Intel® RealSense™ Tracking Camera T265 (VIO) | PX4 Guide (main)

如果摄像机的镜头朝下安装（默认设置），则VIO桥接包不需要任何配置。
对于任何其他方向，请在修改bridge_mavros.launch中以下部分

<node pkg="tf" type="static_transform_publisher" name="tf_baseLink_cameraPose"
    args="0 0 0 0 1.5708 0 base_link camera_pose_frame 1000"/>
这是将ROS中摄像头帧camera_pose_frame链接到mavros无人机帧base_link的静态转换。
前三个参数指定解释为从飞控中心到摄像机的 x，y，z方向，以米为单位。 例如，如果摄像头在飞控前方10cm，上方4cm, 则前三个数字将是：[0.1, 0, 0.04 …]
接下来的三个 参数 是旋转弧度(偏航, 俯仰, 横滚)。 所以 [… 0, 1.5708, 0] 是向下俯仰90度(朝向地面)。 直面是 [… 0 0 0]。

PX4中还需要修改下面三个参数。
EKF2_EV_POS_X, EKF2_EV_POS_Y, EKF2_EV_POS_Z 设置T265相对于机体的位置。

也可以看看超维空间的博客PX4实战之旅（五）：利用T265实现室内定点飞行-CSDN博客

配置好了之后就可以source一下首先启动mavros

roslaunch mavros px4.launch

（vio_ws是你的工作空间，放vio源码的）

cd ~/vio_ws  
catkin_make

source devel/setup.bash 
roslaunch px4_realsense_bridge bridge.launch

 现在可以看一下rostopic

里面如果有一个/mavros/odometru/out话题有稳定数据的话就可以（可以晃晃飞机）

rostopic echo /mavros/odometry/out

此时也可以通过去查看地面站的mavlink检测去查看 

---

如果有NED这个数据就说明连上了

然后此时飞机可以进入定点模式（手痒的可以飞飞看看）

5.offboard代码控制

这里我用的c++

先来个参考链接MAVROS Offboard 控制示例 (C++) | PX4 Guide (main)

先创建工作空间

mkdir -p off_ws/src
cd off_ws/src
touch offbord.cpp
vi offboard.cpp

把代码复制进去

/**
 * @file offb_node.cpp
 * @brief Offboard control example node, written with MAVROS version 0.19.x, PX4 Pro Flight
 * Stack and tested in Gazebo Classic 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;
}

代码解释我就不说了，链接有

我们可以通过更改

geometry_msgs::PoseStamped pose; 
pose.pose.position.x = 0; 
pose.pose.position.y = 0;
pose.pose.position.z = 2;

这个来控制刚起飞的时候无人机的位置

接下来更改cmakelist文件

把src同一目录下的CMakeLists功能包改成

  geometry_msgs
  mavros_msgs
  roscpp
  std_msgs

然后我参考这个链接PX4 & Gazebo & Mavros 外部控制画矩形 — 位置+速度话题 - 知乎

通过去等待定时器来选择在某一点悬停时间

也可以直接复制它的代码，我这里就不贴了，直接参考链接就行

记得改CMakeLists

然后在工作空间目录下

catkin_make

6.测试

我们首先运行px4

roslaunch mavros px4.launch

接下来运行t265和px4桥接 它会自动启动t265

cd vio_ws
source devel/setup.bash 
roslaunch px4_realsense_bridge bridge.launch

先把遥控器切换到offboard模式

然后启动自己写的offboard代码

cd off_ws
source ./devel/setup.bash
rosrun offboard offboard

这里要记得改成自己的节点和代码名字

然后就可以看见无人机自动起飞了