使用 ROS1-Noetic 和 mavros v1.20.1,
携带经纬度海拔的话题主要有三个:
- /mavros/global_position/raw/fix
- /mavros/gpsstatus/gps1/raw
- /mavros/global_position/global
查看 mavros 源码,来分析他们的发布过程。发现前两个话题都对应了同一个 mavlink 消息,他们都在 GPS_RAW_INT 的订阅回调中发布,但是对应不同的源文件,对信息的处理方法略有不同。
/mavros/global_position/raw/fix
的发布在源文件 mavros/mavros/src/plugins/global_position.cpp
中
raw_fix_pub = gp_nh.advertise<sensor_msgs::NavSatFix>("raw/fix", 10);
/mavros/gpsstatus/gps1/raw
的发布在插件中 mavros/mavros_extras/src/plugins/gps_status.cpp
中
gps1_raw_pub = gpsstatus_nh.advertise<mavros_msgs::GPSRAW>("gps1/raw", 10);
搜索两个发布者被调用的位置。
raw_fix_pub 主要用来将原始 GPS 数据(未经 EKF 融合)转发到 /mavros/global_position/raw/fix
,并对海拔进行了转换,符合 WGS-84。
// mavros/mavros/src/plugins/global_position.cpp
void handle_gps_raw_int(const mavlink::mavlink_message_t *msg, mavlink::common::msg::GPS_RAW_INT &raw_gps)
{
auto fix = boost::make_shared<sensor_msgs::NavSatFix>();
fix->header = m_uas->synchronized_header(child_frame_id, raw_gps.time_usec);
fix->status.service = sensor_msgs::NavSatStatus::SERVICE_GPS;
if (raw_gps.fix_type > 2)
fix->status.status = sensor_msgs::NavSatStatus::STATUS_FIX;
else {
ROS_WARN_THROTTLE_NAMED(30, "global_position", "GP: No GPS fix");
fix->status.status = sensor_msgs::NavSatStatus::STATUS_NO_FIX;
}
fill_lla(raw_gps, fix);
float eph = (raw_gps.eph != UINT16_MAX) ? raw_gps.eph / 1E2F : NAN;
float epv = (raw_gps.epv != UINT16_MAX) ? raw_gps.epv / 1E2F : NAN;
ftf::EigenMapCovariance3d gps_cov(fix->position_covariance.data());
// With mavlink v2.0 use accuracies reported by sensor
if (msg->magic == MAVLINK_STX &&
raw_gps.h_acc > 0 && raw_gps.v_acc > 0) {
gps_cov.diagonal() << std::pow(raw_gps.h_acc / 1E3, 2), std::pow(raw_gps.h_acc / 1E3, 2), std::pow(raw_gps.v_acc / 1E3, 2);
fix->position_covariance_type = sensor_msgs::NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN;
}
// With mavlink v1.0 approximate accuracies by DOP
else if (!std::isnan(eph) && !std::isnan(epv)) {
gps_cov.diagonal() << std::pow(eph * gps_uere, 2), std::pow(eph * gps_uere, 2), std::pow(epv * gps_uere, 2);
fix->position_covariance_type = sensor_msgs::NavSatFix::COVARIANCE_TYPE_APPROXIMATED;
}
else {
fill_unknown_cov(fix);
}
// store & publish
m_uas->update_gps_fix_epts(fix, eph, epv, raw_gps.fix_type, raw_gps.satellites_visible);
raw_fix_pub.publish(fix);
if (raw_gps.vel != UINT16_MAX &&
raw_gps.cog != UINT16_MAX) {
double speed = raw_gps.vel / 1E2; // m/s
double course = angles::from_degrees(raw_gps.cog / 1E2); // rad
auto vel = boost::make_shared<geometry_msgs::TwistStamped>();
vel->header.stamp = fix->header.stamp;
vel->header.frame_id = frame_id;
// From nmea_navsat_driver
vel->twist.linear.x = speed * std::sin(course);
vel->twist.linear.y = speed * std::cos(course);
raw_vel_pub.publish(vel);
}
// publish satellite count
auto sat_cnt = boost::make_shared<std_msgs::UInt32>();
sat_cnt->data = raw_gps.satellites_visible;
raw_sat_pub.publish(sat_cnt);
}
// 涉及子函数
void UAS::update_gps_fix_epts(sensor_msgs::NavSatFix::Ptr &fix,
float eph, float epv,
int fix_type, int satellites_visible)
{
lock_guard lock(mutex);
gps_fix = fix;
gps_eph = eph;
gps_epv = epv;
gps_fix_type = fix_type;
gps_satellites_visible = satellites_visible;
}
mavlink 消息定义 https://mavlink.io/zh/messages/common.html#GPS_RAW_INT。
注意这个原始消息携带的信息很多,被拆分转发到了多个 ROS 话题中。
time_usec | uint64_t | us | Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. | ||
---|---|---|---|---|---|
fix_type | uint8_t | GPS_FIX_TYPE | GPS fix type. | ||
lat | int32_t | degE7 | Latitude (WGS84, EGM96 ellipsoid) | ||
lon | int32_t | degE7 | Longitude (WGS84, EGM96 ellipsoid) | ||
alt | int32_t | 毫米 | Altitude (MSL). Positive for up. Note that virtually all GPS modules provide the MSL altitude in addition to the WGS84 altitude. | ||
eph | uint16_t | 1E-2 | invalid:UINT16_MAX | GPS HDOP horizontal dilution of position (unitless * 100). If unknown, set to: UINT16_MAX | |
epv | uint16_t | 1E-2 | invalid:UINT16_MAX | GPS VDOP vertical dilution of position (unitless * 100). If unknown, set to: UINT16_MAX | |
vel | uint16_t | 厘米/秒 | invalid:UINT16_MAX | GPS ground speed. If unknown, set to: UINT16_MAX | |
cog | uint16_t | cdeg | invalid:UINT16_MAX | Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0…359.99 degrees. If unknown, set to: UINT16_MAX | |
satellites_visible | uint8_t | invalid:UINT8_MAX | 可见卫星数量。 If unknown, set to UINT8_MAX | ||
alt_ellipsoid ++ | int32_t | 毫米 | Altitude (above WGS84, EGM96 ellipsoid). Positive for up. | ||
h_acc ++ | uint32_t | 毫米 | Position uncertainty. | ||
v_acc ++ | uint32_t | 毫米 | Altitude uncertainty. | ||
vel_acc ++ | uint32_t | 毫米/秒 | Speed uncertainty. | ||
hdg_acc ++ | uint32_t | degE5 | Heading / track uncertainty | ||
yaw ++ | uint16_t | cdeg | invalid:0 | Yaw in earth frame from north. Use 0 if this GPS does not provide yaw. Use UINT16_MAX if this GPS is configured to provide yaw and is currently unable to provide it. Use 36000 for north. |
/mavros/global_position/raw/fix
,消息类型是 sensor_msgs/NavSatFix
,定义如下
rosmsg show sensor\_msgs/NavSatFix
uint8 COVARIANCE_TYPE_UNKNOWN=0
uint8 COVARIANCE_TYPE_APPROXIMATED=1
uint8 COVARIANCE_TYPE_DIAGONAL_KNOWN=2
uint8 COVARIANCE_TYPE_KNOWN=3
std_msgs/Header header
uint32 seq
time stamp
string frame_id
sensor_msgs/NavSatStatus status
int8 STATUS_NO_FIX=-1
int8 STATUS_FIX=0
int8 STATUS_SBAS_FIX=1
int8 STATUS_GBAS_FIX=2
uint16 SERVICE_GPS=1
uint16 SERVICE_GLONASS=2
uint16 SERVICE_COMPASS=4
uint16 SERVICE_GALILEO=8
int8 status
uint16 service
float64 latitude
float64 longitude
float64 altitude
float64[9] position_covariance
uint8 position_covariance_type
其中 float64 latitude
,float64 longitude
,float64 altitude
三个字段的赋值过程如下
fill_lla(raw_gps, fix);
template<typename MsgT>
inline void fill_lla(MsgT &msg, sensor_msgs::NavSatFix::Ptr fix)
{
fix->latitude = msg.lat / 1E7; // deg
fix->longitude = msg.lon / 1E7; // deg
fix->altitude = msg.alt / 1E3 + m_uas->geoid_to_ellipsoid_height(fix); // in meters
}
/**
* @brief Conversion from height above geoid (AMSL)
* to height above ellipsoid (WGS-84)
*/
template <class T>
inline double geoid_to_ellipsoid_height(T lla)
{
if (egm96_5)
return GeographicLib::Geoid::GEOIDTOELLIPSOID * (*egm96_5)(lla->latitude, lla->longitude);
else
return 0.0;
}
这里看到经纬度由原始的整型转换到了常用的 degree,且海拔由 mavros 进行了一次转换
- AMSL(Above Mean Sea Level):即 “海平面高度”,是 GPS 等设备通常输出的高度。
- Ellipsoid Height:是相对于地球椭球体(如 WGS-84 椭球)的高度,是 GNSS 内部用来计算的位置。
- 它们之间的差值由地球重力模型(如EGM96)提供,称为大地水准面起伏(geoid undulation)。
总之输出的 ROS 话题符合 WGS-84。
此外,原始 mavlink 消息包含了两个非常重要的信息,当前GPS锁定状态(QGC 中的 GPS Lock)和接收的卫星数(QGC 的 GPS Count)。他们对室外实物飞行有着重要意义,作为传感器与飞控融合算法状态是否良好的判断依据。
当前接收的卫星数量被转发到了另外的话题 /mavros/global_position/raw/satellites
。
sat_cnt->data = raw_gps.satellites_visible;
raw_sat_pub.publish(sat_cnt);
// 发布者定义如下
// raw_sat_pub = gp_nh.advertise<std_msgs::UInt32>("raw/satellites", 10);
当前GPS锁定状态,发布到了 /mavros/gpsstatus/gps1/raw
。这个话题侧重对 GPS_RAW_INT 类型 mavlink 消息进行直接转发,不做任何处理。因此,是 /mavros/global_position/raw/satellites
的超集,和 /mavros/global_position/raw/fix
相比,同样是转发原始的未 ekf 融合的 GPS 数据,/mavros/gpsstatus/gps1/raw
没有对高度进行转发。
/* -*- callbacks -*- */
/**
* @brief Publish <a href="https://mavlink.io/en/messages/common.html#GPS_RAW_INT">mavlink GPS_RAW_INT message</a> into the gps1/raw topic.
*/
void handle_gps_raw_int(const mavlink::mavlink_message_t *msg, mavlink::common::msg::GPS_RAW_INT &mav_msg) {
auto ros_msg = boost::make_shared<mavros_msgs::GPSRAW>();
ros_msg->header = m_uas->synchronized_header("/wgs84", mav_msg.time_usec);
ros_msg->fix_type = mav_msg.fix_type;
ros_msg->lat = mav_msg.lat;
ros_msg->lon = mav_msg.lon;
ros_msg->alt = mav_msg.alt;
ros_msg->eph = mav_msg.eph;
ros_msg->epv = mav_msg.epv;
ros_msg->vel = mav_msg.vel;
ros_msg->cog = mav_msg.cog;
ros_msg->satellites_visible = mav_msg.satellites_visible;
ros_msg->alt_ellipsoid = mav_msg.alt_ellipsoid;
ros_msg->h_acc = mav_msg.h_acc;
ros_msg->v_acc = mav_msg.v_acc;
ros_msg->vel_acc = mav_msg.vel_acc;
ros_msg->hdg_acc = mav_msg.hdg_acc;
ros_msg->dgps_numch = UINT8_MAX; // information not available in GPS_RAW_INT mavlink message
ros_msg->dgps_age = UINT32_MAX;// information not available in GPS_RAW_INT mavlink message
ros_msg->yaw = mav_msg.yaw;
gps1_raw_pub.publish(ros_msg);
}
/mavros/global_position/global
的发布在源文件 mavros/mavros/src/plugins/global_position.cpp
中
// mavros/mavros/src/plugins/global_position.cpp
/** @todo Handler for GLOBAL_POSITION_INT_COV */
void handle_global_position_int(const mavlink::mavlink_message_t *msg, mavlink::common::msg::GLOBAL_POSITION_INT &gpos)
{
auto odom = boost::make_shared<nav_msgs::Odometry>();
auto fix = boost::make_shared<sensor_msgs::NavSatFix>();
auto relative_alt = boost::make_shared<std_msgs::Float64>();
auto compass_heading = boost::make_shared<std_msgs::Float64>();
auto header = m_uas->synchronized_header(child_frame_id, gpos.time_boot_ms);
// Global position fix
fix->header = header;
fill_lla(gpos, fix);
// fill GPS status fields using GPS_RAW data
auto raw_fix = m_uas->get_gps_fix();
if (raw_fix) {
fix->status.service = raw_fix->status.service;
fix->status.status = raw_fix->status.status;
fix->position_covariance = raw_fix->position_covariance;
fix->position_covariance_type = raw_fix->position_covariance_type;
}
else {
// no GPS_RAW_INT -> fix status unknown
fix->status.service = sensor_msgs::NavSatStatus::SERVICE_GPS;
fix->status.status = sensor_msgs::NavSatStatus::STATUS_NO_FIX;
// we don't know covariance
fill_unknown_cov(fix);
}
relative_alt->data = gpos.relative_alt / 1E3; // in meters
compass_heading->data = (gpos.hdg != UINT16_MAX) ? gpos.hdg / 1E2 : NAN; // in degrees
/**
* @brief Global position odometry:
*
* X: spherical coordinate X-axis (meters)
* Y: spherical coordinate Y-axis (meters)
* Z: spherical coordinate Z-axis (meters)
* VX: latitude vel (m/s)
* VY: longitude vel (m/s)
* VZ: altitude vel (m/s)
* Angular rates: unknown
* Pose covariance: computed, with fixed diagonal
* Velocity covariance: unknown
*/
odom->header.stamp = header.stamp;
odom->header.frame_id = frame_id;
odom->child_frame_id = child_frame_id;
// Linear velocity
tf::vectorEigenToMsg(Eigen::Vector3d(gpos.vy, gpos.vx, gpos.vz) / 1E2,
odom->twist.twist.linear);
// Velocity covariance unknown
ftf::EigenMapCovariance6d vel_cov_out(odom->twist.covariance.data());
vel_cov_out.fill(0.0);
vel_cov_out(0) = -1.0;
// Current fix in ECEF
Eigen::Vector3d map_point;
try {
/**
* @brief Conversion from geodetic coordinates (LLA) to ECEF (Earth-Centered, Earth-Fixed)
*
* Note: "ecef_origin" is the origin of "map" frame, in ECEF, and the local coordinates are
* in spherical coordinates, with the orientation in ENU (just like what is applied
* on Gazebo)
*/
GeographicLib::Geocentric map(GeographicLib::Constants::WGS84_a(),
GeographicLib::Constants::WGS84_f());
/**
* @brief Checks if the "map" origin is set.
* - If not, and the home position is also not received, it sets the current fix as the origin;
* - If the home position is received, it sets the "map" origin;
* - If the "map" origin is set, then it applies the rotations to the offset between the origin
* and the current local geocentric coordinates.
*/
// Current fix to ECEF
map.Forward(fix->latitude, fix->longitude, fix->altitude,
map_point.x(), map_point.y(), map_point.z());
// Set the current fix as the "map" origin if it's not set
if (!is_map_init && fix->status.status >= sensor_msgs::NavSatStatus::STATUS_FIX) {
map_origin.x() = fix->latitude;
map_origin.y() = fix->longitude;
map_origin.z() = fix->altitude;
ecef_origin = map_point; // Local position is zero
is_map_init = true;
}
}
catch (const std::exception& e) {
ROS_INFO_STREAM("GP: Caught exception: " << e.what() << std::endl);
}
// Compute the local coordinates in ECEF
local_ecef = map_point - ecef_origin;
// Compute the local coordinates in ENU
tf::pointEigenToMsg(ftf::transform_frame_ecef_enu(local_ecef, map_origin), odom->pose.pose.position);
/**
* @brief By default, we are using the relative altitude instead of the geocentric
* altitude, which is relative to the WGS-84 ellipsoid
*/
if (use_relative_alt)
odom->pose.pose.position.z = relative_alt->data;
odom->pose.pose.orientation = m_uas->get_attitude_orientation_enu();
// Use ENU covariance to build XYZRPY covariance
ftf::EigenMapConstCovariance3d gps_cov(fix->position_covariance.data());
ftf::EigenMapCovariance6d pos_cov_out(odom->pose.covariance.data());
pos_cov_out.setZero();
pos_cov_out.block<3, 3>(0, 0) = gps_cov;
pos_cov_out.block<3, 3>(3, 3).diagonal() <<
rot_cov,
rot_cov,
rot_cov;
// publish
gp_fix_pub.publish(fix);
gp_odom_pub.publish(odom);
gp_rel_alt_pub.publish(relative_alt);
gp_hdg_pub.publish(compass_heading);
// TF
if (tf_send) {
geometry_msgs::TransformStamped transform;
transform.header.stamp = odom->header.stamp;
transform.header.frame_id = tf_frame_id;
transform.child_frame_id = tf_child_frame_id;
// setRotation()
transform.transform.rotation = odom->pose.pose.orientation;
// setOrigin()
transform.transform.translation.x = odom->pose.pose.position.x;
transform.transform.translation.y = odom->pose.pose.position.y;
transform.transform.translation.z = odom->pose.pose.position.z;
m_uas->tf2_broadcaster.sendTransform(transform);
}
}
同样是通过 fill_lla 赋值,发布过程和 /mavros/global_position/raw/fix
类似,对海拔做了转换。
fill_lla(gpos, fix);
template<typename MsgT>
inline void fill_lla(MsgT &msg, sensor_msgs::NavSatFix::Ptr fix)
{
fix->latitude = msg.lat / 1E7; // deg
fix->longitude = msg.lon / 1E7; // deg
fix->altitude = msg.alt / 1E3 + m_uas->geoid_to_ellipsoid_height(fix); // in meters
}
总结一下:
- 卫星数和GPS锁定状态可以通过
/mavros/gpsstatus/gps1/raw
获取; - 未经PX4融合估计的原始的经纬度海拔通过
/mavros/global_position/raw/fix
(WGS-84)获取; - EKF 融合后的经纬度海拔通过
/mavros/global_position/global
获取(这个话题的频率实验会比仿真低很多。极端情况,如室内无卫星/GPS传感器异常或者数据跳变导致飞控拒绝融合时可能不会发布此消息)。