Building a four rotor multi aircraft flight platform based on ROS and PX4 under Optitrack visual positioning

Keywords: Linux Ubuntu Autonomous vehicles

1 stand alone platform

The first is to build a stand-alone platform, which is divided into two steps

  1. Assemble the aircraft to realize the manual flight of the remote controller
  2. The onboard board is added to realize the autonomous flight of onboard control aircraft

1.1 four rotor hardware assembly

Four rotor hardware components:

  • frame
  • Paddle Blade
  • Battery, motor, electric regulator
  • Flight control, selected pixhawkV4
  • Flight control accessories: safety switch, galvanometer, etc
  • Receiver
  • Remote control

a) Attention

  1. The electric regulation has an appropriate battery, and the appropriate battery size shall be selected according to the electric regulation parameters
  2. After the construction is completed, pay attention to that the flight control is firmly tied, otherwise the manual control flight is unstable, which does not rule out the possibility that the flight control itself does not stick and shake

1.2 airborne board environment configuration

I use raspberry pie 4b, Ubuntu system. You also need to install ROS operating system and mavros function package
Installation tutorial of ROS: 2021-06-01 solution to ROS + initialization failure during Ubuntu 1604 installation
mavros is a function package developed by ROS based on mavlink protocol. It packages specific mavlink related messages into topics for users to use. Mavlink protocol is the communication mode adopted by flight control PX4. Specific installation steps:

sudo apt-get install ros-kinetic-mavros ros-kinetic-mavros-extras

The latter two parts are not realized, and the problem is not big. The actual measurement can be used

1.3 configuration of flight control parameters

In order to realize visual positioning, flight control parameters need to be configured as visual positioning

 - EKF2_HGT_MODE Change to vision pattern
 - EKF2_AID_MASK Tick vision_position_fusion and vision_yaw_fusion For two items, the check result should be 24, if the new version is used QGC The error may be 7 after checking. It is recommended to use the old version of ground station to set this item

In addition, in order to realize the communication between flight control and raspberry pie, it is also necessary to configure communication parameters, formulate specific telem communication ports, and determine the effective working mode and baud rate

Flight control 1.9 The above versions are generally selected Telem2 The port is used as the communication port. It is set to receive communication in the recording mode. The measured baud rate is 921600, which is enough, up to 3000000.
MAV_1_CONFIG = TELEM 2 (MAV_1_CONFIG is often used to map the TELEM 2 port)
MAV_1_MODE = Onboard
SER_TEL2_BAUD = 921600 (921600 or higher recommended for applications like log streaming or FastRTPS)

Flight control 1.9 Following versions
    0 to disable MAVLink output on TELEM2 (default)
    921600 to enable MAVLink output at 921600 baud, 8N1 (recommended)
    57600 to enable MAVLink output at 57600 baud, 8N1
    157600 to enable MAVLink in OSD mode at 57600 baud
    257600 to enable MAVLink in listen-only mode at 57600 baud

The above operations can be found in the PX4 developer documentation

a) Attention

  1. There is a problem with ekf2 fusion in flight control version 1.8. The self positioning data of flight control is often lost and reset. Burning is not recommended
  2. Select MAV_X_CONFIG is OK, just match

1.4 whole process of actual flight


1. Open optirack, build rigid body and send data

2. Raspberry pie vrpn

roslaunch vrpn_cient_ros sample.launch server:= to your own mobile ip address

See for the specific operations of the previous two steps Optitrack instructions - indoor positioning based on ROS & vrpn

3. Raspberry sends mavros to establish communication with flight control,

  • Hardware: I use the serial port cable connection. The method here is optional, but make sure you know what port you choose for raspberry pie
  • Software: Raspberry pie mavros:
roslaunch mavros px4.launch fcu_url:=/dev/ttyUSB0:921600
/dev/ttyUSB0 The raspberry pie port I chose
921600 The baud rate agreed in the flight control for me

4. Raspberry sends the coordinate conversion node, receives the vrpn solution, calculates the position information, and transfers it to the mavros coordinate system
I use it Amu Laboratory The code is changed, and their source code is pasted here

 * px4_pos_estimator.cpp
 * Author: Qyp
 * Update Time: 2020.10.29
 * Description: mavros location estimation program
 *      1. Subscribe to the location information published by laser Slam (cartographer_ros node) and convert it from laser coordinate system to NED coordinate system
 *      2. Subscribe to the location information published by the mocap device (vrpn client ROS node) and convert it from the mocap coordinate system to the NED coordinate system
 *      3. Subscribe to the location, speed and Euler angle information released by flight control for comparison
 *      4. Store flight data, experimental analysis and mapping
 *      5. Select laser SLAM or Mocap equipment as the location source, and release the location and yaw angle (xyz+yaw) to flight control

//Header file
#include <ros/ros.h>

#include <iostream>
#include <Eigen/Eigen>
#include "state_from_mavros.h"
#include "math_utils.h"
#include "prometheus_control_utils.h"
#include "message_utils.h"

using namespace std;
#define TRA_WINDOW 1000
#define TIMEOUT_MAX 0.05
#define NODE_NAME "pos_estimator"
//---------------------------------------Related parameters-----------------------------------------------
int input_source;
float rate_hz;
Eigen::Vector3f pos_offset;
float yaw_offset;
string object_name;
ros::Time last_timestamp;
//---------------------------------------vicon positioning correlation------------------------------------------
Eigen::Vector3d pos_drone_mocap; //UAV current position (vicon)
Eigen::Quaterniond q_mocap;
Eigen::Vector3d Euler_mocap; //UAV current attitude (vicon)
//---------------------------------------laser positioning correlation------------------------------------------
Eigen::Vector3d pos_drone_laser; //UAV current position (laser)
Eigen::Quaterniond q_laser;
Eigen::Vector3d Euler_laser; //UAV current attitude (laser)

geometry_msgs::TransformStamped laser; //Data published by cartographer at the current time
Eigen::Vector3d pos_drone_t265;
Eigen::Quaterniond q_t265;
Eigen::Vector3d Euler_t265;
//---------------------------------------gazebo truth correlation------------------------------------------
Eigen::Vector3d pos_drone_gazebo;
Eigen::Quaterniond q_gazebo;
Eigen::Vector3d Euler_gazebo;
//---------------------------------------SLAM related------------------------------------------
Eigen::Vector3d pos_drone_slam;
Eigen::Quaterniond q_slam;
Eigen::Vector3d Euler_slam;
//---------------------------------------Publish related variables--------------------------------------------
ros::Publisher vision_pub;
ros::Publisher drone_state_pub;
ros::Publisher message_pub;
prometheus_msgs::Message message;
ros::Publisher odom_pub;
ros::Publisher trajectory_pub;
prometheus_msgs::DroneState Drone_State;
nav_msgs::Odometry Drone_odom;
std::vector<geometry_msgs::PoseStamped> posehistory_vector_;
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>Function declaration<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
void send_to_fcu();
void pub_to_nodes(prometheus_msgs::DroneState State_from_fcu);
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>Callback function<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
void laser_cb(const tf2_msgs::TFMessage::ConstPtr &msg)
    //Confirm the / tf message sent by cartographer
    //Sometimes / tf this message is published by more than one publisher
    //Can be changed to TF monitoring
    if (msg->transforms[0].header.frame_id == "map" && msg->transforms[0].child_frame_id == "base_link" && input_source == 1)  
        laser = msg->transforms[0];

        //Position xy [convert the solved position from map coordinate system to world coordinate system]
        pos_drone_laser[0] = laser.transform.translation.x + pos_offset[0];
        pos_drone_laser[1] = laser.transform.translation.y + pos_offset[1];
        pos_drone_laser[2] = laser.transform.translation.z + pos_offset[2]; 

         // Read the Quaternion from the Carto Package [Frame: Laser[ENU]]
         Eigen::Quaterniond q_laser_enu(laser.transform.rotation.w, laser.transform.rotation.x, laser.transform.rotation.y, laser.transform.rotation.z);

         q_laser = q_laser_enu;

         // Transform the Quaternion to Euler Angles
         Euler_laser = quaternion_to_euler(q_laser);

        // pub_message(message_pub, prometheus_msgs::Message::NORMAL, NODE_NAME, "test.");

            //cout << "Position [X Y Z] : " << pos_drone_laser[0] << " [ m ] "<< pos_drone_laser[1]<<" [ m ] "<< pos_drone_laser[2]<<" [ m ] "<<endl;
            //cout << "Euler [X Y Z] : " << Euler_laser[0] << " [m/s] "<< Euler_laser[1]<<" [m/s] "<< Euler_laser[2]<<" [m/s] "<<endl;

void mocap_cb(const geometry_msgs::PoseStamped::ConstPtr &msg)
    //Location -- optitrack system to ENU system
    //Frame convention 0: Z-up -- 1: Y-up (See the configuration in the motive software)
    int optitrack_frame = 0;
    if (optitrack_frame == 0)
        // Read the Drone Position from the Vrpn Package [Frame: Vicon]  (Vicon to ENU frame)
        pos_drone_mocap = Eigen::Vector3d(msg->pose.position.x, msg->pose.position.y, msg->pose.position.z);

        pos_drone_mocap[0] = pos_drone_mocap[0] - pos_offset[0];
        pos_drone_mocap[1] = pos_drone_mocap[1] - pos_offset[1];
        pos_drone_mocap[2] = pos_drone_mocap[2] - pos_offset[2];
        // Read the Quaternion from the Vrpn Package [Frame: Vicon[ENU]]
        q_mocap = Eigen::Quaterniond(msg->pose.orientation.w, msg->pose.orientation.x, msg->pose.orientation.y, msg->pose.orientation.z);
        // Read the Drone Position from the Vrpn Package [Frame: Vicon]  (Vicon to ENU frame)
        pos_drone_mocap = Eigen::Vector3d(-msg->pose.position.x, msg->pose.position.z, msg->pose.position.y);
        // Read the Quaternion from the Vrpn Package [Frame: Vicon[ENU]]
        q_mocap = Eigen::Quaterniond(msg->pose.orientation.w, msg->pose.orientation.x, msg->pose.orientation.z, msg->pose.orientation.y); //Y-up convention, switch the q2 & q3
        pos_drone_mocap[0] = pos_drone_mocap[0] - pos_offset[0];
        pos_drone_mocap[1] = pos_drone_mocap[1] - pos_offset[1];
        pos_drone_mocap[2] = pos_drone_mocap[2] - pos_offset[2];

    // Transform the Quaternion to Euler Angles
    Euler_mocap = quaternion_to_euler(q_mocap);
    last_timestamp = msg->header.stamp;

void gazebo_cb(const nav_msgs::Odometry::ConstPtr &msg)
    if (msg->header.frame_id == "world")
        pos_drone_gazebo = Eigen::Vector3d(msg->pose.pose.position.x, msg->pose.pose.position.y, msg->pose.pose.position.z);

        q_gazebo = Eigen::Quaterniond(msg->pose.pose.orientation.w, msg->pose.pose.orientation.x, msg->pose.pose.orientation.y, msg->pose.pose.orientation.z);
        Euler_gazebo = quaternion_to_euler(q_gazebo);
        // Euler_gazebo[2] = Euler_gazebo[2] + yaw_offset;
        // q_gazebo = quaternion_from_rpy(Euler_gazebo);
        pub_message(message_pub, prometheus_msgs::Message::NORMAL, NODE_NAME, "wrong gazebo ground truth frame id.");

void slam_cb(const geometry_msgs::PoseStamped::ConstPtr &msg)
    if (msg->header.frame_id == "map_slam")
        pos_drone_slam = Eigen::Vector3d(msg->pose.position.x, msg->pose.position.y, msg->pose.position.z);
        // pos_drone_gazebo[0] = msg->pose.pose.position.x + pos_offset[0];
        // pos_drone_gazebo[1] = msg->pose.pose.position.y + pos_offset[1];
        // pos_drone_gazebo[2] = msg->pose.pose.position.z + pos_offset[2];

        q_slam = Eigen::Quaterniond(msg->pose.orientation.w, msg->pose.orientation.x, msg->pose.orientation.y, msg->pose.orientation.z);
        Euler_slam = quaternion_to_euler(q_slam);
        // Euler_gazebo[2] = Euler_gazebo[2] + yaw_offset;
        // q_gazebo = quaternion_from_rpy(Euler_gazebo);
        pub_message(message_pub, prometheus_msgs::Message::NORMAL, NODE_NAME, "wrong slam frame id.");

void t265_cb(const nav_msgs::Odometry::ConstPtr &msg)
    if (msg->header.frame_id == "t265_odom_frame")
        pos_drone_t265 = Eigen::Vector3d(msg->pose.pose.position.x, msg->pose.pose.position.y, msg->pose.pose.position.z);
        // pos_drone_t265[0] = msg->pose.pose.position.x + pos_offset[0];
        // pos_drone_t265[1] = msg->pose.pose.position.y + pos_offset[1];
        // pos_drone_t265[2] = msg->pose.pose.position.z + pos_offset[2];

        q_t265 = Eigen::Quaterniond(msg->pose.pose.orientation.w, msg->pose.pose.orientation.x, msg->pose.pose.orientation.y, msg->pose.pose.orientation.z);
        Euler_t265 = quaternion_to_euler(q_gazebo);
        // Euler_t265[2] = Euler_t265[2] + yaw_offset;
        // q_t265 = quaternion_from_rpy(Euler_t265);
        pub_message(message_pub, prometheus_msgs::Message::NORMAL, NODE_NAME, "wrong t265 frame id.");

void timerCallback(const ros::TimerEvent &e)
    pub_message(message_pub, prometheus_msgs::Message::NORMAL, NODE_NAME, "Program is running.");
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>Main function<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
int main(int argc, char **argv)
    ros::init(argc, argv, "px4_pos_estimator");
    ros::NodeHandle nh("~");

    //Read the parameters in the parameter table
    // Location data input source 0 for icon, 1 for laser slam, 2 for gazebo ground truth, 3 for t265, 9 for outdoor 
    nh.param<int>("input_source", input_source, 0);
    // The name of the rigid body set in the motion capture device
    nh.param<string>("object_name", object_name, "UAV");
    //Program execution frequency
    nh.param<float>("rate_hz", rate_hz, 20);
    //Positioning device offset
    nh.param<float>("offset_x", pos_offset[0], 0);
    nh.param<float>("offset_y", pos_offset[1], 0);
    nh.param<float>("offset_z", pos_offset[2], 0);
    nh.param<float>("offset_yaw", yaw_offset, 0);

    // [subscription] cartographer estimated location
    ros::Subscriber laser_sub = nh.subscribe<tf2_msgs::TFMessage>("/tf", 100, laser_cb);

    //  [subscription] t265 estimated location
    ros::Subscriber t265_sub = nh.subscribe<nav_msgs::Odometry>("/t265/odom/sample", 100, t265_cb);

    // [subscription] optitrack estimated location
    ros::Subscriber optitrack_sub = nh.subscribe<geometry_msgs::PoseStamped>("/vrpn_client_node/"+ object_name + "/pose", 100, mocap_cb);

    // [subscription] gazebo simulation truth value
    ros::Subscriber gazebo_sub = nh.subscribe<nav_msgs::Odometry>("/prometheus/ground_truth/p300_basic", 100, gazebo_cb);

    // [subscription] SLAM estimated pose
    ros::Subscriber slam_sub = nh.subscribe<geometry_msgs::PoseStamped>("/slam/pose", 100, slam_cb);

    // [release] UAV position and yaw angle coordinate system ENU system
    //  This topic is to be sent to flight control (via mavros_extras/src/plugins/vision_pose_estimate.cpp), and the corresponding Mavlink message is VISION_POSITION_ESTIMATE(#102), the corresponding uORB message in flight control is vehicle_vision_position.msg and vehicle_vision_attitude.msg
    vision_pub = nh.advertise<geometry_msgs::PoseStamped>("/mavros/vision_pose/pose", 10);

    // [release] UAV status quantity
    drone_state_pub = nh.advertise<prometheus_msgs::DroneState>("/prometheus/drone_state", 10);

    //[release] UAV odometry for RVIZ display
    odom_pub = nh.advertise<nav_msgs::Odometry>("/prometheus/drone_odom", 10);

    // [release] UAV movement track for RVIZ display
    trajectory_pub = nh.advertise<nav_msgs::Path>("/prometheus/drone_trajectory", 10);
    // [publish] prompt message
    message_pub = nh.advertise<prometheus_msgs::Message>("/prometheus/message/main", 10);

    // Print regularly for 10 seconds to ensure that the program is running correctly
    ros::Timer timer = nh.createTimer(ros::Duration(10.0), timerCallback);

    // The class used to communicate with mavros and receive the message from mavros to flight control [flight control - > mavros - > this program]
    state_from_mavros _state_from_mavros;

    // frequency
    ros::Rate rate(rate_hz);

    //>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>Main Loop<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
    while (ros::ok())
        //The callback updates the sensor status once

        // Send the collected positioning information and yaw angle information of airborne equipment to flight control according to parameter input_source select the location information source

        // Release UAV status to other nodes, such as px4_pos_controller.cpp node


    return 0;

void send_to_fcu()
    geometry_msgs::PoseStamped vision;

    if (input_source == 0)
        vision.pose.position.x = pos_drone_mocap[0];
        vision.pose.position.y = pos_drone_mocap[1];
        vision.pose.position.z = pos_drone_mocap[2];

        vision.pose.orientation.x = q_mocap.x();
        vision.pose.orientation.y = q_mocap.y();
        vision.pose.orientation.z = q_mocap.z();
        vision.pose.orientation.w = q_mocap.w();
        // The time here is mainly used to monitor whether the mobile capture and T265 equipment work normally
        if( prometheus_control_utils::get_time_in_sec(last_timestamp) > TIMEOUT_MAX)
            pub_message(message_pub, prometheus_msgs::Message::ERROR, NODE_NAME, "Mocap Timeout.");
        } //laser
    else if (input_source == 1)
        vision.pose.position.x = pos_drone_laser[0];
        vision.pose.position.y = pos_drone_laser[1];
        vision.pose.position.z = pos_drone_laser[2];
        //At present, it is a two-dimensional radar simulation, so the z-axis uses other sources
        vision.pose.position.z = pos_drone_gazebo[2];

        vision.pose.orientation.x = q_laser.x();
        vision.pose.orientation.y = q_laser.y();
        vision.pose.orientation.z = q_laser.z();
        vision.pose.orientation.w = q_laser.w();
    else if (input_source == 2)
        vision.pose.position.x = pos_drone_gazebo[0];
        vision.pose.position.y = pos_drone_gazebo[1];
        vision.pose.position.z = pos_drone_gazebo[2];

        vision.pose.orientation.x = q_gazebo.x();
        vision.pose.orientation.y = q_gazebo.y();
        vision.pose.orientation.z = q_gazebo.z();
        vision.pose.orientation.w = q_gazebo.w();
    else if (input_source == 3)
        vision.pose.position.x = pos_drone_t265[0];
        vision.pose.position.y = pos_drone_t265[1];
        vision.pose.position.z = pos_drone_t265[2];

        vision.pose.orientation.x = q_t265.x();
        vision.pose.orientation.y = q_t265.y();
        vision.pose.orientation.z = q_t265.z();
        vision.pose.orientation.w = q_t265.w();
    else if (input_source == 4)
        vision.pose.position.x = pos_drone_slam[0];
        vision.pose.position.y = pos_drone_slam[1];
        vision.pose.position.z = pos_drone_slam[2];

        vision.pose.orientation.x = q_slam.x();
        vision.pose.orientation.y = q_slam.y();
        vision.pose.orientation.z = q_slam.z();
        vision.pose.orientation.w = q_slam.w();

    vision.header.stamp = ros::Time::now();

void pub_to_nodes(prometheus_msgs::DroneState State_from_fcu)
    // Release UAV status. See Prometheus for details_ msgs::DroneState
    Drone_State = State_from_fcu;
    Drone_State.header.stamp = ros::Time::now();
    // Outdoor conditions, use relative height
    if(input_source == 9 )
        Drone_State.position[2]  = Drone_State.rel_alt;

    // Release the current odometry of UAV for navigation and rviz display
    nav_msgs::Odometry Drone_odom;
    Drone_odom.header.stamp = ros::Time::now();
    Drone_odom.header.frame_id = "world";
    Drone_odom.child_frame_id = "base_link";

    Drone_odom.pose.pose.position.x = Drone_State.position[0];
    Drone_odom.pose.pose.position.y = Drone_State.position[1];
    Drone_odom.pose.pose.position.z = Drone_State.position[2];

    // The height specified in the navigation algorithm cannot be less than 0
    if (Drone_odom.pose.pose.position.z <= 0)
        Drone_odom.pose.pose.position.z = 0.01;

    Drone_odom.pose.pose.orientation = Drone_State.attitude_q;
    Drone_odom.twist.twist.linear.x = Drone_State.velocity[0];
    Drone_odom.twist.twist.linear.y = Drone_State.velocity[1];
    Drone_odom.twist.twist.linear.z = Drone_State.velocity[2];

    // Release UAV trajectory for rviz display
    geometry_msgs::PoseStamped drone_pos;
    drone_pos.header.stamp = ros::Time::now();
    drone_pos.header.frame_id = "world";
    drone_pos.pose.position.x = Drone_State.position[0];
    drone_pos.pose.position.y = Drone_State.position[1];
    drone_pos.pose.position.z = Drone_State.position[2];

    drone_pos.pose.orientation = Drone_State.attitude_q;

    //Publish the pose and trajectory of UAV for display in rviz
    posehistory_vector_.insert(posehistory_vector_.begin(), drone_pos);
    if (posehistory_vector_.size() > TRA_WINDOW)

    nav_msgs::Path drone_trajectory;
    drone_trajectory.header.stamp = ros::Time::now();
    drone_trajectory.header.frame_id = "world";
    drone_trajectory.poses = posehistory_vector_;

5. So far, the complete communication link of the stand-alone platform has been built. Here is my hover code. Finally, start the control node, and then cut the arm and offboard with the remote controller to realize fixed-point hovering


2 multi computer communication

Compared with single machine, the core requirement of multi machine is information sharing. I use the host network provided by ROS based on raspberry pie

2.1 multi machine ip address storage

First, all the raspberry pies on the plane and my host (which can be A raspberry pie on A certain plane or your laptop) are referred to as the plane. The meaning of this step is to let plane A and plane B store each other's ip addresses. Otherwise, when A wants to retrieve the topic published by B, although they can find the topic in the shared ROS network, they can't find the corresponding ip entry, Unable to read data. (my personal summary, specific principles ⑧ understand)
View the ip name of each aircraft


This name should be saved in other planes. It can be modified if it is not easy to type

sudo gedit /etc/hostname
 After modification, the network needs to be updated to take effect

ip addresses of other aircraft are stored in each aircraft

sudo gedit /etc/hosts

Fill in, for example (tap the space in the middle)	uav1	uav2

2.2 ROS host address setting

Then, in order for all aircraft to access the same network, we need to specify a host. For example, if I use my laptop as the host and its hostname is legion, we need to do it in other slaves

sudo gedit ~/.bashrc

Then add the ROS host address

export ROS_MASTER_URI=http://legion:11311

Finish source

2.3 actual flight verification

1. The host computer starts the ROS network, starts the vrpn, and releases the position data of all aircraft

2. Each aircraft starts with mavros and coordinate conversion nodes. px4.launch is required here. I start with group, that is, add a prefix, otherwise a network node will conflict, and the coordinate conversion node is also the problem. My launch file:

To be added

3. After the communication link is built, I post a code implemented by the virtual point method. As above, the host computer acts as the control node, and the remote controller switches to arm and offboard

To be added

Posted by daneilair on Sat, 02 Oct 2021 16:58:33 -0700