|
@@ -9,6 +9,7 @@
|
|
|
#include "coordsconvertion.h"
|
|
|
#include <localization/localMsg.h>
|
|
|
#include "../common/message.h"
|
|
|
+#include <iostream>
|
|
|
|
|
|
|
|
|
|
|
@@ -20,7 +21,7 @@ std::string Localization::Name() const { return "localization"; }
|
|
|
int Localization::Init() {
|
|
|
try
|
|
|
{
|
|
|
- LOG(INFO)<<Name()<<" init start ...";
|
|
|
+ LOG(INFO)<<Name()<<" init start ...";
|
|
|
serial_port = "/dev/ttyUSB0";
|
|
|
baudrate = 115200;
|
|
|
timeout = 1000;
|
|
@@ -49,7 +50,7 @@ int Localization::Start() {
|
|
|
if (IsOpen())
|
|
|
{
|
|
|
//指定循环的频率
|
|
|
- ros::Rate loop_rate(50);
|
|
|
+ ros::Rate loop_rate(2000);
|
|
|
while(ros::ok())
|
|
|
{
|
|
|
|
|
@@ -104,12 +105,14 @@ int Localization::Analyse(const std::string &strSerialResult, NMEA0183 &nmea){
|
|
|
LOG(INFO)<<Name()<<" Analyse start.";
|
|
|
try
|
|
|
{
|
|
|
- std::string delim("$");
|
|
|
+ std::string delim("\r\n");
|
|
|
std::vector<std::string> v_nmeaData = Split(strSerialResult, delim);
|
|
|
+ if (v_nmeaData.size() !=3)
|
|
|
+ return ROBOT_FAILTURE;
|
|
|
std::vector<std::string>::iterator iter_start = v_nmeaData.begin();
|
|
|
std::vector<std::string>::iterator iter_end = v_nmeaData.end();
|
|
|
std::vector<std::string>::iterator iter;
|
|
|
- for ( iter = iter_start; iter != iter_end; iter++)
|
|
|
+ for (iter =iter_start; iter != iter_end; iter++)
|
|
|
{
|
|
|
std::string type = (*iter).substr(1, 5);
|
|
|
LOG(INFO)<<Name()<<" type:"<<type;
|
|
@@ -165,11 +168,6 @@ int Localization::ParaGPRMC(std::string &strGPRMC, GPRMC &gprmc){
|
|
|
gprmc.Deviated = atof(nmea_gprmc[8].c_str());
|
|
|
memset(&gprmc.Date, 0x00, 5);
|
|
|
memcpy(&gprmc.Date, nmea_gprmc[9].c_str(), 4);
|
|
|
- gprmc.Magnetic = atof(nmea_gprmc[10].c_str());
|
|
|
- memset(&gprmc.MagneticDirection, 0x00, 2);
|
|
|
- memcpy(&gprmc.MagneticDirection, nmea_gprmc[11].c_str(), 1);
|
|
|
- memset(&gprmc.Mode, 0x00, 2);
|
|
|
- memcpy(&gprmc.Mode, nmea_gprmc[12].c_str(), 1);
|
|
|
}
|
|
|
catch (ros::Exception ex)
|
|
|
{
|
|
@@ -197,11 +195,8 @@ int Localization::ParaGPGGA(std::string &strGPGGA, GPGGA &gpgga){
|
|
|
gpgga.SatelliteNumber = atoi(nmea_gpgga[7].c_str());
|
|
|
gpgga.Hdop = atof(nmea_gpgga[8].c_str());
|
|
|
gpgga.Height = atof(nmea_gpgga[9].c_str());
|
|
|
- gpgga.ReleaveHeight = atoi(nmea_gpgga[10].c_str());
|
|
|
- gpgga.CTKTime = atof(nmea_gpgga[11].c_str());
|
|
|
- gpgga.CTKID = atoi(nmea_gpgga[12].c_str());
|
|
|
- memset(&gpgga.Check, 0x00, 5);
|
|
|
- memcpy(&gpgga.Check, nmea_gpgga[13].c_str(), 4);
|
|
|
+ memset(&gpgga.HeightUint, 0x00, 2);
|
|
|
+ memcpy(&gpgga.HeightUint, nmea_gpgga[10].c_str(), 1);
|
|
|
}
|
|
|
catch(ros::Exception ex)
|
|
|
{
|