先前在树莓派上用C/C++读取过GPS北斗双模定位模块，但因为定位模块的若干条定位数据无法立刻读取，需要用delay()延迟1到2秒的时间才能把所有定位数据都读取进程序，又不想写多线程，因此当时的东西做得很鬼畜，读一次定位数据就要卡一下。而如今定位模块要用在实时性要求较高的东西上，自然就一定要从了多线程了。不过偶然得知ROS读取GPS自带多线程效果，再加上这次的平台上已经集成了若干使用ROS发布、订阅数据的传感器，就决定试一下用ROS读取定位数据。
为什么之前不用ROS读取定位模块的数据呢……因为定位模块不像其他传感器自带ROS包，使用的是公共的ROS包；网上这方面的资料除了Adding a GPS sensor to the Robot Pose EKF Filter以及无数这篇文章的机翻转载文之外，ROS读取GPS的中英文教程少之又少，北斗的干脆没有。
在这少之又少之中主要是两种方法：ROS包gps_common法以及ROS包nmea_navsat_driver法。前者推荐从Github上下载源码，下载下来后要分别用rosmake进行编译，而且似乎有点不全，博主不甚理解，因此在此篇中主要介绍ROS包nmea_navsat_driver读取GPS、北斗定位信息。
运行机器是Jetson TX2（其他x86的电脑或者arm板都可以），环境是Ubuntu16.04，ROS版本是ROS Kinetic，使用的定位模块是淘宝上买的下面这货：ATGM332D，型号是5N3X，是一款自带GPS和北斗定位的双模定位模块。
如图可见，该模块通过TTL转USB芯片与处理板相连，另一端是天线部分。ATGM332D的资料可见传感器——ATGM332D 北斗定位模块这位大佬的介绍。
硬件方面连接好后，把天线扔出室外，再输入
cutecom打开串口工具（没有的话apt-get install安装一下），以9600的码率读取ttyUSB1（这个根据接入定位模块后/dev/里新增的ttyUSB名为准）的数据，确认能读取到传感器——ATGM332D 北斗定位模块中所说的定位数据。定位数据的含义可见NMEA 0183协议解析，该模块中并未单独发布北斗的定位数据，但有GPS、北斗的联合定位数据（GN开头），其中GPS、北斗联合最简定位数据GNRMC是最推荐、最需要获取的数据。
要把GNRMC数据读取到C/C++程序中，软件方面需要做如下几步：
1、安装nmea-navsat-driver包，确认在/opt/ros/kinetic/include/目录下有nmea_msgs文件夹：
sudo apt-get install ros-kinetic-nmea-navsat-driver libgps-dev2、打开新Terminal窗口打开ROS系统（相关ROS环境创建、ROS包编译等基础知识务必先在网上查清楚），输入
roscore再打开一个新Terminal窗口发布（Publish）定位模块的信息，输入
rosrun nmea_navsat_driver nmea_topic_serial_reader _port:=/dev/ttyUSB1 _baud:=9600发布定位模块信息时，窗口中不会像cutecom中那样不停显示最新的定位数据，会提示SyntaxWarning: The publisher should be created with an explicit keyword argument 'queue_size'.，忽略即可。该ROS包把数据通过/nmea_sentence这个Topic发布出来，为了验证定位数据发布成功，我们再打开一个新Terminal窗口读取/nmea_sentence中的信息，输入
rostopic echo /nmea_sentence可见其下逐条输出了定位模块中的数据，数据部分主要在sentence中。这个窗口可以用Ctrl+C的方式终止，前两个需要保持常开。该部分网上相关博客错误较多。
3、博主参考了std_msgs/String.h中的收发方式，在已创建好的ROS发布、收听消息的C/C++程序包上进行修改：
首先在CMakeLists的find_package中增加nmea_msgs，其次在/src下的源代码的各部分中分别增加
#include <nmea_msgs/Sentence.h>
ros::NodeHandle nGPS_;
ros::Subscriber gps_sub;
gps_sub = nGPS_.subscribe("nmea_sentence", 1, &ImageConverter::GPSReader,this);
void GPSReader(const nmea_msgs::Sentence::ConstPtr& msg)
{
ROS_INFO("I heard: [%s]", msg->sentence.c_str());
}
整体代码如下所示：
#ifndef _ROSTEST_HPP_
#define _ROSTEST_HPP_
#include <ros/ros.h>
#include <std_msgs/String.h>
#include <nmea_msgs/Sentence.h>
#include <iostream>
#include <math.h>
#include <string.h>
#include <sstream>
#define RAD2DEG(x) ((x)*180./M_PI)
class ImageConverter
{
ros::NodeHandle nh_;
ros::NodeHandle nListener_;
ros::Subscriber listener_sub;
ros::NodeHandle nGPS_;
ros::Subscriber gps_sub;
public:
ImageConverter()
: it_(nh_)
{
Lidar_stop = 0;
car_here = 0;
car_avoidance = 0;
RightTurn_Mark = 0;
RightTurn = 0;
listener_sub = nListener_.subscribe("chatter", 1, &ImageConverter::chatterCallback,this);
gps_sub = nGPS_.subscribe("nmea_sentence", 1, &ImageConverter::GPSReader,this);//1 can be changed for larger
}
~ImageConverter()
{
//cv::destroyWindow(OPENCV_WINDOW);
}
void chatterCallback(const std_msgs::String::ConstPtr& msg)
{
string CA = "CarArrive";
string CL = "CarLost";
ROS_INFO("I heard: [%s]", msg->data.c_str());
//std::cout << msg->data.c_str() << std::endl;
if(CA.compare(msg->data.c_str()) == 0)
{
car_here = 1;
std::cout << "Car here" << std::endl;
}
if(CL.compare(msg->data.c_str()) == 0)
{
car_here = 0;
std::cout << "Car away" << std::endl;
}
}
void GPSReader(const nmea_msgs::Sentence::ConstPtr& msg)
{
ROS_INFO("I heard: [%s]", msg->sentence.c_str());
}
private:
int Lidar_stop;
int car_here;
int car_avoidance;
int RightTurn_Mark;
int RightTurn;
};
int main(int argc, char** argv)
{
ros::init(argc, argv, "image_converter");
ImageConverter ic;
ros::spin();
return 0;
}
#endif
其中msg->sentence.c_str()即存储着定位信息，上述代码中暂时以ROS_INFO的形式输出。后续可通过在GPSReader()函数中增加
sscanf(ptr,"$GNRMC,%d.000,%c,%f,N,%f,E,%f,%f,%d,,,%c*",&(gps_data->time),&(gps_data->pos_state),&(gps_data->latitude),&(gps_data->longitude),&(gps_data->speed),&(gps_data->direction),&(gps_data->date),&(gps_data->mode));的方式将字符串形式的信息读取，并把GNRMC数据单独筛选出。需要注意的是GPS读取数据的频率较慢（一般10Hz左右，可通过rostopic hz /nmea_sentence查看），需要控制调用频率，并对历史数据进行存储，结合IMU数据实现实时性高的室外定位功能。博主的运行结果如下图所示：
如有建议意见，望多多指教！
