华测导航GPCHC协议ROS驱动包，CGI610、410接收机，NavSatStatus、GPSFix和普通格式

---

Ubuntu 20.04 + noetic
华测CGI 610——RS232-C——GPCHC

---

一、消息类型

1.1 sensor_msgs/NavSatFix

官方介绍：sensor_msgs/NavSatFix Message

# Navigation Satellite fix for any Global Navigation Satellite System
#
# Specified using the WGS 84 reference ellipsoid
# 用于WGS84椭球坐标系# header.stamp specifies the ROS time for this measurement (the
#        corresponding satellite time may be reported using the
#        sensor_msgs/TimeReference message).
# header.stamp 使用ROS时间，卫星时间用sensor_msgs/TimeReference# header.frame_id is the frame of reference reported by the satellite
#        receiver, usually the location of the antenna.  This is a
#        Euclidean frame relative to the vehicle, not a reference
#        ellipsoid.
# header.frame_id 是卫星接收器报告的坐标系，通常是GPS天线的位置。
# 这是相对于车辆(中心)的欧几里得坐标变换，而不是参考椭球坐标系。
Header header# satellite fix status information
# 卫星定位状态信息
NavSatStatus status# Latitude [degrees]. Positive is north of equator; negative is south.
# 纬度，单位是 度
float64 latitude# Longitude [degrees]. Positive is east of prime meridian; negative is west.
# 经度，单位是 度
float64 longitude# Altitude [m]. Positive is above the WGS 84 ellipsoid
# WSG 84椭球体下的高度，单位m
# (quiet NaN if no altitude is available).
float64 altitude# Position covariance [m^2] defined relative to a tangential plane
# through the reported position. The components are East, North, and
# Up (ENU), in row-major order.
#位置协方差[m ^ 2]: 相对于切线平面的位置协方差。 组件是East，North和Up（ENU），按行优先顺序排列。
# Beware: this coordinate system exhibits singularities at the poles.
#注意：此坐标系在极点处表现出奇异性。float64[9] position_covariance# If the covariance of the fix is known, fill it in completely. If the
# GPS receiver provides the variance of each measurement, put them
# along the diagonal. If only Dilution of Precision is available,
# estimate an approximate covariance from that.
# 3 - 如果已知修正的协方差，请完全填写。
# 2 - 如果GPS接收器提供了每次测量的方差，请将其沿对角线放置。
# 1 - 如果只有DOP精度衰减因子可用，请据此估计近似协方差。
# 0 - 协方差未知uint8 COVARIANCE_TYPE_UNKNOWN = 0
uint8 COVARIANCE_TYPE_APPROXIMATED = 1
uint8 COVARIANCE_TYPE_DIAGONAL_KNOWN = 2
uint8 COVARIANCE_TYPE_KNOWN = 3uint8 position_covariance_type

1.2 sensor_msgs/NavSatStatus

是sensor_msgs/NavSatFix的子信息。
官方介绍：sensor_msgs/NavSatStatus Message
星基增强和地基增强的介绍: link

# Navigation Satellite fix status for any Global Navigation Satellite System
# 任何GNSS的导航卫星求解固定状态# Whether to output an augmented fix is determined by both the fix
# type and the last time differential corrections were received.  A
# fix is valid when status >= STATUS_FIX.
#是否输出增强解取决于求解固定状态和上一次收到差分校正的时间int8 STATUS_NO_FIX =  -1        # unable to fix position 不固定
int8 STATUS_FIX =      0        # unaugmented fix		未增强
int8 STATUS_SBAS_FIX = 1        # with satellite-based augmentation  星基增强
int8 STATUS_GBAS_FIX = 2        # with ground-based augmentation   地基增强int8 status# Bits defining which Global Navigation Satellite System signals were
# used by the receiver.
# 定义接收机使用的GNSS卫星信号uint16 SERVICE_GPS =     1
uint16 SERVICE_GLONASS = 2
uint16 SERVICE_COMPASS = 4      # includes BeiDou.
uint16 SERVICE_GALILEO = 8uint16 service

1.3 gps_common::GPSFix

官方介绍: gps_common::GPSFix

# A more complete GPS fix to supplement sensor_msgs/NavSatFix.
# 更加完整的GPS状态，以补充 sensor_msgs/NavSatFix.
Header headerGPSStatus status# Latitude (degrees). Positive is north of equator; negative is south.
float64 latitude# Longitude (degrees). Positive is east of prime meridian, negative west.
float64 longitude# Altitude (meters). Positive is above reference (e.g., sea level).
float64 altitude# Direction (degrees from north)
# 单位是 度，从北旋转的方向
float64 track# Ground speed (meters/second)
# 地面速度 m/s
float64 speed# Vertical speed (meters/second)
# 垂直速度 m/s
float64 climb# Device orientation (units in degrees)
float64 pitch
float64 roll
float64 dip# GPS time
float64 time## Dilution of precision; Xdop<=0 means the value is unknown# Total (positional-temporal) dilution of precision
float64 gdop# Positional (3D) dilution of precision
float64 pdop# Horizontal dilution of precision
float64 hdop# Vertical dilution of precision
float64 vdop# Temporal dilution of precision
float64 tdop## Uncertainty of measurement, 95% confidence# Spherical position uncertainty (meters) [epe]
float64 err# Horizontal position uncertainty (meters) [eph]
float64 err_horz# Vertical position uncertainty (meters) [epv]
float64 err_vert# Track uncertainty (degrees) [epd]
float64 err_track# Ground speed uncertainty (meters/second) [eps]
float64 err_speed# Vertical speed uncertainty (meters/second) [epc]
float64 err_climb# Temporal uncertainty [ept]
float64 err_time# Orientation uncertainty (degrees)
float64 err_pitch
float64 err_roll
float64 err_dip# Position covariance [m^2] defined relative to a tangential plane
# through the reported position. The components are East, North, and
# Up (ENU), in row-major order.float64[9] position_covarianceuint8 COVARIANCE_TYPE_UNKNOWN = 0
uint8 COVARIANCE_TYPE_APPROXIMATED = 1
uint8 COVARIANCE_TYPE_DIAGONAL_KNOWN = 2
uint8 COVARIANCE_TYPE_KNOWN = 3uint8 position_covariance_type

1.4 sensor_msgs::Imu

官方介绍: sensor_msgs::Imu

std_msgs/Header header
geometry_msgs/Quaternion orientation
float64[9] orientation_covariance
geometry_msgs/Vector3 angular_velocity
float64[9] angular_velocity_covariance
geometry_msgs/Vector3 linear_acceleration
float64[9] linear_acceleration_covariance

二、部分源码

2.1 相关的依赖和库

Eigen——计算四元数
Protobuf——结构数据序列化
autoware——信息格式

2.2 文件结构

ROS工作空间的include包括两个头文件，src下有两个cpp文件，由launch文件启动。

2.3 字段分割函数

把GPCHC数据流分割成一个一个的字段，然后进行相应的定义

void supersplit(const std::string& s, std::vector<std::string>& v, const std::string& c)
{std::string::size_type pos1, pos2;size_t len = s.length();pos2 = s.find(c);pos1 = 0;while(std::string::npos != pos2){if("" == s.substr(pos1, pos2-pos1)){v.emplace_back("0");}else{v.emplace_back(s.substr(pos1, pos2-pos1));}pos1 = pos2 + c.size();pos2 = s.find(c, pos1);}if(pos1 != len)v.emplace_back(s.substr(pos1));
}

typedef enum GNSS_GPCHC_INDEX{GNSS_GPCHC_INDEX_HEADER = 0,GNSS_GPCHC_INDEX_GPSWEEK,GNSS_GPCHC_INDEX_GPSTime,GNSS_GPCHC_INDEX_HEADING,GNSS_GPCHC_INDEX_PICH,GNSS_GPCHC_INDEX_ROLL,GNSS_GPCHC_INDEX_GRRO_X,GNSS_GPCHC_INDEX_GRRO_Y,GNSS_GPCHC_INDEX_GRRO_Z,GNSS_GPCHC_INDEX_ACC_X,GNSS_GPCHC_INDEX_ACC_Y,GNSS_GPCHC_INDEX_ACC_Z,GNSS_GPCHC_INDEX_LAT,GNSS_GPCHC_INDEX_LON,GNSS_GPCHC_INDEX_ALT,GNSS_GPCHC_INDEX_VE,GNSS_GPCHC_INDEX_VN,GNSS_GPCHC_INDEX_VU,GNSS_GPCHC_INDEX_SPEED,GNSS_GPCHC_INDEX_NSV1,GNSS_GPCHC_INDEX_NSV2,GNSS_GPCHC_INDEX_STATUS,GNSS_GPCHC_INDEX_AGE,GNSS_GPCHC_INDEX_WARMING,GNSS_GPCHC_INDEX_MAX
}GNSS_GPCHC_INDEX;

2.4 定义消息话题

定义五种格式：

1. sensor_msgs::NavSatFix message_NavSatFix;
2. gps_common::GPSFix message_gpsfix;
3. sensor_msgs::Imu message_imu;
4. geometry_msgs::PoseStamped angle_qua; //欧拉角和四元数
5. geometry_msgs::PoseStamped lla_qua; //经纬高和四元数

发布ROS话题

	imu_raw_pub = nh_.advertise<sensor_msgs::Imu>("/imu_raw", 500);nav_sat_fix_pub = nh_.advertise<sensor_msgs::NavSatFix>("/nav_sat_fix", 500);gps_fix_pub = nh_.advertise<gps_common::GPSFix>("/gps_fix", 500);angle_qua_pub = nh_.advertise<geometry_msgs::PoseStamped>("/angle_qua", 500);lla_qua_pub = nh_.advertise<geometry_msgs::PoseStamped>("/lla_qua", 500);