1 stand alone platform
The first is to build a stand-alone platform, which is divided into two steps
- Assemble the aircraft to realize the manual flight of the remote controller
- 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
- The electric regulation has an appropriate battery, and the appropriate battery size shall be selected according to the electric regulation parameters
- 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
wget https://raw.githubusercontent.com/mavlink/mavros/master/mavros/scripts/install_geographiclib_datasets.sh ./install_geographiclib_datasets.sh
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
- 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
- Select MAV_X_CONFIG is OK, just match
1.4 whole process of actual flight
Steps:
1. Open optirack, build rigid body and send data
2. Raspberry pie vrpn
roslaunch vrpn_cient_ros sample.launch server:=192.168.1.50//Change 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
//---------------------------------------T265------------------------------------------
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);
}
else
{
// 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);
}
else
{
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);
}
else
{
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);
}
else
{
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
ros::spinOnce();
// 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
send_to_fcu();
// Release UAV status to other nodes, such as px4_pos_controller.cpp node
pub_to_nodes(_state_from_mavros._DroneState);
rate.sleep();
}
return 0;
}
void send_to_fcu()
{
geometry_msgs::PoseStamped vision;
//vicon
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();
vision_pub.publish(vision);
}
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;
}
drone_state_pub.publish(Drone_State);
// 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];
odom_pub.publish(Drone_odom);
// 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)
{
posehistory_vector_.pop_back();
}
nav_msgs::Path drone_trajectory;
drone_trajectory.header.stamp = ros::Time::now();
drone_trajectory.header.frame_id = "world";
drone_trajectory.poses = posehistory_vector_;
trajectory_pub.publish(drone_trajectory);
}
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
//hover
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
hostname
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)
192.168.1.21 uav1 192.168.1.22 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