complete tool_trace2vectormap. add rtk_hcp2 to ros folder.

src/ros/catkin/src/rtk_hcp2/CMakeLists.txt

@@ -0,0 +1,107 @@
+cmake_minimum_required(VERSION 2.8.11)
+project(rtk_hcp2)
+
+
+message(STATUS  "Enter rtk_hcp2")
+
+
+SET(QT_PATH  /opt/qt/5.13.2/gcc_64)
+SET(AGX_QT_PATH  /usr/include/aarch64-linux-gnu/qt5)
+
+find_path(QT_EXIST   QtCore   ${QT_PATH}/include/QtCore  ${AGX_QT_PATH}/QtCore)
+
+if(QT_EXIST)
+include_directories(
+  ${QT_PATH}/include
+  ${QT_PATH}/include/QtCore
+  ${QT_PATH}/include/QtSerialPort
+  ${AGX_QT_PATH}
+  ${AGX_QT_PATH}/QtCore
+  ${AGX_QT_PATH}/QtSerialPort
+)
+link_directories(
+  ${QT_PATH}/lib
+)
+else()
+  message(FATAL_ERROR "Please set QT_PATH")
+endif()
+
+
+
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  pluginlib
+  sensor_msgs
+)
+find_package(Boost REQUIRED)
+find_package(Protobuf REQUIRED)
+
+set(CMAKE_INCLUDE_CURRENT_DIR ON)
+set(CMAKE_AUTOMOC ON)
+set(CMAKE_AUTOUIC ON)
+set(CMAKE_AUTORCC ON)
+
+
+if(CMAKE_COMPILER_IS_GNUCXX)
+    set(CMAKE_CXX_FLAGS "-fPIC ${CMAKE_CXX_FLAGS}")
+    message(STATUS "optional:-fPIC")   
+endif(CMAKE_COMPILER_IS_GNUCXX)
+
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+#find_package(Qt5Core  REQUIRED)
+#qt5_wrap_cpp(MOC src/qt_ros_test.h)
+#qt5_wrap_ui(UIC src/qt_ros_test.ui)
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if you package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+##   INCLUDE_DIRS include
+#  LIBRARIES client_plugin
+   CATKIN_DEPENDS 
+    roscpp std_msgs 
+    sensor_msgs pluginlib
+  DEPENDS
+    Boost
+    Protobuf
+#  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+##  INCLUDE_DIRS include
+  ${CMAKE_CURRENT_BINARY_DIR}/..
+  ${catkin_INCLUDE_DIRS} include
+  ${Boost_INCLUDE_DIR}
+  ${autoware_msgs_INCLUDE_DIRS}
+)
+
+## Declare a C++ executable
+add_executable(${PROJECT_NAME}_node src/main.cpp  src/gnss_coordinate_convert.cpp)
+target_link_libraries(${PROJECT_NAME}_node ${catkin_LIBRARIES}  ${OpenCV_LIBRARIES}    ${Protobuf_LIBRARIES}   Qt5Core Qt5SerialPort)
+

src/ros/catkin/src/rtk_hcp2/package.xml

@@ -0,0 +1,62 @@
+<?xml version="1.0"?>
+<package>
+  <name>rtk_hcp2</name>
+  <version>0.0.0</version>
+  <description>The rtk_hcp2 package for convert rtk message to ros message</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag --> 
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="hans@todo.todo">hans</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+
+  <!-- Url tags are optional, but mutiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/client_plugin</url> -->
+
+
+  <!-- Author tags are optional, mutiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintianers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *_depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use run_depend for packages you need at runtime: -->
+  <!--   <run_depend>message_runtime</run_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <run_depend>roscpp</run_depend>
+  <run_depend>std_msgs</run_depend>
+
+  <build_depend>pluginlib</build_depend>
+  <run_depend>pluginlib</run_depend>
+
+  <build_depend>sensor_msgs</build_depend>
+  <run_depend>sensor_msgs</run_depend>
+
+
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

src/ros/catkin/src/rtk_hcp2/src/gnss_coordinate_convert.cpp

@@ -0,0 +1,153 @@
+#include  "gnss_coordinate_convert.h"
+
+void gps2xy(double J4, double K4, double *x, double *y)
+{
+    int L4 = (int)((K4 - 1.5) / 3) + 1;
+    double M4 = K4 - (L4 * 3);
+    double N4 = sin(J4 * 3.1415926536 / 180);
+    double O4 = cos(J4 * 3.1415926536 / 180);
+    double P4 = tan(J4 * 3.1415926536 / 180);
+    double Q4 = 111134.8611 * J4 - N4 * O4 * (32005.7799 + 133.9238 * N4 * N4 + 0.6973 * N4 * N4 * N4 * N4 + 0.0039 * N4 * N4 * N4 * N4 * N4 * N4);
+    double R4 = sqrt(0.006738525414683) * O4;
+    double S4 = sqrt(1 + R4 * R4);
+    double T4 = 6399698.901783 / S4;
+    double U4 = (T4 / S4) / S4;
+    double V4 = O4 * M4 * 3.1415926536 / 180;
+    double W4 = 0.5 + (5 - P4 * P4 + 9 * R4 * R4 + 4 * R4 * R4 * R4 * R4) * V4 * V4 / 24;
+    double X4 = V4 * V4 * V4 * V4 / 720 * (61 + (P4 * P4 - 58) * P4 * P4);
+    double Y4 = 1 + V4 * V4 * (1 - P4 * P4 + R4 * R4) / 6;
+    double Z4 = V4 * V4 * V4 * V4 * (5 - 18 * P4 * P4 * P4 * P4 * P4 * P4 + 14 * R4 * R4 - 58 * R4 * R4 * P4 * P4) / 120;
+
+    *y = Q4 + T4 * P4 * V4 * V4 * (W4 + X4);
+    *x = 500000 + T4 * V4 * (Y4 + Z4);
+}
+
+//高斯投影由经纬度(Unit:DD)反算大地坐标(含带号，Unit:Metres)
+void GaussProjCal(double longitude, double latitude, double *X, double *Y)
+{
+    int ProjNo = 0; int ZoneWide; ////带宽
+    double longitude1, latitude1, longitude0, latitude0, X0, Y0, xval, yval;
+    double a, f, e2, ee, NN, T, C, A, M, iPI;
+    iPI = 0.0174532925199433; ////3.1415926535898/180.0;
+    ZoneWide = 6; ////6度带宽
+    a = 6378245.0; f = 1.0 / 298.3; //54年北京坐标系参数
+                                    ////a=6378140.0; f=1/298.257; //80年西安坐标系参数
+    ProjNo = (int)(longitude / ZoneWide);
+    longitude0 = ProjNo * ZoneWide + ZoneWide / 2;
+    longitude0 = longitude0 * iPI;
+    latitude0 = 0;
+    longitude1 = longitude * iPI; //经度转换为弧度
+    latitude1 = latitude * iPI; //纬度转换为弧度
+    e2 = 2 * f - f * f;
+    ee = e2 * (1.0 - e2);
+    NN = a / sqrt(1.0 - e2 * sin(latitude1)*sin(latitude1));
+    T = tan(latitude1)*tan(latitude1);
+    C = ee * cos(latitude1)*cos(latitude1);
+    A = (longitude1 - longitude0)*cos(latitude1);
+    M = a * ((1 - e2 / 4 - 3 * e2*e2 / 64 - 5 * e2*e2*e2 / 256)*latitude1 - (3 * e2 / 8 + 3 * e2*e2 / 32 + 45 * e2*e2
+        *e2 / 1024)*sin(2 * latitude1)
+        + (15 * e2*e2 / 256 + 45 * e2*e2*e2 / 1024)*sin(4 * latitude1) - (35 * e2*e2*e2 / 3072)*sin(6 * latitude1));
+    xval = NN * (A + (1 - T + C)*A*A*A / 6 + (5 - 18 * T + T * T + 72 * C - 58 * ee)*A*A*A*A*A / 120);
+    yval = M + NN * tan(latitude1)*(A*A / 2 + (5 - T + 9 * C + 4 * C*C)*A*A*A*A / 24
+        + (61 - 58 * T + T * T + 600 * C - 330 * ee)*A*A*A*A*A*A / 720);
+    X0 = 1000000L * (ProjNo + 1) + 500000L;
+    Y0 = 0;
+    xval = xval + X0; yval = yval + Y0;
+    *X = xval;
+    *Y = yval;
+}
+
+
+
+//=======================zhaobo0904
+#define PI  3.14159265358979
+void ZBGaussProjCal(double lon, double lat, double *X, double *Y)
+{
+    // 1975 年国际椭球体长半轴 a, 第一离心率 e2, 第二离心率 ep2
+    double a = 6378140.0;
+    double e2 = 0.006694384999588;
+    double ep2 = e2/(1-e2);
+
+    // 原点所在经度
+    double lon_origin = 6.0*int(lon/6) + 3.0;
+    lon_origin *= PI / 180.0;
+
+    double k0 = 0.9996;
+
+    // 角度转弧度
+    double lat1 = lat * PI / 180.0;
+    double lon1 = lon * PI / 180.0;
+
+
+    // 经线在该点处的曲率半径,
+    double N = a / sqrt(1 - e2*sin(lat1)*sin(lat1));
+
+
+    // 赤道到该点的经线长度近似值 M, 使用泰勒展开逐项积分然后取前四项.
+    // 这个近似值是将 N 作为纬度 \phi 的函数展开为泰勒计数, 然后在区间 [0, lat1] 上积分得到的.
+    // 首先计算前四项的系数 a1~a4.
+    double a1 = 1 - e2/4 - (3*e2*e2)/64 - (5*e2*e2*e2)/256;
+    double a2 = (3*e2)/8 + (3*e2*e2)/32 + (45*e2*e2*e2)/1024;
+    double a3 = (15*e2*e2)/256 + (45*e2*e2*e2)/1024;
+    double a4 = (35*e2*e2*e2)/3072;
+    double M = a * (a1*lat1 - a2*sin(2*lat1) + a3*sin(4*lat1) - a4*sin(6*lat1));
+
+    // 辅助量
+    double T = tan(lat1)*tan(lat1);
+    double C = ep2*cos(lat1)*cos(lat1);
+    double A = (lon1 - lon_origin)*cos(lat1);
+
+    *X = 500000.0 + k0 * N * (A + (1 - T + C)*(A*A*A)/6 + (5 - 18*T + T*T + 72*C - 58*ep2)*(A*A*A*A*A)/120);
+    *Y = M + N * tan(lat1) * ((A*A)/2 +
+                              (5 - T + 9*C + 4*C*C)*(A*A*A*A)/24 +
+                              (61 - 58*T + T*T + 600*C - 330*ep2)*(A*A*A*A*A*A)/720);
+
+
+    *Y *= k0;
+    return;
+}
+//==========================================================
+
+
+
+
+
+//高斯投影由大地坐标(Unit:Metres)反算经纬度(Unit:DD)
+void GaussProjInvCal(double X, double Y, double *longitude, double *latitude)
+{
+    int ProjNo; int ZoneWide; ////带宽
+    double longitude1, latitude1, longitude0, latitude0, X0, Y0, xval, yval;
+    double e1, e2, f, a, ee, NN, T, C, M, D, R, u, fai, iPI;
+    iPI = 0.0174532925199433; ////3.1415926535898/180.0;
+    a = 6378245.0; f = 1.0 / 298.3; //54年北京坐标系参数
+    ////a=6378140.0; f=1/298.257; //80年西安坐标系参数
+    ZoneWide = 6; ////6度带宽
+    ProjNo = (int)(X / 1000000L); //查找带号
+    longitude0 = (ProjNo - 1) * ZoneWide + ZoneWide / 2;
+    longitude0 = longitude0 * iPI; //中央经线
+    X0 = ProjNo * 1000000L + 500000L;
+    Y0 = 0;
+    xval = X - X0; yval = Y - Y0; //带内大地坐标
+    e2 = 2 * f - f * f;
+    e1 = (1.0 - sqrt(1 - e2)) / (1.0 + sqrt(1 - e2));
+    ee = e2 / (1 - e2);
+    M = yval;
+    u = M / (a*(1 - e2 / 4 - 3 * e2*e2 / 64 - 5 * e2*e2*e2 / 256));
+    fai = u + (3 * e1 / 2 - 27 * e1*e1*e1 / 32)*sin(2 * u) + (21 * e1*e1 / 16 - 55 * e1*e1*e1*e1 / 32)*sin(
+                4 * u)
+            + (151 * e1*e1*e1 / 96)*sin(6 * u) + (1097 * e1*e1*e1*e1 / 512)*sin(8 * u);
+    C = ee * cos(fai)*cos(fai);
+    T = tan(fai)*tan(fai);
+    NN = a / sqrt(1.0 - e2 * sin(fai)*sin(fai));
+    R = a * (1 - e2) / sqrt((1 - e2 * sin(fai)*sin(fai))*(1 - e2 * sin(fai)*sin(fai))*(1 - e2 * sin
+                                                                                       (fai)*sin(fai)));
+    D = xval / NN;
+    //计算经度(Longitude) 纬度(Latitude)
+    longitude1 = longitude0 + (D - (1 + 2 * T + C)*D*D*D / 6 + (5 - 2 * C + 28 * T - 3 * C*C + 8 * ee + 24 * T*T)*D
+                               *D*D*D*D / 120) / cos(fai);
+    latitude1 = fai - (NN*tan(fai) / R)*(D*D / 2 - (5 + 3 * T + 10 * C - 4 * C*C - 9 * ee)*D*D*D*D / 24
+                                         + (61 + 90 * T + 298 * C + 45 * T*T - 256 * ee - 3 * C*C)*D*D*D*D*D*D / 720);
+    //转换为度 DD
+    *longitude = longitude1 / iPI;
+    *latitude = latitude1 / iPI;
+}

src/ros/catkin/src/rtk_hcp2/src/gnss_coordinate_convert.h

@@ -0,0 +1,27 @@
+#pragma once
+#ifndef _IV_PERCEPTION_GNSS_CONVERT_
+#define _IV_PERCEPTION_GNSS_CONVERT_
+
+#include <math.h>
+//double nmeaConvert2Lat(string lat)
+//{
+//	Console.WriteLine(lat);
+//	double nmea_d = double.Parse(lat.Substring(0, 2));
+//	double nmea_m = double.Parse(lat.Substring(2, 6));
+//	return nmea_d + nmea_m / 60;
+//}
+//
+//double nmeaConvert2Lon(string lon)
+//{
+//	Console.WriteLine(lon);
+//	double nmea_d = double.Parse(lon.Substring(0, 3));
+//	double nmea_m = double.Parse(lon.Substring(3, 7));
+//	return nmea_d + nmea_m / 60;
+//}
+
+void gps2xy(double , double , double *, double *);
+void GaussProjCal(double longitude, double latitude, double *X, double *Y);
+void ZBGaussProjCal(double longitude, double latitude, double *X, double *Y);
+void GaussProjInvCal(double X, double Y, double *longitude, double *latitude);
+
+#endif // !_IV_PERCEPTION_GNSS_CONVERT_

src/ros/catkin/src/rtk_hcp2/src/main.cpp

@@ -0,0 +1,398 @@
+#include "ros/ros.h"
+
+
+#include <QMutex>
+#include <QSerialPort>
+
+#include <nav_msgs/Odometry.h>
+#include <geometry_msgs/PointStamped.h>
+#include <geometry_msgs/TwistStamped.h>
+#include <geometry_msgs/PoseStamped.h>
+
+#include <thread>
+
+#include "gnss_coordinate_convert.h"
+
+static std::string _point_topic = "/points_raw";
+static std::string _point_msgname = "lidar_pc";
+
+static std::string _image_topic = "/image_raw";
+static std::string _image_msgname = "picfront";
+
+static std::string  _odom_topic = "/odom";
+static std::string _odom_msgname = "odom";
+
+static std::string _waypoints_topic = "/final_waypoints";
+static std::string _waypoints_msgname = "waypoints";
+
+static std::string _pose_topic = "/current_pose";
+static std::string _twist_topic =  "/current_velocity";
+
+static std::string gstrserialportname = "/dev/ttyUSB0";
+
+ros::Publisher pose_pub;
+ros::Publisher twist_pub;
+
+static double glon0 = 117;
+static double glat0 = 39;
+static double ghead0 = 0;
+
+static bool _use_rtk_odom = true; //if use rtk , use_rtk_odom is true;
+static bool  _use_pilot_waypoints = true; //if use pilot to calculate waypoints, this is true
+
+QString mstrHCP2;
+
+void SerialGPSDecodeSen(QString strsen); 
+
+
+#include <cmath>
+
+struct Quaternion {
+    double w, x, y, z;
+};
+
+struct EulerAngles {
+    double roll, pitch, yaw;
+};
+
+EulerAngles ToEulerAngles(Quaternion q) {
+    EulerAngles angles;
+
+    // roll (x-axis rotation)
+    double sinr_cosp = 2 * (q.w * q.x + q.y * q.z);
+    double cosr_cosp = 1 - 2 * (q.x * q.x + q.y * q.y);
+    angles.roll = std::atan2(sinr_cosp, cosr_cosp);
+
+    // pitch (y-axis rotation)
+    double sinp = 2 * (q.w * q.y - q.z * q.x);
+    if (std::abs(sinp) >= 1)
+        angles.pitch = std::copysign(M_PI / 2, sinp); // use 90 degrees if out of range
+    else
+        angles.pitch = std::asin(sinp);
+
+    // yaw (z-axis rotation)
+    double siny_cosp = 2 * (q.w * q.z + q.x * q.y);
+    double cosy_cosp = 1 - 2 * (q.y * q.y + q.z * q.z);
+    angles.yaw = std::atan2(siny_cosp, cosy_cosp);
+
+    return angles;
+}
+
+
+Quaternion ToQuaternion(double yaw, double pitch, double roll) // yaw (Z), pitch (Y), roll (X)
+{
+    // Abbreviations for the various angular functions
+    double cy = cos(yaw * 0.5);
+    double sy = sin(yaw * 0.5);
+    double cp = cos(pitch * 0.5);
+    double sp = sin(pitch * 0.5);
+    double cr = cos(roll * 0.5);
+    double sr = sin(roll * 0.5);
+
+    Quaternion q;
+    q.w = cy * cp * cr + sy * sp * sr;
+    q.x = cy * cp * sr - sy * sp * cr;
+    q.y = sy * cp * sr + cy * sp * cr;
+    q.z = sy * cp * cr - cy * sp * sr;
+
+    return q;
+}
+
+//原文链接：https://blog.csdn.net/xiaoma_bk/article/details/79082629
+
+
+static void SerialGPSDecode()
+{
+    int xpos = mstrHCP2.indexOf('\n');
+    if(xpos>0)
+    {
+
+        QString xsen = mstrHCP2.left(xpos+1);
+        SerialGPSDecodeSen(xsen);
+        mstrHCP2.remove(0,xpos+1);
+        SerialGPSDecode();
+    }
+}
+
+
+static bool checknmeasen(const char * strsen,const unsigned int nlen)
+{
+    if(nlen< 4)return false;
+
+    int i;
+    char check;
+    int nstarpos = -1;
+    check = strsen[1]^strsen[2];
+    for(i=3;i<nlen;i++)
+    {
+        if(strsen[i] == '*')
+        {
+            nstarpos = i;
+            break;
+        }
+        check = check^strsen[i];
+    }
+    if(nstarpos < 0)return false;
+    if(nstarpos >(nlen -3))return false;
+    char strcheck[3];
+    int ncheck = check;
+    snprintf(strcheck,3,"%02X",ncheck);
+    char strsencheck[3];
+    strsencheck[2] = 0;
+    strsencheck[0] = strsen[nstarpos + 1];
+    strsencheck[1] = strsen[nstarpos + 2];
+    if(strncmp(strcheck,strsencheck,2) == 0)
+    {
+   //     qDebug("  r sen is %s",strsen);
+        return true;
+    }
+
+
+//    qDebug("  sen is %s",strsen);
+    return false;
+}
+
+
+void SerialGPSDecodeSen(QString strsen)
+{
+    QStringList strlistrmc;
+    strlistrmc = strsen.split(",");
+
+   if(strlistrmc.size() < 23)return;
+      if(strlistrmc.at(0) != "$GPCHC")return;
+
+   if(!checknmeasen(strsen.toLatin1().data(),strsen.length()))
+   {
+       int nPos = strsen.indexOf('$',10);
+       if(nPos > 0)
+       {
+           QString strnewsen = strsen.right(strsen.size()-nPos);
+//           qDebug("new sen is %s",strnewsen.toLatin1().data());
+           SerialGPSDecodeSen(strnewsen);
+       }
+       return;
+   }
+
+   double fheading,fLat,fLon,fVel,fPitch,fRoll;
+   double fHgt,gyro_x,gyro_y,gyro_z,acc_x,acc_y,acc_z;
+   int nsv1,nsv2;
+   int gpsweek,gpstime;
+   int insstate,rtkstate;
+   QString strx = strlistrmc.at(3);
+   fheading = strx.toDouble();
+
+   strx = strlistrmc.at(1);
+   gpsweek = strx.toInt();
+
+   strx = strlistrmc.at(2);
+   gpstime = strx.toInt();
+
+   strx = strlistrmc.at(12);
+   fLat = strx.toDouble();
+
+   strx  = strlistrmc.at(13);
+   fLon = strx.toDouble();
+
+   strx = strlistrmc.at(14);
+   fHgt = strx.toDouble();
+
+   double ve,vn,vu;
+   strx = strlistrmc.at(15);
+   ve = strx.toDouble();
+
+   strx = strlistrmc.at(16);
+   vn = strx.toDouble();
+
+   strx = strlistrmc.at(17);
+   vu = strx.toDouble();
+
+//   fVel = sqrt(ve*ve + vn*vn + vu*vu);
+   strx = strlistrmc.at(18);
+   fVel = strx.toDouble();
+
+
+   strx = strlistrmc.at(4);
+   fPitch = strx.toDouble();
+
+   strx = strlistrmc.at(5);
+   fRoll = strx.toDouble();
+
+   strx = strlistrmc.at(6);
+   gyro_x = strx.toDouble();
+
+   strx = strlistrmc.at(7);
+   gyro_y = strx.toDouble();
+
+   strx = strlistrmc.at(8);
+   gyro_z = strx.toDouble();
+
+   strx = strlistrmc.at(9);
+   acc_x = strx.toDouble();
+
+   strx = strlistrmc.at(10);
+   acc_y = strx.toDouble();
+
+   strx = strlistrmc.at(11);
+   acc_z = strx.toDouble();
+
+   strx = strlistrmc.at(19);
+   nsv1 = strx.toInt();
+
+   strx = strlistrmc.at(20);
+   nsv2 = strx.toInt();
+
+   strx = strlistrmc.at(21);
+   char strstate[3];
+   strstate[2] = 0;
+   if(strx.size() >= 2)
+   {
+
+       memcpy(strstate,strx.data(),2);
+  //     qDebug(strstate);
+       char xstate;
+       xstate = strstate[0];
+       rtkstate = 4;
+       int xs;
+       if(xstate == '0')xs = 0;
+       if(xstate == '1')xs = 3;
+       if(xstate == '2')xs = 4;
+       if(xstate == '3')xs = 8;
+       if(xstate == '4')xs = 6;
+       if(xstate == '5')xs = 5;
+       if(xstate == '6')xs = 3;
+       if(xstate == '7')xs = 4;
+       if(xstate == '8')xs = 5;
+       if(xstate == '9')xs = 5;
+
+       rtkstate = xs;
+//       if(mstate == 0)minsstate = 0;
+//       else minsstate = 4;
+
+       xstate = strstate[1];
+       if(xstate == '0')insstate = 3;
+       else insstate = 4;
+       if(rtkstate == 0)insstate = 3;
+   }
+
+
+    double x_o,y_o;
+    double x_now,y_now;
+    GaussProjCal(glon0,glat0,&x_o,&y_o);
+    GaussProjCal(fLon,fLat,&x_now,&y_now);
+    double rel_x = x_now - x_o;
+    double rel_y = y_now - y_o;
+    double rel_z = fHgt;
+
+    double pitch,roll,yaw;
+    pitch = fPitch * M_PI/180.0;
+    roll = fRoll * M_PI/180.0;
+    double hdg_o = (90 - ghead0)*M_PI/180.0;
+    yaw = (90 - fheading)*M_PI/180.0;//  - hdg_o;
+    while(yaw< 0)yaw = yaw +2.0*M_PI;
+    while(yaw>=(2.0*M_PI))yaw = yaw - 2.0*M_PI;
+    Quaternion quat = ToQuaternion(yaw,pitch,roll);
+
+    nav_msgs::Odometry msg;
+
+    msg.pose.pose.position.x = rel_x;
+    msg.pose.pose.position.y = rel_y;
+    msg.pose.pose.position.z = rel_z;
+
+    msg.pose.pose.orientation.x = quat.x;
+    msg.pose.pose.orientation.y = quat.y;
+    msg.pose.pose.orientation.z = quat.z;
+    msg.pose.pose.orientation.w = quat.w;
+
+    msg.twist.twist.linear.x = sqrt(pow(vn,2)+pow(ve,2));
+
+        geometry_msgs::PoseStamped  xPose;
+    xPose.pose = msg.pose.pose;
+    geometry_msgs::TwistStamped xtwist;
+    xtwist.twist = msg.twist.twist;
+
+    pose_pub.publish(xPose);
+    twist_pub.publish(xtwist);
+
+   
+
+
+}
+
+
+void ThreadRecvRTK()
+{
+    QSerialPort  *m_serialPort_GPS;
+    m_serialPort_GPS->setPortName(gstrserialportname.data());
+    m_serialPort_GPS->setBaudRate(230400);
+    m_serialPort_GPS->setParity(QSerialPort::NoParity);
+    m_serialPort_GPS->setDataBits(QSerialPort::Data8);
+    m_serialPort_GPS->setStopBits(QSerialPort::OneStop);
+    m_serialPort_GPS->setFlowControl(QSerialPort::NoFlowControl);
+    m_serialPort_GPS->setReadBufferSize(0);
+
+    bool bSerialPortOpen = false;
+     if(m_serialPort_GPS->open(QSerialPort::ReadWrite))
+    {
+        bSerialPortOpen = true;
+    }
+    else
+    {
+        std::cout<<" Open Port "<<gstrserialportname<<"  Fail."<<std::endl;
+        return;
+    }
+
+    while(bSerialPortOpen)
+    {
+        QByteArray ba = m_serialPort_GPS->readAll();
+
+        if(ba.length() > 0)
+        {
+            mstrHCP2.append(ba);
+            SerialGPSDecode();
+         //   mnNotRecvCount = 0;
+            //Decode
+        }
+        else
+        {
+            std::this_thread::sleep_for(std::chrono::milliseconds(1));
+        }
+    }
+}
+
+int main(int argc, char *argv[])
+{
+	ros::init(argc, argv, "pilottoros");
+//	ros::NodeHandle n;
+    ros::NodeHandle private_nh("~");
+	ros::Rate loop_rate(1);
+
+	    
+    private_nh.getParam("points_node",_point_topic);
+	private_nh.getParam("points_msgname",_point_msgname);
+    std::cout<<"_point_topic is "<<_point_topic<<std::endl;
+	std::cout<<"_point_msgname : "<<_point_msgname<<std::endl;
+
+    private_nh.getParam("image_node",_image_topic);
+	private_nh.getParam("image_msgname",_image_msgname);
+    std::cout<<"_image_topic is "<<_image_topic<<std::endl;
+	std::cout<<"_image_msgname : "<<_image_msgname<<std::endl;
+
+    private_nh.getParam("waypoints_node",_waypoints_topic);
+    private_nh.getParam("waypoints_msgname",_waypoints_msgname);
+    std::cout<<"_waypoints_topic is "<<_waypoints_topic<<std::endl;
+	std::cout<<"_waypoints_msgname : "<<_waypoints_msgname<<std::endl;
+
+    private_nh.getParam("use_rtk_odom",_use_rtk_odom);
+    private_nh.getParam("use_pilot_waypoints",_use_pilot_waypoints);
+
+
+            pose_pub = private_nh.advertise<geometry_msgs::PoseStamped>(_pose_topic,10);
+        twist_pub = private_nh.advertise<geometry_msgs::TwistStamped>(_twist_topic,10);
+
+
+std::thread * pthread = new std::thread(ThreadRecvRTK);
+
+
+ros::spin();
+
+}

src/tool/tool_trace2vectormap/mainwindow.cpp

@@ -2,6 +2,7 @@
 #include "ui_mainwindow.h"
 
 #include <QMessageBox>
+#include <QFileDialog>
 
 MainWindow::MainWindow(QWidget *parent)
     : QMainWindow(parent)
@@ -33,6 +34,15 @@ MainWindow::~MainWindow()
 
 void MainWindow::on_pushButton_Convert_clicked()
 {
+    QString str = QFileDialog::getOpenFileName(this,tr("Open file"),"",tr("Trace File(*.txt)"));
+    if(str.isEmpty())return;
+
+    QString dir = QFileDialog::getExistingDirectory(this, tr("Select Save Directory"),
+                                                    getenv("HOME"),
+                                                    QFileDialog::ShowDirsOnly
+                                                    | QFileDialog::DontResolveSymlinks);
+    if(dir.isEmpty())return;
+
     iv::trace2vectormap_param xparam;
     xparam.fLat0 = ui->lineEdit_Lat0->text().toDouble();
     xparam.fLon0 = ui->lineEdit_Lon0->text().toDouble();
@@ -57,8 +67,8 @@ void MainWindow::on_pushButton_Convert_clicked()
     {
         xparam.strColorRight = "Y";
     }
-    xparam.strtracefilepath = "/home/yuchuli/map/2022-1-14-9-56-44.txt";
-    xparam.stroutfolder = "/home/yuchuli/map/tracevector";
+    xparam.strtracefilepath = str.toStdString();
+    xparam.stroutfolder = dir.toStdString();//"/home/yuchuli/map/tracevector";
 
     trace2vectormap xtv(xparam);
     int nrtn = xtv.convert();

src/tool/tool_trace2vectormap/trace2vectormap.cpp

@@ -54,7 +54,7 @@ int trace2vectormap::convert()
             double flon = QString(badata[1]).toDouble();
             double flat = QString(badata[2]).toDouble();
             double fheading = QString(badata[5]).toDouble();
-            std::cout<<"lat: "<<flat<<" lon:"<<flon<<" heading:"<<fheading<<std::endl;
+//            std::cout<<"lat: "<<flat<<" lon:"<<flon<<" heading:"<<fheading<<std::endl;
             double x,y,fyaw;
             llh2xyy(flon,flat,fheading,x,y,fyaw,xparam.fLon0,xparam.fLat0);
             iv::vectormap::point xpoint;