|
@@ -20,7 +20,6 @@ std::string Localization::Name() const { return "localization"; }
|
|
|
int Localization::Init() {
|
|
|
try
|
|
|
{
|
|
|
-
|
|
|
LOG(INFO)<<Name()<<" init start ...";
|
|
|
serial_port = "/dev/ttyUSB0";
|
|
|
baudrate = 115200;
|
|
@@ -40,22 +39,39 @@ int Localization::Start() {
|
|
|
LOG(INFO)<<Name()<<" start start...";
|
|
|
try
|
|
|
{
|
|
|
+ geometry_pub = node_handle_->advertise<localization::localMsg>(robot::common::Geometry, 200);
|
|
|
//设置串口属性,并打开串口
|
|
|
serial_gnss.setPort(serial_port);
|
|
|
serial_gnss.setBaudrate(baudrate);
|
|
|
serial::Timeout to = serial::Timeout::simpleTimeout(timeout);
|
|
|
serial_gnss.setTimeout(to);
|
|
|
serial_gnss.open();
|
|
|
+ if (IsOpen())
|
|
|
+ {
|
|
|
+ //指定循环的频率
|
|
|
+ ros::Rate loop_rate(50);
|
|
|
+ while(ros::ok())
|
|
|
+ {
|
|
|
+
|
|
|
+ if(serial_gnss.available() >0){
|
|
|
+ LOG(INFO)<<Name()<<" Reading from serial port. data size :"<<serial_gnss.available();
|
|
|
+ std::string strSerialResult;
|
|
|
+ strSerialResult= serial_gnss.read(serial_gnss.available());
|
|
|
+ LOG(INFO)<<Name()<<" Read: " << strSerialResult;
|
|
|
+ NMEA0183 nmea;
|
|
|
+ if (Analyse(strSerialResult, nmea) == ROBOT_SUCCESS)
|
|
|
+ PublishData(nmea);
|
|
|
+ }
|
|
|
+ ros::spinOnce();
|
|
|
+ loop_rate.sleep();
|
|
|
+ }
|
|
|
+ }
|
|
|
}
|
|
|
catch (serial::IOException& e)
|
|
|
{
|
|
|
LOG(ERROR)<<Name()<< " Unable to open port " << e.what();
|
|
|
return ROBOT_FAILTURE;
|
|
|
}
|
|
|
- if (IsOpen())
|
|
|
- {
|
|
|
- GetSerialData();
|
|
|
- }
|
|
|
LOG(INFO)<<Name()<<" start end.";
|
|
|
return ROBOT_SUCCESS;
|
|
|
}
|
|
@@ -75,36 +91,6 @@ bool Localization::IsOpen()
|
|
|
}
|
|
|
}
|
|
|
|
|
|
-int Localization::GetSerialData()
|
|
|
-{
|
|
|
- try
|
|
|
- {
|
|
|
- //指定循环的频率
|
|
|
- ros::Rate loop_rate(50);
|
|
|
- while(ros::ok())
|
|
|
- {
|
|
|
-
|
|
|
- if(serial_gnss.available() >0){
|
|
|
- LOG(INFO)<<Name()<<"Reading from serial port\n";
|
|
|
- std::string strSerialResult;
|
|
|
- strSerialResult= serial_gnss.read(serial_gnss.available());
|
|
|
- LOG(INFO)<<Name()<<"Read: " << strSerialResult;
|
|
|
- NMEA0183 nmea;
|
|
|
- Analyse(strSerialResult, nmea);
|
|
|
- PublishData(nmea);
|
|
|
- }
|
|
|
- //处理ROS的信息,比如订阅消息,并调用回调函数
|
|
|
- ros::spinOnce();
|
|
|
- loop_rate.sleep();
|
|
|
- }
|
|
|
- }
|
|
|
- catch(ros::Exception ex)
|
|
|
- {
|
|
|
- LOG(ERROR)<<Name()<< " get serial data error" << ex.what();
|
|
|
- return ROBOT_FAILTURE;
|
|
|
- }
|
|
|
- return ROBOT_SUCCESS;
|
|
|
-}
|
|
|
|
|
|
void Localization::Stop() {
|
|
|
LOG(INFO)<<Name()<<" Stop start...";
|
|
@@ -115,7 +101,7 @@ void Localization::Stop() {
|
|
|
|
|
|
int Localization::Analyse(const std::string &strSerialResult, NMEA0183 &nmea){
|
|
|
|
|
|
- LOG(INFO)<<Name()<<"Analyse start\n";
|
|
|
+ LOG(INFO)<<Name()<<" Analyse start.";
|
|
|
try
|
|
|
{
|
|
|
std::string delim("$");
|
|
@@ -125,21 +111,26 @@ int Localization::Analyse(const std::string &strSerialResult, NMEA0183 &nmea){
|
|
|
std::vector<std::string>::iterator iter;
|
|
|
for ( iter = iter_start; iter != iter_end; iter++)
|
|
|
{
|
|
|
- std::string type = (*iter).substr(0, 4);
|
|
|
+ std::string type = (*iter).substr(1, 5);
|
|
|
+ LOG(INFO)<<Name()<<" type:"<<type;
|
|
|
if (type.compare("GPRMC") == 0)
|
|
|
{
|
|
|
GPRMC temGPRMC;
|
|
|
- ParaGPRMC(*iter, temGPRMC);
|
|
|
+ if (ParaGPRMC(*iter, temGPRMC) != ROBOT_SUCCESS)
|
|
|
+ return ROBOT_FAILTURE;
|
|
|
nmea.gprmc = temGPRMC;
|
|
|
- LOG(INFO)<<Name()<<"para GPRMC end\n";
|
|
|
+ nmea.gprmc.print();
|
|
|
+ LOG(INFO)<<Name()<<"para GPRMC end.";
|
|
|
}
|
|
|
|
|
|
else if (type.compare("GPGGA") == 0)
|
|
|
{
|
|
|
GPGGA tempGPGGA;
|
|
|
- ParaGPGGA(*iter, tempGPGGA);
|
|
|
+ if (ParaGPGGA(*iter, tempGPGGA) != ROBOT_SUCCESS)
|
|
|
+ return ROBOT_FAILTURE;
|
|
|
nmea.gpgga = tempGPGGA;
|
|
|
- LOG(INFO)<<Name()<<"para GPGGA end\n";
|
|
|
+ nmea.gpgga.print();
|
|
|
+ LOG(INFO)<<Name()<<" para GPGGA end.";
|
|
|
}
|
|
|
}
|
|
|
|
|
@@ -149,59 +140,79 @@ int Localization::Analyse(const std::string &strSerialResult, NMEA0183 &nmea){
|
|
|
LOG(ERROR)<<Name()<< " Analyse error" << ex.what();
|
|
|
return ROBOT_FAILTURE;
|
|
|
}
|
|
|
- LOG(INFO)<<Name()<<"Analyse end\n";
|
|
|
+ LOG(INFO)<<Name()<<" Analyse end.";
|
|
|
return ROBOT_SUCCESS;
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
-void Localization::ParaGPRMC(std::string &strGPRMC, GPRMC &gprmc){
|
|
|
- std::vector<std::string> nmea_gprmc = Split(strGPRMC, ",");
|
|
|
- gprmc.utcTime = atof(nmea_gprmc[1].c_str());
|
|
|
- memset(&gprmc.locationStatus, 0x00, 2);
|
|
|
- memcpy(&gprmc.locationStatus, nmea_gprmc[2].c_str(), 1);
|
|
|
- gprmc. Latitude = StrToDouble(nmea_gprmc[3].c_str(), 2);
|
|
|
- memset(&gprmc.LatType, 0x00, 2);
|
|
|
- memcpy(&gprmc.LatType, nmea_gprmc[4].c_str(), 1);
|
|
|
- gprmc.Longitude = StrToDouble(nmea_gprmc[5].c_str(), 3);
|
|
|
- memset(&gprmc.LonType, 0x00, 2);
|
|
|
- memcpy(&gprmc.LonType, nmea_gprmc[6].c_str(), 1);
|
|
|
- gprmc.Speed = atof(nmea_gprmc[7].c_str());
|
|
|
- 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);
|
|
|
+int Localization::ParaGPRMC(std::string &strGPRMC, GPRMC &gprmc){
|
|
|
+ try
|
|
|
+ {
|
|
|
+ std::vector<std::string> nmea_gprmc = Split(strGPRMC, ",");
|
|
|
+ gprmc.utcTime = atof(nmea_gprmc[1].c_str());
|
|
|
+ memset(&gprmc.locationStatus, 0x00, 2);
|
|
|
+ memcpy(&gprmc.locationStatus, nmea_gprmc[2].c_str(), 1);
|
|
|
+ gprmc. Latitude = StrToDouble(nmea_gprmc[3].c_str(), 2);
|
|
|
+ memset(&gprmc.LatType, 0x00, 2);
|
|
|
+ memcpy(&gprmc.LatType, nmea_gprmc[4].c_str(), 1);
|
|
|
+ gprmc.Longitude = StrToDouble(nmea_gprmc[5].c_str(), 3);
|
|
|
+ memset(&gprmc.LonType, 0x00, 2);
|
|
|
+ memcpy(&gprmc.LonType, nmea_gprmc[6].c_str(), 1);
|
|
|
+ gprmc.Speed = atof(nmea_gprmc[7].c_str());
|
|
|
+ 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)
|
|
|
+ {
|
|
|
+ LOG(ERROR)<<Name()<< " ParaGPRMC error" << ex.what();
|
|
|
+ return ROBOT_FAILTURE;
|
|
|
+ }
|
|
|
+ return ROBOT_SUCCESS;
|
|
|
}
|
|
|
|
|
|
-void Localization::ParaGPGGA(std::string &strGPGGA, GPGGA &gpgga){
|
|
|
- std::vector<std::string> nmea_gpgga = Split(strGPGGA, ",");
|
|
|
- gpgga.utcTime = atof(nmea_gpgga[1].c_str());
|
|
|
- gpgga. Latitude = StrToDouble(nmea_gpgga[2].c_str(), 2);
|
|
|
- memset(&gpgga.LatType, 0x00, 2);
|
|
|
- memcpy(&gpgga.LatType, nmea_gpgga[3].c_str(), 1);
|
|
|
- gpgga.Longitude = StrToDouble(nmea_gpgga[4].c_str(), 3);
|
|
|
- memset(&gpgga.LonType, 0x00, 2);
|
|
|
- memcpy(&gpgga.LonType, nmea_gpgga[5].c_str(), 1);
|
|
|
- memset(&gpgga.GPSStatus, 0x00, 5);
|
|
|
- memcpy(&gpgga.GPSStatus, nmea_gpgga[6].c_str(), 4);
|
|
|
- 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);
|
|
|
+int Localization::ParaGPGGA(std::string &strGPGGA, GPGGA &gpgga){
|
|
|
+ try
|
|
|
+ {
|
|
|
+ std::vector<std::string> nmea_gpgga = Split(strGPGGA, ",");
|
|
|
+ gpgga.utcTime = atof(nmea_gpgga[1].c_str());
|
|
|
+ if (nmea_gpgga[2].compare("") == 0)
|
|
|
+ return ROBOT_FAILTURE;
|
|
|
+ gpgga.Latitude = StrToDouble(nmea_gpgga[2].c_str(), 2);
|
|
|
+ memset(&gpgga.LatType, 0x00, 2);
|
|
|
+ memcpy(&gpgga.LatType, nmea_gpgga[3].c_str(), 1);
|
|
|
+ gpgga.Longitude = StrToDouble(nmea_gpgga[4].c_str(), 3);
|
|
|
+ memset(&gpgga.LonType, 0x00, 2);
|
|
|
+ memcpy(&gpgga.LonType, nmea_gpgga[5].c_str(), 1);
|
|
|
+ memset(&gpgga.GPSStatus, 0x00, 5);
|
|
|
+ memcpy(&gpgga.GPSStatus, nmea_gpgga[6].c_str(), 4);
|
|
|
+ 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);
|
|
|
+ }
|
|
|
+ catch(ros::Exception ex)
|
|
|
+ {
|
|
|
+ LOG(ERROR)<<Name()<< " ParaGPGGA error" << ex.what();
|
|
|
+ return ROBOT_FAILTURE;
|
|
|
+ }
|
|
|
+ return ROBOT_SUCCESS;
|
|
|
}
|
|
|
|
|
|
|
|
|
int Localization::PublishData(NMEA0183 &nmea)
|
|
|
{
|
|
|
- LOG(INFO)<<Name()<<"PublishData start\n";
|
|
|
+ LOG(INFO)<<Name()<<" PublishData start\n";
|
|
|
try{
|
|
|
|
|
|
double northing, easting;
|
|
@@ -212,8 +223,6 @@ int Localization::PublishData(NMEA0183 &nmea)
|
|
|
localization.Latitude = nmea.gprmc.Latitude;
|
|
|
localization.Height = nmea.gpgga.Height;
|
|
|
localization.AngleDeviated = nmea.gprmc.Deviated;
|
|
|
-
|
|
|
- ros::Publisher geometry_pub = node_handle_->advertise<localization::localMsg>(robot::common::Geometry, 200);
|
|
|
geometry_pub.publish(localization);
|
|
|
}
|
|
|
catch (ros::Exception ex)
|
|
@@ -221,7 +230,7 @@ int Localization::PublishData(NMEA0183 &nmea)
|
|
|
LOG(ERROR)<<Name()<< " PublishData error" << ex.what();
|
|
|
return ROBOT_FAILTURE;
|
|
|
}
|
|
|
- LOG(INFO)<<Name()<<"PublishData end\n";
|
|
|
+ LOG(INFO)<<Name()<<" PublishData end\n";
|
|
|
return ROBOT_SUCCESS;
|
|
|
|
|
|
}
|