华测导航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 = 3

uint8 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 = 8

uint16 service

1.3 gps_common::GPSFix

官方介绍: gps_common::GPSFix

# A more complete GPS fix to supplement sensor_msgs/NavSatFix.
# 更加完整的GPS状态，以补充 sensor_msgs/NavSatFix.
Header header

GPSStatus 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_covariance

uint8 COVARIANCE_TYPE_UNKNOWN = 0
uint8 COVARIANCE_TYPE_APPROXIMATED = 1
uint8 COVARIANCE_TYPE_DIAGONAL_KNOWN = 2
uint8 COVARIANCE_TYPE_KNOWN = 3

uint8 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);