当前位置： 首页 > news >正文

汇邦团建网站谁做的成都优化推广公司

news 2025/11/2 16:49:37
汇邦团建网站谁做的,成都优化推广公司,建设网站收费明细,网站开发的方法有哪些使用 ROS1-Noetic 和 mavros v1.20.1， 携带经纬度海拔的话题主要有三个： /mavros/global_position/raw/fix/mavros/gpsstatus/gps1/raw/mavros/global_position/global 查看 mavros 源码，来分析他们的发布过程。发现前两个话题都对应了同一…

使用 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.cppvoid 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 sensorif (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 DOPelse 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 & publishm_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/sdouble course = angles::from_degrees(raw_gps.cog / 1E2);	// radauto vel = boost::make_shared<geometry_msgs::TwistStamped>();vel->header.stamp = fix->header.stamp;vel->header.frame_id = frame_id;// From nmea_navsat_drivervel->twist.linear.x = speed * std::sin(course);vel->twist.linear.y = speed * std::cos(course);raw_vel_pub.publish(vel);}// publish satellite countauto 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_usecuint64_tusTimestamp (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_typeuint8_tGPS_FIX_TYPEGPS fix type.
latint32_tdegE7Latitude (WGS84, EGM96 ellipsoid)
lonint32_tdegE7Longitude (WGS84, EGM96 ellipsoid)
altint32_t毫米Altitude (MSL). Positive for up. Note that virtually all GPS modules provide the MSL altitude in addition to the WGS84 altitude.
ephuint16_t1E-2invalid:UINT16_MAXGPS HDOP horizontal dilution of position (unitless * 100). If unknown, set to: UINT16_MAX
epvuint16_t1E-2invalid:UINT16_MAXGPS VDOP vertical dilution of position (unitless * 100). If unknown, set to: UINT16_MAX
veluint16_t厘米/秒invalid:UINT16_MAXGPS ground speed. If unknown, set to: UINT16_MAX
coguint16_tcdeginvalid:UINT16_MAXCourse over ground (NOT heading, but direction of movement) in degrees * 100, 0.0…359.99 degrees. If unknown, set to: UINT16_MAX
satellites_visibleuint8_tinvalid: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_tdegE5Heading / track uncertainty
yaw ++uint16_tcdeginvalid:0Yaw 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/NavSatFixuint8 COVARIANCE_TYPE_UNKNOWN=0
uint8 COVARIANCE_TYPE_APPROXIMATED=1
uint8 COVARIANCE_TYPE_DIAGONAL_KNOWN=2
uint8 COVARIANCE_TYPE_KNOWN=3
std_msgs/Header headeruint32 seqtime stampstring frame_id
sensor_msgs/NavSatStatus statusint8 STATUS_NO_FIX=-1int8 STATUS_FIX=0int8 STATUS_SBAS_FIX=1int8 STATUS_GBAS_FIX=2uint16 SERVICE_GPS=1uint16 SERVICE_GLONASS=2uint16 SERVICE_COMPASS=4uint16 SERVICE_GALILEO=8int8 statusuint16 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;		// degfix->longitude = msg.lon / 1E7;		// degfix->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);elsereturn 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 messageros_msg->dgps_age          = UINT32_MAX;// information not available in GPS_RAW_INT mavlink messageros_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 fixfix->header = header;fill_lla(gpos, fix);// fill GPS status fields using GPS_RAW dataauto 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 unknownfix->status.service = sensor_msgs::NavSatStatus::SERVICE_GPS;fix->status.status = sensor_msgs::NavSatStatus::STATUS_NO_FIX;// we don't know covariancefill_unknown_cov(fix);}relative_alt->data = gpos.relative_alt / 1E3;	// in meterscompass_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 velocitytf::vectorEigenToMsg(Eigen::Vector3d(gpos.vy, gpos.vx, gpos.vz) / 1E2,odom->twist.twist.linear);// Velocity covariance unknownftf::EigenMapCovariance6d vel_cov_out(odom->twist.covariance.data());vel_cov_out.fill(0.0);vel_cov_out(0) = -1.0;// Current fix in ECEFEigen::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 ECEFmap.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 setif (!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 zerois_map_init = true;}}catch (const std::exception& e) {ROS_INFO_STREAM("GP: Caught exception: " << e.what() << std::endl);}// Compute the local coordinates in ECEFlocal_ecef = map_point - ecef_origin;// Compute the local coordinates in ENUtf::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 covarianceftf::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;// publishgp_fix_pub.publish(fix);gp_odom_pub.publish(odom);gp_rel_alt_pub.publish(relative_alt);gp_hdg_pub.publish(compass_heading);// TFif (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;		// degfix->longitude = msg.lon / 1E7;		// degfix->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传感器异常或者数据跳变导致飞控拒绝融合时可能不会发布此消息）。
http://www.dtcms.com/a/558985.html

相关文章：

  • 平谷网站建设域名怎么解析到网站
  • 品牌平价网站建设如何在vps上搭建网站
  • 南阳网站运营招聘信息网站常用的优化方法
  • 苏州seo全网营销windows优化大师下载安装
  • 如何查询网站的服务器网站建设公司哪好
  • 工程建设科学技术奖申报网站建设物流
  • 企业招聘网站排行榜gta5房地产网站建设中
  • 上海做机床的公司网站制作网站公
  • 有什么网站做生鲜配送的成都房产信息查询官方网站
  • linux实现设备驱动-字符型设备驱动和杂项设备驱动
  • 大英做网站wordpress主题选项单选框
  • 分类信息网站 建议 建设百度搜索推广采取
  • 招标网站建设招标方案建设我们的网站 教案
  • 安徽海川建设公司网站企业所得税计算公式2022
  • apache 配置网站做网站的哪里好
  • 广州wap网站建设广州在线网站制作公司
  • 做网站的策划方案汽车最好网站建设
  • Spring AI——入门介绍
  • 奢侈品网站怎么做tuig优化wordpress虚拟3d网站
  • 手机网站怎么做淘宝客个人网页制作的流程和步骤
  • 甘肃城乡建设局安全质量网站上海雍熙网站建设
  • 网站建设地域名app是什么意思
  • 云南品牌网站开发网站建设合同的注意点
  • 企业网站建设公司电话西安张家港做网站优化排名
  • 临高网站建设网络平台推广宣传方案
  • php mysql网站开发项目式教程怎么免费建设个人博客网站
  • 深圳移动网站建设公司价格贷款公司通过做网站来给予平台贷款
  • 数据结构算法学习day3——二分查找
  • 专业电商网站海口网站开发师招聘
  • 宁波网站建设有限公司导购网站开发