change for use fusion_gpsimu.

src/decition/common/perception_sf/impl_gps.cpp

@@ -324,6 +324,11 @@ void iv::perception::GPSSensor::UpdateGPSIMU(iv::gps::gpsimu xgpsimu)
 
     data->speed = sqrt(pow(xgpsimu.ve(),2)+pow(xgpsimu.vn(),2)) * 3.6;
 
+    if(xgpsimu.has_speed())
+    {
+        data->speed =  xgpsimu.speed() * 3.6;
+    }
+
     GaussProjCal(data->gps_lng, data->gps_lat, &data->gps_x, &data->gps_y);
 
     ServiceCarStatus.mRTKStatus = data->rtk_status;

src/decition/common/perception_sf/sensor_gps.cpp

@@ -1,22 +1,52 @@
 #include <perception_sf/sensor_gps.h>
 #include "ivlog.h"
 
+#include "chassis.pb.h"
+
 extern std::string gstrmemgps;
 extern iv::Ivlog * givlog;
 
+
+
 namespace iv {
 namespace perception {
     GPSSensor * ggpsimu;
+    static int64_t mlastfusiongpsimutime;
+
+
+
+
+
 
     void Listengpsimu(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
     {
 //        ggpsimu->UpdateGPSIMU(strdata,nSize);
 
+        if(strcmp(strmemname,"fusion_gpsimu") == 0)
+        {
+            mlastfusiongpsimutime = std::chrono::system_clock::now().time_since_epoch().count()/1000000;
+        }
+        else
+        {
+            int64_t nmsnow = std::chrono::system_clock::now().time_since_epoch().count()/1000000;
+            if(abs(nmsnow - mlastfusiongpsimutime) < 3000)
+            {
+                std::cout<<" fusion_gpsimu is ok. use fusiongpsimu."<<std::endl;
+                return;
+            }
+        }
+
+
+
+
+
         iv::gps::gpsimu xgpsimu;
         if(!xgpsimu.ParseFromArray(strdata,nSize))
         {
             std::cout<<"ListenRaw Parse error."<<std::endl;
         }
+
+
 //        qDebug(" gps time is %ld",QDateTime::currentMSecsSinceEpoch());
         ggpsimu->UpdateGPSIMU(xgpsimu);
 
@@ -36,11 +66,13 @@ namespace perception {
 iv::perception::GPSSensor::GPSSensor() {
 	signal_gps_data = createSignal<sig_cb_gps_sensor_data>();
     ggpsimu = this;
+    mlastfusiongpsimutime = 0;
 }
 iv::perception::GPSSensor::~GPSSensor() {
 
     thread_sensor_run_->interrupt();
     iv::modulecomm::Unregister(mpagpsimu);
+    iv::modulecomm::Unregister(mpafusiongpsimu);
     std::cout<<"~GPSSensor()"<<std::endl;
 }
 
@@ -49,6 +81,7 @@ void iv::perception::GPSSensor::start()
     thread_sensor_run_ = new boost::thread(boost::bind(&iv::perception::GPSSensor::processSensor, this));
 
     mpagpsimu = iv::modulecomm::RegisterRecv(gstrmemgps.data(),iv::perception::Listengpsimu);
+    mpafusiongpsimu = iv::modulecomm::RegisterRecv("fusion_gpsimu",iv::perception::Listengpsimu);
     //OutputDebugString(TEXT("\nGPS thread Run\n"));
 }
 

src/decition/common/perception_sf/sensor_gps.h

@@ -210,6 +210,8 @@ namespace iv {
 
         private:
             void * mpagpsimu;
+            void * mpafusiongpsimu;
+
 
         public:
             void UpdateGPSIMU(iv::gps::gpsimu xgpsimu);

src/decition/decition_brain_sf_changan_shenlan/decition/brain.cpp

@@ -498,6 +498,8 @@ void iv::decition::BrainDecition::run() {
         eyes->getSensorObstacle(obs_radar_, obs_camera_,gps_data_, obs_lidar_grid_);	//从传感器线程临界区交换数据
 
 
+
+
         ServiceLidar.copylidarperto(lidar_per);
 
 

src/decition/decition_brain_sf_changan_shenlan/decition/decide_gps_00.cpp

@@ -1615,6 +1615,21 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
         }
     }
 
+    if(now_gps_ins.rtk_status == 16)
+    {
+        double sugSpeed = realspeed - 2.0;
+        if(sugSpeed<=5.0)sugSpeed = 5.0;
+        dSpeed = min(sugSpeed,dSpeed);
+
+    }
+    if(now_gps_ins.rtk_status == 15)
+    {
+        double sugSpeed = realspeed - 2.0;
+        if(sugSpeed<=2.0)sugSpeed = 5.0;
+        dSpeed = min(sugSpeed,dSpeed);
+
+    }
+
     if (gpsMapLine[PathPoint]->speed_mode == 2)
     {
         dSpeed = min(25.0,dSpeed);

src/decition/decition_brain_sf_changan_shenlan/decition_brain_sf_changan_shenlan.pro

@@ -36,7 +36,6 @@ SOURCES += $$PWD/../common/main.cpp \
     ../../include/msgtype/v2r.pb.cc \
     ../../include/msgtype/groupmsg.pb.cc
 
-
 include($$PWD/../common/common/common.pri)
 include($$PWD/decition/decition.pri)
 include($$PWD/../common/perception_sf/perception_sf.pri)
@@ -97,8 +96,7 @@ HEADERS += \
     ../common/perception_sf/sensor_lidar.h \
     ../common/perception_sf/sensor_radar.h \
     ../common/common/sysparam_type.h \
-    ../../include/msgtype/carstate.pb.h \
-
+    ../../include/msgtype/carstate.pb.h
     ../../include/msgtype/v2r.pb.h \
 
     ../../include/msgtype/groupmsg.pb.h

src/fusion/fusion_gpsndt/fusion_gpsndt.pro

@@ -31,7 +31,8 @@ SOURCES += main.cpp \
     ../../include/msgtype/imu.pb.cc \
     ../../include/msgtype/gpsimu.pb.cc \
     ../../include/msgtype/gps.pb.cc \
-    ../../include/msgtype/fusiongpslidar.pb.cc
+    ../../include/msgtype/fusiongpslidar.pb.cc \
+    ../../include/msgtype/chassis.pb.cc
 
 
 
@@ -41,7 +42,8 @@ HEADERS += \
     ../../include/msgtype/imu.pb.h \
     ../../include/msgtype/gpsimu.pb.h \
     ../../include/msgtype/gps.pb.h \
-    ../../include/msgtype/fusiongpslidar.pb.h
+    ../../include/msgtype/fusiongpslidar.pb.h \
+    ../../include/msgtype/chassis.pb.h
 
 
 

src/fusion/fusion_gpsndt/main.cpp

@@ -15,6 +15,7 @@
 #include "gpsimu.pb.h"
 #include "ndtgpspos.pb.h"
 #include "fusiongpslidar.pb.h"
+#include "chassis.pb.h"
 
 #include "ivbacktrace.h"
 #include "ivfault.h"
@@ -49,7 +50,7 @@ QMutex mMutexgps;
 QMutex mMutexndt;
 
 
-void * gpa,* gpb, * gpc, * gpd;
+void * gpa,* gpb, * gpc, * gpd,* gpe;
 
 std::string gstrgpsmsg;
 std::string gstrndtmsg;
@@ -63,6 +64,33 @@ QUdpSocket * gudpSocketGPSIMU;
 
 
 
+static int64_t glastchassitime = 0;
+static double gfChassisVelSpeed;
+
+
+void UpdateChassis(const char *strdata, const unsigned int nSize, const unsigned int index, const QDateTime *dt, const char *strmemname)
+{
+    (void)index;
+    (void)dt;
+    (void)strmemname;
+    iv::chassis xchassis;
+    //    static int ncount = 0;
+    if(!xchassis.ParseFromArray(strdata,nSize))
+    {
+        std::cout<<"iv::decition::BrainDecition::UpdateChassis ParseFrom Array Error."<<std::endl;
+        return;
+    }
+
+
+    if(xchassis.has_vel())
+    {
+        gfChassisVelSpeed = xchassis.vel();
+        glastchassitime = std::chrono::system_clock::now().time_since_epoch().count()/1000000;
+  //      std::cout<<" gf Veh speed : "<<gfVehSpd<<std::endl;
+    }
+
+}
+
 void ListenRaw(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
 {
     static unsigned int nFixCount = 0;
@@ -179,9 +207,18 @@ static void sharefusiongpsimu(iv::fusion::fusiongpslidar * pfusiongpslidar)
     gpsimu.set_roll(pfusiongpslidar->roll());
     gpsimu.set_pitch(pfusiongpslidar->pitch());
 
+
     gpsimu.set_rtk_state(pfusiongpslidar->state());
     gpsimu.set_ins_state(4);
 
+    int64_t nmsnow = std::chrono::system_clock::now().time_since_epoch().count()/1000000;
+    if(abs(nmsnow - glastchassitime) < 1000)
+    {
+        gpsimu.set_speed(gfChassisVelSpeed);
+        std::cout<<"use chassis speed."<<std::endl;
+        return;
+    }
+
     char * strser;
     bool bser;
     int nbytesize;
@@ -433,10 +470,10 @@ int main(int argc, char *argv[])
     gfault->SetFaultState(0,0,"Initialize.");
 
     gstrfusionmsg = xparam.GetParam("fusionmsg","fusion_gpslidar");
-    gstrgpsmsg = xparam.GetParam("gpsmsg","hcp1_gpsimu");
+    gstrgpsmsg = xparam.GetParam("gpsmsg","hcp2_gpsimu");
     gstrndtmsg = xparam.GetParam("ndtgpsposmsg","ndtgpspos");
     gstrlidaronly = xparam.GetParam("LidarOnly","false");
-    gstrfusiongpsimumsg = xparam.GetParam("fusiongpsimumsg","hcp2_gpsimu");
+    gstrfusiongpsimumsg = xparam.GetParam("fusiongpsimumsg","fusion_gpsimu");
     gstrshareudp = xparam.GetParam("shareudp","true");
     gstrsharegpsimu = xparam.GetParam("sharegpsimu","true");
 
@@ -459,6 +496,7 @@ int main(int argc, char *argv[])
     gpa = iv::modulecomm::RegisterRecv(gstrgpsmsg.data(),ListenRaw);
     gpb = iv::modulecomm::RegisterRecv(gstrndtmsg.data(),ListenNDTGPS);
     gpd = iv::modulecomm::RegisterSend(gstrfusiongpsimumsg.data(),10000,1);
+    gpe = iv::modulecomm::RegisterRecv("chassis",UpdateChassis);
 
 
     std::cout<<"start fusion_gpsndt"<<std::endl;

src/tool/map_collectfromveh/main.cpp

@@ -6,10 +6,94 @@
 
 iv::Ivlog * givlog;
 
+#include <getopt.h>
+#include <iostream>
+
+char gstr_memname[256];
+
+
+void print_useage()
+{
+    std::cout<<" -m --memname $mappath : share memory name. eq.  -m lidar_pc"<<std::endl;
+    std::cout<<" -h --help print help"<<std::endl;
+}
+
+int  GetOptLong(int argc, char *argv[]) {
+    int nRtn = 0;
+    int opt; // getopt_long() 的返回值
+    int digit_optind = 0; // 设置短参数类型及是否需要参数
+    (void)digit_optind;
+
+    // 如果option_index非空，它指向的变量将记录当前找到参数符合long_opts里的
+    // 第几个元素的描述，即是long_opts的下标值
+    int option_index = 0;
+    // 设置短参数类型及是否需要参数
+    const char *optstring = "m:h";
+
+    // 设置长参数类型及其简写，比如 --reqarg <==>-r
+    /*
+    struct option {
+             const char * name;  // 参数的名称
+             int has_arg; // 是否带参数值，有三种：no_argument， required_argument，optional_argument
+             int * flag; // 为空时，函数直接将 val 的数值从getopt_long的返回值返回出去，
+                     // 当非空时，val的值会被赋到 flag 指向的整型数中，而函数返回值为0
+             int val; // 用于指定函数找到该选项时的返回值，或者当flag非空时指定flag指向的数据的值
+        };
+    其中：
+        no_argument(即0)，表明这个长参数不带参数（即不带数值，如：--name）
+            required_argument(即1)，表明这个长参数必须带参数（即必须带数值，如：--name Bob）
+            optional_argument(即2)，表明这个长参数后面带的参数是可选的，（即--name和--name Bob均可）
+     */
+    static struct option long_options[] = {
+        {"memname", required_argument, NULL, 'm'},
+        {"help",  no_argument,       NULL, 'h'},
+ //       {"optarg", optional_argument, NULL, 'o'},
+        {0, 0, 0, 0}  // 添加 {0, 0, 0, 0} 是为了防止输入空值
+    };
+
+    while ( (opt = getopt_long(argc,
+                               argv,
+                               optstring,
+                               long_options,
+                               &option_index)) != -1) {
+//        printf("opt = %c\n", opt); // 命令参数，亦即 -a -b -n -r
+//        printf("optarg = %s\n", optarg); // 参数内容
+//        printf("optind = %d\n", optind); // 下一个被处理的下标值
+//        printf("argv[optind - 1] = %s\n",  argv[optind - 1]); // 参数内容
+//        printf("option_index = %d\n", option_index);  // 当前打印参数的下标值
+//        printf("\n");
+        switch(opt)
+        {
+        case 'm':
+            strncpy(gstr_memname,optarg,255);
+            break;
+        case 'h':
+            print_useage();
+            nRtn = 1; //because use -h
+            break;
+        default:
+            break;
+        }
+
+    }
+
+    return nRtn;
+}
+
 int main(int argc, char *argv[])
 {
     givlog = new iv::Ivlog("map_collectfromveh");
     QApplication a(argc, argv);
+
+    snprintf(gstr_memname,255,"hcp2_gpsimu");
+
+    int nRtn = GetOptLong(argc,argv);
+    if(nRtn == 1)  //show help,so exit.
+    {
+        return 0;
+    }
+
+
     MainWindow w;
     w.show();
     return a.exec();

src/tool/map_collectfromveh/mainwindow.cpp

@@ -19,6 +19,8 @@ MainWindow * gw;
 std::string strtypevalue[]={"None","Solid","Dash","Solid Solid","Dash Dash","Solid Dash","Dash Solid"};
 std::string strcolorvalue[]={"White","Yellow"};
 
+extern char gstr_memname[256];
+
 void UpdateGPS(const char * strdata,const unsigned int nSize)
 {
     std::shared_ptr<iv::gps::gpsimu> xgpsimu_ptr = std::shared_ptr<iv::gps::gpsimu>(new iv::gps::gpsimu);
@@ -91,7 +93,7 @@ MainWindow::MainWindow(QWidget *parent)
     mpscene->addItem(ptext);
     ptext->setPos(1000,1000);
 
-    RegisterRecvGPS((char *)"hcp2_gpsimu",UpdateGPS);
+    RegisterRecvGPS((char *)gstr_memname,UpdateGPS);
 
     mpNowPosItem = new QGraphicsEllipseItem(0-mfNowSize/2.0,0-mfNowSize/2.0,mfNowSize,mfNowSize);
     mpNowPosItem->setBrush(Qt::green);

src/tool/view_gps/main.cpp

@@ -86,6 +86,14 @@ int main(int argc, char *argv[])
 
     //snprintf(gstr_memname,255,"ins550d_gpsimu");
     snprintf(gstr_memname,255,"hcp2_gpsimu");
+
+    int nRtn = GetOptLong(argc,argv);
+    if(nRtn == 1)  //show help,so exit.
+    {
+        return 0;
+    }
+
+
     MainWindow w;
     w.show();