PX4_pos_estimator

全局变量

相关参数

int input_source;
float rate_hz;
Eigen::Vector3f pos_offset;
float yaw_offset;
string object_name;
ros::Time last_timestamp;

laser定位相关

Eigen::Vector3d pos_drone_laser; //无人机当前位置 (laser)
Eigen::Quaterniond q_laser;
Eigen::Vector3d Euler_laser; //无人机当前姿态(laser)

T265

Eigen::Vector3d pos_drone_t265;
Eigen::Quaterniond q_t265;
Eigen::Vector3d Euler_t265;

gazebo真值相关

Eigen::Vector3d pos_drone_gazebo;
Eigen::Quaterniond q_gazebo;
Eigen::Vector3d Euler_gazebo;

SLAM相关

Eigen::Vector3d pos_drone_slam;
Eigen::Quaterniond q_slam;
Eigen::Vector3d Euler_slam;

发布相关变量

geometry_msgs::TransformStamped laser; //当前时刻cartorgrapher发布的数据
ros::Publisher vision_pub;
ros::Publisher drone_state_pub;
ros::Publisher message_pub;
ros::Publisher odom_pub;
ros::Publisher trajectory_pub;
prometheus_msgs::Message message;
prometheus_msgs::DroneState Drone_State;
nav_msgs::Odometry Drone_odom;
std::vector<geometry_msgs::PoseStamped> posehistory_vector_;

主函数

初始化

ros::init(argc, argv, "px4_pos_estimator");
ros::NodeHandle nh("~");

读取参数表中的参数

//读取参数表中的参数
// 定位数据输入源 0 for vicon， 1 for 激光SLAM, 2 for gazebo ground truth, 3 for T265 ,  9 for outdoor 
nh.param<int>("input_source", input_source, 0);
//　程序执行频率
nh.param<float>("rate_hz", rate_hz, 20);
//　定位设备偏移量
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);

订阅

// 【订阅】cartographer估计位置
ros::Subscriber laser_sub = nh.subscribe<tf2_msgs::TFMessage>("/tf", 100, laser_cb);
// 【订阅】t265估计位置
ros::Subscriber t265_sub = nh.subscribe<nav_msgs::Odometry>("/t265/odom/sample", 100, t265_cb);
// 【订阅】gazebo仿真真值
ros::Subscriber gazebo_sub = nh.subscribe<nav_msgs::Odometry>("/prometheus/ground_truth/p300_basic", 100, gazebo_cb);
// 【订阅】SLAM估计位姿
ros::Subscriber slam_sub = nh.subscribe<geometry_msgs::PoseStamped>("/slam/pose", 100, slam_cb);
// 10秒定时打印，以确保程序在正确运行
ros::Timer timer = nh.createTimer(ros::Duration(10.0), timerCallback);

回调函数

laser_cb

gazebo_cb

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.oriention.w, msg->pose.pose.oriention.x, msg->pose.pose.oriention.y, msg->pose.pose.oriention.z);
Euler_gazebo = quaterniond_to_euler(q_gazebo);

slam_cb

t265_cb

timerCallback

用于与mavros通讯的类，通过mavros接收来至飞控的消息【飞控->mavros->本程序】

state_from_mavros _state_from_mavros;

state_from_mavros订阅的话题

// 【订阅】无人机当前状态 - 来自飞控
state_sub = state_nh.subscribe<mavros_msgs::State>(uav_name + "/mavros/state", 10, &state_from_mavros::state_cb,this);
// 【订阅】无人机当前状态 - 来自飞控
extended_state_sub = state_nh.subscribe<mavros_msgs::ExtendedState>(uav_name + "/mavros/extended_state", 10, &state_from_mavros::extended_state_cb,this);
// 【订阅】无人机当前位置 坐标系:ENU系 （此处注意，所有状态量在飞控中均为NED系，但在ros中mavros将其转换为ENU系处理。所以，在ROS中，所有和mavros交互的量都为ENU系）
position_sub = state_nh.subscribe<geometry_msgs::PoseStamped>(uav_name + "/mavros/local_position/pose", 10, &state_from_mavros::pos_cb,this);
// 【订阅】无人机当前速度 坐标系:ENU系
velocity_sub = state_nh.subscribe<geometry_msgs::TwistStamped>(uav_name + "/mavros/local_position/velocity_local", 10, &state_from_mavros::vel_cb,this);
// 【订阅】无人机当前欧拉角 坐标系:ENU系
attitude_sub = state_nh.subscribe<sensor_msgs::Imu>(uav_name + "/mavros/imu/data", 10, &state_from_mavros::att_cb,this); 
// 【订阅】无人机相对高度 此订阅仅针对户外实验
alt_sub = state_nh.subscribe<std_msgs::Float64>(uav_name + "/mavros/global_position/rel_alt", 10, &state_from_mavros::alt_cb,this);

主循环

send_to_fcu();

• 将采集的机载设备的定位信息及偏航角信息发送至飞控，根据参数input_source选择定位信息来源

pub_to_nodes(_state_from_mavros._DroneState);

• 发布无人机状态至其他节点
• 发布无人机当前odometry,用于导航及rviz显示
• 发布无人机运动轨迹，用于rviz显示
  
  rqt_graph