README.md

@@ -13,6 +13,7 @@
 
 3.首次下载完成后，请在modularization/路径下，右键打开终端，依次执行如下命令 
   > $chmod +x *.sh
+  > $./autogen_lib.sh
 
 4.修改autodeploy.sh中的CONFIG_IVSysMan为对应项目的配文件。具体文件可以在./other/目录下找到
 
@@ -42,3 +43,6 @@
   > DEFINES += MODULECOMM_NO_FASTRTPS屏蔽掉，这样就不需要
     libfastcdr.so libfastrtps .so libtinyxml2.so这三个库了。
 
+11.如果编译某个模块，报错：c++: internal compiler error: Segmentation fault (program cc1plus) 
+  > 修改系统设置限制：sudo gedit /etc/security/limits.conf，将所有stack大小设置为4096
+  > 重启系统，sudo reboot

deploylib.sh

@@ -1,6 +1,6 @@
 #! /bin/bash
 
-Qtgccdir=''
+Qtgccdir='/usr/lib/x86_64-linux-gnu/qt5'
 if [ ${#Qtgccdir} -lt 6 ]; then
   echo "Because not set gcc_64 , so auto find gcc_64 "
   optfiles=`find /opt -name 'gcc_64'` 

deploywithfind.sh

@@ -1,6 +1,6 @@
 #! /bin/bash
 
-Qtgccdir=''
+Qtgccdir= '' #'/usr/lib/x86_64-linux-gnu/qt5'
 if [ ${#Qtgccdir} -lt 6 ]; then
   echo "Because not set gcc_64 , so auto find gcc_64 "
   optfiles=`find /opt -name 'gcc_64'` 
@@ -23,6 +23,17 @@ if [ ${#Qtgccdir} -lt 6 ]; then
   fi
 fi
 
+if [ ${#Qtgccdir} -lt 6 ]; then
+  if [ -d '/usr/lib/x86_64-linux-gnu/qt5' ]; then
+    echo " Qt in /usr/lib"
+    Qtgccdir='/usr/lib/x86_64-linux-gnu/qt5'
+    QtPlatformdir=$Qtgccdir/plugins/platforms
+    QtLibDir=/usr/lib/x86_64-linux-gnu/
+  else 
+    echo "please sudo apt install qt"
+  fi
+fi
+
 if [ ${#Qtgccdir} -lt 6 ]; then
    echo -e "\033[31m""  -----not found gccdir: so exit""\033[0m"
    exit 1
@@ -35,25 +46,25 @@ libm.so.*
 libgcc_s.so.*
 #libc.so.*
 #libpthread.so.*
-libGL.so.*
+#libGL.so.*
 libz.so.*
 libc.so.*
 libgthread*
 libglib*
 libexpat*
-libxcb*
+#libxcb*
 #libdl.so.*
 libxshmfence*
 libglapi.so.*
 libXext.so.*
 libXdamage.so.*
 libXfixes.so.*
-libX11*
+#libX11*
 libXxf86vm.so.*
 libdrm.so.*
 libpcre.so.*
-libXau.so.*
-libXdmcp.so.*
+#libXau.so.*
+#libXdmcp.so.*
 )
 
 EXE="$1"
@@ -111,6 +122,12 @@ cd ..
 
 cp -r app $PWD/deploy/
 
+xlib=`ls ./deploy/app/lib/lib*`  
+for fileName in $xlib
+  do
+     patchelf --set-rpath '$ORIGIN' $fileName
+  done
+
 rm -rf app
 
 

include/common.pri

@@ -21,7 +21,7 @@ LIBS += -L$$PWD/../bin/ -lxmlparam -lmodulecomm  -livlog -livfault -livexit -liv
 
 unix:LIBS += -lboost_system  -lbacktrace -ldl
 
-QMAKE_CXXFLAGS +=  -g
+#QMAKE_CXXFLAGS +=  -g
 
 CONFIG += c++11
 

sh/envInstall_simple.sh

@@ -37,3 +37,6 @@ gsettings set org.gnome.Vino vnc-password $(echo -n '123456'|base64)
 
 echo 'nvidia' | sudo -S gpasswd --add nvidia dialout
 
+#if not install ros, some program need tf
+echo "nvidia" | sudo -S apt-get install libtf-dev
+

src/chassis_test/shenlan_test/main.cpp

@@ -0,0 +1,11 @@
+#include "mainwindow.h"
+#include <QApplication>
+
+int main(int argc, char *argv[])
+{
+    QApplication a(argc, argv);
+    MainWindow w;
+    w.show();
+
+    return a.exec();
+}

src/chassis_test/shenlan_test/mainwindow.cpp

@@ -0,0 +1,197 @@
+#include "mainwindow.h"
+#include "ui_mainwindow.h"
+#include "iostream"
+#include  "modulecomm.h"
+#include "decition.pb.h"
+#include "chassis.pb.h"
+
+#include <QTimer>
+
+static bool gbHaveVehSpd = false;
+static double gfVehSpd = 0.0;
+static double gfAng=0.0;
+QString qsSpd ;
+QString qsAng ;
+void UpdateChassis(const char *strdata, const unsigned int nSize, const unsigned int index, const QDateTime *dt, const char *strmemname);
+//void ListenChassis(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
+//{
+//    UpdateChassis(strdata,nSize);
+//}
+
+
+MainWindow* instance;
+MainWindow::MainWindow(QWidget *parent) :
+    QMainWindow(parent),
+    ui(new Ui::MainWindow)
+{
+    ui->setupUi(this);
+    instance=this;
+
+    this->setWindowTitle("中汽智联车辆线控测试系统");
+
+    this->setObjectName("parent");
+    this->setStyleSheet("#parent { border-image: url(:/new/prefix1/background4.png);}");
+    this->setWindowFlags(Qt::WindowMinimizeButtonHint|Qt::WindowCloseButtonHint);
+
+    timer =new QTimer(this);
+
+    mpaVechicleDeciton = iv::modulecomm::RegisterSend("deciton",10000,10);
+    gpachassis = iv::modulecomm::RegisterRecv("chassis",UpdateChassis);
+
+    connect(timer,SIGNAL(timeout()),this,SLOT(ShareChassisDebug()));
+
+    timer->start(20);
+
+    decition_debug.set_wheelangle(5);
+    decition_debug.set_torque(1);
+    decition_debug.set_brake(2);
+    decition_debug.set_leftlamp(0);
+    decition_debug.set_rightlamp(0);
+
+}
+
+
+//建立自身调用
+MainWindow *MainWindow::getInstance()
+{
+    return instance;
+}
+
+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_epsmode())
+    {
+    if(xchassis.epsmode() == 0)
+    {
+        bool gbChassisEPS = true;
+    }
+    }
+
+    if(xchassis.has_vel())
+    {
+        gfVehSpd = xchassis.vel();
+        gbHaveVehSpd = true;
+        qsSpd = QString::number(gfVehSpd, 'f', 2);
+
+  //      std::cout<<" gf Veh speed : "<<gfVehSpd<<std::endl;
+    }
+    if(xchassis.has_angle_feedback())
+    {
+       gfAng = xchassis.angle_feedback();
+       qsAng = QString::number(gfAng, 'f', 2);
+    }
+
+    MainWindow::getInstance()->ShowChassisData();
+}
+
+void MainWindow::ShowChassisData()
+{
+    ui->Vehicle_Speed->setText(qsSpd);
+    ui->EPS_Ang->setText(qsAng);
+}
+
+void MainWindow::ShareChassisDebug()
+{
+    int nsize = decition_debug.ByteSize();
+    char * str = new char[nsize];
+    std::shared_ptr<char> pstr;
+    pstr.reset(str);
+    if(ui->left_turn->isChecked())
+    {
+        decition_debug.set_leftlamp(1);
+        decition_debug.set_rightlamp(0);
+    }
+    else if(ui->right_turn->isChecked())
+    {
+        decition_debug.set_leftlamp(0);
+        decition_debug.set_rightlamp(1);
+    }
+    else
+    {
+        decition_debug.set_leftlamp(0);
+        decition_debug.set_rightlamp(0);
+    }
+    if(decition_debug.SerializeToArray(str,nsize))
+    {
+        if(ui->start_send->isChecked())
+         iv::modulecomm::ModuleSendMsg(mpaVechicleDeciton,str,nsize);
+    }
+    else
+    {
+        std::cout<<"iv::decition::ShareChassisDebug::ShareChassisDebug serialize error."<<std::endl;
+    }
+}
+
+MainWindow::~MainWindow()
+{
+    iv::modulecomm::Unregister(mpaVechicleDeciton);
+    iv::modulecomm::Unregister(gpachassis);
+    delete ui;
+}
+
+void MainWindow::on_doubleang_pos_valueChanged(double arg1)
+{
+    decition_debug.set_wheelangle(arg1);
+}
+
+void MainWindow::on_targe_acc_valueChanged(double arg1)  //加速扭矩
+{
+    if(arg1>=0.0)
+    {
+      decition_debug.set_torque(arg1);
+      decition_debug.set_brake(0);
+      ui->brake->setValue(0.0);
+    }
+    else
+    {
+      decition_debug.set_torque(0);
+    }
+}
+
+void MainWindow::on_brake_valueChanged(double arg1)
+{
+    if(arg1<=0.0)
+    {
+      decition_debug.set_torque(0);
+      decition_debug.set_brake(arg1);
+      ui->targe_acc->setValue(0.0);
+    }
+    else
+    {
+      decition_debug.set_brake(arg1);
+    }
+}
+
+void MainWindow::on_left_turn_clicked()
+{
+
+}
+void MainWindow::on_left_turn_clicked(bool checked)
+{
+//    if(checked)
+//    {
+//        decition_debug.set_leftlamp(1);
+//        decition_debug.set_rightlamp(0);
+//    }
+//    else
+//        decition_debug.set_leftlamp(0);
+}
+
+void MainWindow::on_right_turn_clicked(bool checked)
+{
+//    if(checked)
+//        decition_debug.set_rightlamp(1);
+//    else
+//        decition_debug.set_rightlamp(0);
+}

src/chassis_test/shenlan_test/mainwindow.h

@@ -0,0 +1,55 @@
+#ifndef MAINWINDOW_H
+#define MAINWINDOW_H
+
+#include <QMainWindow>
+#include  "modulecomm.h"
+#include "decition.pb.h"
+
+    namespace Ui {
+    class MainWindow;
+    }
+
+    using namespace iv;
+
+class MainWindow : public QMainWindow
+{
+    Q_OBJECT
+
+private slots:
+     //void ShareChassisDebug(iv::brain::decition xbs);
+     void ShareChassisDebug();
+
+     void on_doubleang_pos_valueChanged(double arg1);
+
+     void on_targe_acc_valueChanged(double arg1);
+
+     void on_brake_valueChanged(double arg1);
+
+     void on_left_turn_clicked();
+
+     void on_left_turn_clicked(bool checked);
+
+     void on_right_turn_clicked(bool checked);
+
+public:
+    explicit MainWindow(QWidget *parent = 0);
+    ~MainWindow();
+    void * mpaVechicleDeciton;
+    void * gpachassis;
+    iv::brain::decition decition_debug;
+    static MainWindow* getInstance();
+     void UpdateDecition(const char *pdata, const int ndatasize);
+     void ShowChassisData();
+//     void UpdateChassis(const char *strdata, const unsigned int nSize, const unsigned int index, const QDateTime *dt, const char *strmemname);
+
+private:
+    Ui::MainWindow *ui;
+
+
+    QTimer *timer;
+
+};
+
+
+
+#endif // MAINWINDOW_H

src/chassis_test/shenlan_test/mainwindow.ui

@@ -0,0 +1,1606 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<ui version="4.0">
+ <class>MainWindow</class>
+ <widget class="QMainWindow" name="MainWindow">
+  <property name="geometry">
+   <rect>
+    <x>0</x>
+    <y>0</y>
+    <width>965</width>
+    <height>706</height>
+   </rect>
+  </property>
+  <property name="sizePolicy">
+   <sizepolicy hsizetype="Fixed" vsizetype="Fixed">
+    <horstretch>0</horstretch>
+    <verstretch>0</verstretch>
+   </sizepolicy>
+  </property>
+  <property name="windowTitle">
+   <string>MainWindow</string>
+  </property>
+  <property name="windowIcon">
+   <iconset resource="resources.qrc">
+    <normaloff>:/new/prefix1/adc.ico</normaloff>:/new/prefix1/adc.ico</iconset>
+  </property>
+  <widget class="QWidget" name="centralWidget">
+   <layout class="QGridLayout" name="gridLayout">
+    <item row="1" column="1">
+     <widget class="QGroupBox" name="groupBox">
+      <property name="enabled">
+       <bool>true</bool>
+      </property>
+      <property name="styleSheet">
+       <string notr="true"/>
+      </property>
+      <property name="title">
+       <string>车辆状态实时信息</string>
+      </property>
+      <layout class="QGridLayout" name="gridLayout_8">
+       <item row="0" column="2">
+        <layout class="QHBoxLayout" name="horizontalLayout_16">
+         <item>
+          <widget class="QLabel" name="label_14">
+           <property name="enabled">
+            <bool>false</bool>
+           </property>
+           <property name="text">
+            <string>加速度（m/s2）</string>
+           </property>
+          </widget>
+         </item>
+         <item>
+          <widget class="QLabel" name="real_acc">
+           <property name="enabled">
+            <bool>false</bool>
+           </property>
+           <property name="frameShape">
+            <enum>QFrame::Box</enum>
+           </property>
+           <property name="text">
+            <string>0.0</string>
+           </property>
+          </widget>
+         </item>
+        </layout>
+       </item>
+       <item row="11" column="2">
+        <layout class="QHBoxLayout" name="horizontalLayout_27">
+         <item>
+          <widget class="QLabel" name="label_5">
+           <property name="enabled">
+            <bool>false</bool>
+           </property>
+           <property name="text">
+            <string>右前轮轮速(km/h)</string>
+           </property>
+          </widget>
+         </item>
+         <item>
+          <widget class="QLabel" name="RFspeed">
+           <property name="enabled">
+            <bool>false</bool>
+           </property>
+           <property name="frameShape">
+            <enum>QFrame::Box</enum>
+           </property>
+           <property name="text">
+            <string>0</string>
+           </property>
+           <property name="alignment">
+            <set>Qt::AlignCenter</set>
+           </property>
+          </widget>
+         </item>
+        </layout>
+       </item>
+       <item row="13" column="2">
+        <layout class="QHBoxLayout" name="horizontalLayout_12">
+         <item>
+          <widget class="QLabel" name="label_7">
+           <property name="enabled">
+            <bool>false</bool>
+           </property>
+           <property name="text">
+            <string>右后轮轮速(km/h)</string>
+           </property>
+          </widget>
+         </item>
+         <item>
+          <widget class="QLabel" name="RBspeed">
+           <property name="enabled">
+            <bool>false</bool>
+           </property>
+           <property name="frameShape">
+            <enum>QFrame::Box</enum>
+           </property>
+           <property name="text">
+            <string>0</string>
+           </property>
+           <property name="alignment">
+            <set>Qt::AlignCenter</set>
+           </property>
+          </widget>
+         </item>
+        </layout>
+       </item>
+       <item row="8" column="1">
+        <layout class="QHBoxLayout" name="horizontalLayout_28">
+         <item>
+          <widget class="QLabel" name="label_24">
+           <property name="enabled">
+            <bool>false</bool>
+           </property>
+           <property name="text">
+            <string>近光灯</string>
+           </property>
+          </widget>
+         </item>
+         <item>
+          <widget class="QLabel" name="dippbeam_status">
+           <property name="enabled">
+            <bool>false</bool>
+           </property>
+           <property name="frameShape">
+            <enum>QFrame::Box</enum>
+           </property>
+           <property name="text">
+            <string>0</string>
+           </property>
+           <property name="alignment">
+            <set>Qt::AlignCenter</set>
+           </property>
+          </widget>
+         </item>
+        </layout>
+       </item>
+       <item row="1" column="1">
+        <layout class="QHBoxLayout" name="horizontalLayout_2">
+         <item>
+          <widget class="QLabel" name="label_13">
+           <property name="enabled">
+            <bool>false</bool>
+           </property>
+           <property name="text">
+            <string>方向盘转向</string>
+           </property>
+          </widget>
+         </item>
+         <item>
+          <widget class="QLabel" name="steer_sign">
+           <property name="enabled">
+            <bool>false</bool>
+           </property>
+           <property name="frameShape">
+            <enum>QFrame::Box</enum>
+           </property>
+           <property name="text">
+            <string>R</string>
+           </property>
+           <property name="alignment">
+            <set>Qt::AlignCenter</set>
+           </property>
+          </widget>
+         </item>
+        </layout>
+       </item>
+       <item row="4" column="1">
+        <layout class="QHBoxLayout" name="horizontalLayout_17">
+         <property name="spacing">
+          <number>6</number>
+         </property>
+         <property name="sizeConstraint">
+          <enum>QLayout::SetDefaultConstraint</enum>
+         </property>
+        </layout>
+       </item>
+       <item row="3" column="1">
+        <layout class="QHBoxLayout" name="horizontalLayout_8">
+         <item>
+          <widget class="QLabel" name="label_15">
+           <property name="enabled">
+            <bool>false</bool>
+           </property>
+           <property name="text">
+            <string>当前挡位</string>
+           </property>
+          </widget>
+         </item>
+         <item>
+          <widget class="QLabel" name="real_gear">
+           <property name="enabled">
+            <bool>false</bool>
+           </property>
+           <property name="frameShape">
+            <enum>QFrame::Box</enum>
+           </property>
+           <property name="text">
+            <string>P</string>
+           </property>
+           <property name="alignment">
+            <set>Qt::AlignCenter</set>
+           </property>
+          </widget>
+         </item>
+        </layout>
+       </item>
+       <item row="1" column="2">
+        <layout class="QHBoxLayout" name="horizontalLayout_20"/>
+       </item>
+       <item row="10" column="1">
+        <layout class="QHBoxLayout" name="horizontalLayout_32">
+         <item>
+          <widget class="QLabel" name="label_28">
+           <property name="enabled">
+            <bool>false</bool>
+           </property>
+           <property name="text">
+            <string>制动灯</string>
+           </property>
+          </widget>
+         </item>
+         <item>
+          <widget class="QLabel" name="brakelight_status">
+           <property name="enabled">
+            <bool>false</bool>
+           </property>
+           <property name="frameShape">
+            <enum>QFrame::Box</enum>
+           </property>
+           <property name="text">
+            <string>0</string>
+           </property>
+           <property name="alignment">
+            <set>Qt::AlignCenter</set>
+           </property>
+          </widget>
+         </item>
+        </layout>
+       </item>
+       <item row="11" column="1">
+        <layout class="QHBoxLayout" name="horizontalLayout_26">
+         <item>
+          <widget class="QLabel" name="label_11">
+           <property name="enabled">
+            <bool>false</bool>
+           </property>
+           <property name="text">
+            <string>驾驶模式</string>
+           </property>
+          </widget>
+         </item>
+         <item>
+          <widget class="QLabel" name="driving_state">
+           <property name="enabled">
+            <bool>false</bool>
+           </property>
+           <property name="autoFillBackground">
+            <bool>false</bool>
+           </property>
+           <property name="frameShape">
+            <enum>QFrame::Box</enum>
+           </property>
+           <property name="text">
+            <string>Manual</string>
+           </property>
+           <property name="alignment">
+            <set>Qt::AlignCenter</set>
+           </property>
+          </widget>
+         </item>
+        </layout>
+       </item>
+       <item row="4" column="2">
+        <layout class="QHBoxLayout" name="horizontalLayout_7">
+         <item>
+          <widget class="QLabel" name="gas_show">
+           <property name="enabled">
+            <bool>false</bool>
+           </property>
+           <property name="text">
+            <string>人工干预油门百分比</string>
+           </property>
+           <property name="alignment">
+            <set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter</set>
+           </property>
+          </widget>
+         </item>
+         <item>
+          <widget class="QLabel" name="gas_pedal">
+           <property name="enabled">
+            <bool>false</bool>
+           </property>
+           <property name="frameShape">
+            <enum>QFrame::Box</enum>
+           </property>
+           <property name="text">
+            <string>0</string>
+           </property>
+           <property name="alignment">
+            <set>Qt::AlignCenter</set>
+           </property>
+          </widget>
+         </item>
+        </layout>
+       </item>
+       <item row="12" column="2">
+        <layout class="QHBoxLayout" name="horizontalLayout_11">
+         <item>
+          <widget class="QLabel" name="label_6">
+           <property name="enabled">
+            <bool>false</bool>
+           </property>
+           <property name="text">
+            <string>左后轮轮速(km/h)</string>
+           </property>
+          </widget>
+         </item>
+         <item>
+          <widget class="QLabel" name="LBspeed">
+           <property name="enabled">
+            <bool>false</bool>
+           </property>
+           <property name="frameShape">
+            <enum>QFrame::Box</enum>
+           </property>
+           <property name="text">
+            <string>0</string>
+           </property>
+           <property name="alignment">
+            <set>Qt::AlignCenter</set>
+           </property>
+          </widget>
+         </item>
+        </layout>
+       </item>
+       <item row="2" column="2">
+        <layout class="QHBoxLayout" name="horizontalLayout_24">
+         <item>
+          <widget class="QLabel" name="label_20">
+           <property name="enabled">
+            <bool>false</bool>
+           </property>
+           <property name="text">
+            <string>纵向工作模式</string>
+           </property>
+          </widget>
+         </item>
+         <item>
+          <widget class="QLabel" name="longitude_mode">
+           <property name="enabled">
+            <bool>false</bool>
+           </property>
+           <property name="frameShape">
+            <enum>QFrame::Box</enum>
+           </property>
+           <property name="text">
+            <string>0</string>
+           </property>
+           <property name="alignment">
+            <set>Qt::AlignCenter</set>
+           </property>
+          </widget>
+         </item>
+        </layout>
+       </item>
+       <item row="9" column="1">
+        <layout class="QHBoxLayout" name="horizontalLayout_30">
+         <item>
+          <widget class="QLabel" name="label_26">
+           <property name="enabled">
+            <bool>false</bool>
+           </property>
+           <property name="text">
+            <string>雾灯</string>
+           </property>
+          </widget>
+         </item>
+         <item>
+          <widget class="QLabel" name="frog_status">
+           <property name="enabled">
+            <bool>false</bool>
+           </property>
+           <property name="frameShape">
+            <enum>QFrame::Box</enum>
+           </property>
+           <property name="text">
+            <string>0</string>
+           </property>
+           <property name="alignment">
+            <set>Qt::AlignCenter</set>
+           </property>
+          </widget>
+         </item>
+        </layout>
+       </item>
+       <item row="0" column="1">
+        <layout class="QHBoxLayout" name="horizontalLayout">
+         <item>
+          <widget class="QLabel" name="label_2">
+           <property name="enabled">
+            <bool>true</bool>
+           </property>
+           <property name="text">
+            <string>方向盘转角（°）</string>
+           </property>
+           <property name="alignment">
+            <set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter</set>
+           </property>
+          </widget>
+         </item>
+         <item>
+          <widget class="QLabel" name="EPS_Ang">
+           <property name="frameShape">
+            <enum>QFrame::Box</enum>
+           </property>
+           <property name="text">
+            <string>0</string>
+           </property>
+           <property name="alignment">
+            <set>Qt::AlignCenter</set>
+           </property>
+          </widget>
+         </item>
+        </layout>
+       </item>
+       <item row="3" column="2">
+        <layout class="QHBoxLayout" name="horizontalLayout_25">
+         <item>
+          <widget class="QLabel" name="label_3">
+           <property name="enabled">
+            <bool>false</bool>
+           </property>
+           <property name="text">
+            <string>制动踏板状态</string>
+           </property>
+           <property name="alignment">
+            <set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter</set>
+           </property>
+          </widget>
+         </item>
+         <item>
+          <widget class="QLabel" name="brake_press">
+           <property name="enabled">
+            <bool>false</bool>
+           </property>
+           <property name="frameShape">
+            <enum>QFrame::Box</enum>
+           </property>
+           <property name="text">
+            <string>0</string>
+           </property>
+           <property name="alignment">
+            <set>Qt::AlignCenter</set>
+           </property>
+          </widget>
+         </item>
+        </layout>
+       </item>
+       <item row="7" column="2">
+        <layout class="QHBoxLayout" name="horizontalLayout_10">
+         <item>
+          <widget class="QLabel" name="label_19">
+           <property name="enabled">
+            <bool>false</bool>
+           </property>
+           <property name="text">
+            <string>双闪</string>
+           </property>
+          </widget>
+         </item>
+         <item>
+          <widget class="QLabel" name="emergency_status">
+           <property name="enabled">
+            <bool>false</bool>
+           </property>
+           <property name="frameShape">
+            <enum>QFrame::Box</enum>
+           </property>
+           <property name="text">
+            <string>0</string>
+           </property>
+           <property name="alignment">
+            <set>Qt::AlignCenter</set>
+           </property>
+          </widget>
+         </item>
+        </layout>
+       </item>
+       <item row="14" column="1">
+        <layout class="QHBoxLayout" name="horizontalLayout_21">
+         <property name="spacing">
+          <number>13</number>
+         </property>
+         <property name="sizeConstraint">
+          <enum>QLayout::SetDefaultConstraint</enum>
+         </property>
+         <item>
+          <widget class="QLabel" name="label_10">
+           <property name="enabled">
+            <bool>false</bool>
+           </property>
+           <property name="sizePolicy">
+            <sizepolicy hsizetype="Fixed" vsizetype="Preferred">
+             <horstretch>0</horstretch>
+             <verstretch>0</verstretch>
+            </sizepolicy>
+           </property>
+           <property name="text">
+            <string>手刹状态</string>
+           </property>
+           <property name="alignment">
+            <set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter</set>
+           </property>
+          </widget>
+         </item>
+         <item>
+          <widget class="QLabel" name="epb_state">
+           <property name="enabled">
+            <bool>false</bool>
+           </property>
+           <property name="sizePolicy">
+            <sizepolicy hsizetype="Preferred" vsizetype="Fixed">
+             <horstretch>0</horstretch>
+             <verstretch>0</verstretch>
+            </sizepolicy>
+           </property>
+           <property name="minimumSize">
+            <size>
+             <width>0</width>
+             <height>50</height>
+            </size>
+           </property>
+           <property name="maximumSize">
+            <size>
+             <width>50</width>
+             <height>50</height>
+            </size>
+           </property>
+           <property name="autoFillBackground">
+            <bool>false</bool>
+           </property>
+           <property name="styleSheet">
+            <string notr="true">background-image: url(:/new/pic/EPB_Parking.png)</string>
+           </property>
+           <property name="frameShape">
+            <enum>QFrame::NoFrame</enum>
+           </property>
+           <property name="text">
+            <string/>
+           </property>
+           <property name="alignment">
+            <set>Qt::AlignCenter</set>
+           </property>
+          </widget>
+         </item>
+        </layout>
+       </item>
+       <item row="2" column="1">
+        <layout class="QHBoxLayout" name="horizontalLayout_23">
+         <item>
+          <widget class="QLabel" name="eps_mode_label">
+           <property name="enabled">
+            <bool>false</bool>
+           </property>
+           <property name="text">
+            <string>EPS模式</string>
+           </property>
+          </widget>
+         </item>
+         <item>
+          <widget class="QLabel" name="eps_mode">
+           <property name="enabled">
+            <bool>false</bool>
+           </property>
+           <property name="frameShape">
+            <enum>QFrame::Box</enum>
+           </property>
+           <property name="text">
+            <string>0</string>
+           </property>
+           <property name="alignment">
+            <set>Qt::AlignCenter</set>
+           </property>
+          </widget>
+         </item>
+        </layout>
+       </item>
+       <item row="10" column="2">
+        <layout class="QHBoxLayout" name="horizontalLayout_9">
+         <item>
+          <widget class="QLabel" name="label_4">
+           <property name="enabled">
+            <bool>false</bool>
+           </property>
+           <property name="text">
+            <string>左前轮轮速（km/h)</string>
+           </property>
+          </widget>
+         </item>
+         <item>
+          <widget class="QLabel" name="LFspeed">
+           <property name="enabled">
+            <bool>false</bool>
+           </property>
+           <property name="frameShape">
+            <enum>QFrame::Box</enum>
+           </property>
+           <property name="text">
+            <string>0</string>
+           </property>
+           <property name="alignment">
+            <set>Qt::AlignCenter</set>
+           </property>
+          </widget>
+         </item>
+        </layout>
+       </item>
+       <item row="8" column="2">
+        <layout class="QHBoxLayout" name="horizontalLayout_29">
+         <item>
+          <widget class="QLabel" name="label_25">
+           <property name="enabled">
+            <bool>false</bool>
+           </property>
+           <property name="text">
+            <string>远光灯</string>
+           </property>
+          </widget>
+         </item>
+         <item>
+          <widget class="QLabel" name="highbeam_status">
+           <property name="enabled">
+            <bool>false</bool>
+           </property>
+           <property name="frameShape">
+            <enum>QFrame::Box</enum>
+           </property>
+           <property name="text">
+            <string>0</string>
+           </property>
+           <property name="alignment">
+            <set>Qt::AlignCenter</set>
+           </property>
+          </widget>
+         </item>
+        </layout>
+       </item>
+       <item row="9" column="2">
+        <layout class="QHBoxLayout" name="horizontalLayout_31">
+         <item>
+          <widget class="QLabel" name="label_27">
+           <property name="enabled">
+            <bool>false</bool>
+           </property>
+           <property name="text">
+            <string>雨刮</string>
+           </property>
+          </widget>
+         </item>
+         <item>
+          <widget class="QLabel" name="windshieldwiper_status">
+           <property name="enabled">
+            <bool>false</bool>
+           </property>
+           <property name="frameShape">
+            <enum>QFrame::Box</enum>
+           </property>
+           <property name="text">
+            <string>0</string>
+           </property>
+           <property name="alignment">
+            <set>Qt::AlignCenter</set>
+           </property>
+          </widget>
+         </item>
+        </layout>
+       </item>
+       <item row="7" column="1">
+        <layout class="QHBoxLayout" name="horizontalLayout_13">
+         <item>
+          <widget class="QLabel" name="label_21">
+           <property name="enabled">
+            <bool>false</bool>
+           </property>
+           <property name="text">
+            <string>转向灯</string>
+           </property>
+          </widget>
+         </item>
+         <item>
+          <widget class="QLabel" name="turn_lightst">
+           <property name="enabled">
+            <bool>false</bool>
+           </property>
+           <property name="frameShape">
+            <enum>QFrame::Box</enum>
+           </property>
+           <property name="text">
+            <string>R</string>
+           </property>
+           <property name="alignment">
+            <set>Qt::AlignCenter</set>
+           </property>
+          </widget>
+         </item>
+        </layout>
+       </item>
+      </layout>
+     </widget>
+    </item>
+    <item row="0" column="0" colspan="2">
+     <widget class="QGroupBox" name="groupBox_9">
+      <property name="title">
+       <string>CAN卡操作</string>
+      </property>
+      <layout class="QGridLayout" name="gridLayout_11">
+       <item row="1" column="0">
+        <widget class="QCheckBox" name="start_send">
+         <property name="enabled">
+          <bool>true</bool>
+         </property>
+         <property name="text">
+          <string>握手信号</string>
+         </property>
+        </widget>
+       </item>
+       <item row="0" column="0">
+        <widget class="QPushButton" name="pushButton">
+         <property name="enabled">
+          <bool>false</bool>
+         </property>
+         <property name="text">
+          <string>启动CAN设备</string>
+         </property>
+        </widget>
+       </item>
+       <item row="0" column="1">
+        <widget class="QPushButton" name="pushButton_2">
+         <property name="enabled">
+          <bool>false</bool>
+         </property>
+         <property name="text">
+          <string>关闭CAN设备</string>
+         </property>
+        </widget>
+       </item>
+      </layout>
+     </widget>
+    </item>
+    <item row="1" column="0">
+     <widget class="QGroupBox" name="groupBox_2">
+      <property name="title">
+       <string>车辆控制</string>
+      </property>
+      <layout class="QGridLayout" name="gridLayout_5">
+       <item row="2" column="0" colspan="3">
+        <layout class="QHBoxLayout" name="horizontalLayout_3">
+         <item>
+          <widget class="QLineEdit" name="lineEdit_2">
+           <property name="enabled">
+            <bool>true</bool>
+           </property>
+           <property name="sizePolicy">
+            <sizepolicy hsizetype="Expanding" vsizetype="Fixed">
+             <horstretch>0</horstretch>
+             <verstretch>0</verstretch>
+            </sizepolicy>
+           </property>
+           <property name="minimumSize">
+            <size>
+             <width>90</width>
+             <height>0</height>
+            </size>
+           </property>
+           <property name="maximumSize">
+            <size>
+             <width>90</width>
+             <height>16777215</height>
+            </size>
+           </property>
+           <property name="text">
+            <string>方向盘转角</string>
+           </property>
+          </widget>
+         </item>
+         <item>
+          <widget class="QDoubleSpinBox" name="doubleang_pos">
+           <property name="minimumSize">
+            <size>
+             <width>78</width>
+             <height>0</height>
+            </size>
+           </property>
+           <property name="decimals">
+            <number>1</number>
+           </property>
+           <property name="minimum">
+            <double>-430.000000000000000</double>
+           </property>
+           <property name="maximum">
+            <double>430.000000000000000</double>
+           </property>
+           <property name="singleStep">
+            <double>20.000000000000000</double>
+           </property>
+          </widget>
+         </item>
+         <item>
+          <widget class="QSpinBox" name="ang_pos">
+           <property name="enabled">
+            <bool>false</bool>
+           </property>
+           <property name="maximumSize">
+            <size>
+             <width>42</width>
+             <height>16777215</height>
+            </size>
+           </property>
+           <property name="minimum">
+            <number>-800</number>
+           </property>
+           <property name="maximum">
+            <number>800</number>
+           </property>
+           <property name="value">
+            <number>20</number>
+           </property>
+          </widget>
+         </item>
+         <item>
+          <widget class="QLineEdit" name="lineEdit_3">
+           <property name="enabled">
+            <bool>false</bool>
+           </property>
+           <property name="maximumSize">
+            <size>
+             <width>70</width>
+             <height>16777215</height>
+            </size>
+           </property>
+           <property name="text">
+            <string>刹车</string>
+           </property>
+          </widget>
+         </item>
+         <item>
+          <widget class="QSpinBox" name="ang_speed">
+           <property name="enabled">
+            <bool>false</bool>
+           </property>
+           <property name="maximumSize">
+            <size>
+             <width>42</width>
+             <height>16777215</height>
+            </size>
+           </property>
+           <property name="minimum">
+            <number>0</number>
+           </property>
+           <property name="maximum">
+            <number>500</number>
+           </property>
+           <property name="singleStep">
+            <number>1</number>
+           </property>
+           <property name="value">
+            <number>50</number>
+           </property>
+          </widget>
+         </item>
+         <item>
+          <spacer name="horizontalSpacer_3">
+           <property name="orientation">
+            <enum>Qt::Horizontal</enum>
+           </property>
+           <property name="sizeHint" stdset="0">
+            <size>
+             <width>40</width>
+             <height>20</height>
+            </size>
+           </property>
+          </spacer>
+         </item>
+        </layout>
+       </item>
+       <item row="0" column="0">
+        <widget class="QGroupBox" name="groupBox_6">
+         <property name="title">
+          <string>驾驶模式选择</string>
+         </property>
+         <layout class="QVBoxLayout" name="verticalLayout_5">
+          <item>
+           <widget class="QCheckBox" name="mode">
+            <property name="enabled">
+             <bool>false</bool>
+            </property>
+            <property name="text">
+             <string>驾驶模式使能</string>
+            </property>
+           </widget>
+          </item>
+          <item>
+           <widget class="QCheckBox" name="brkgas_en">
+            <property name="enabled">
+             <bool>false</bool>
+            </property>
+            <property name="text">
+             <string>纵向使能</string>
+            </property>
+           </widget>
+          </item>
+          <item>
+           <widget class="QCheckBox" name="eps_en">
+            <property name="enabled">
+             <bool>false</bool>
+            </property>
+            <property name="text">
+             <string>转向使能</string>
+            </property>
+           </widget>
+          </item>
+          <item>
+           <widget class="QCheckBox" name="gear_en">
+            <property name="enabled">
+             <bool>false</bool>
+            </property>
+            <property name="text">
+             <string>挡位使能</string>
+            </property>
+           </widget>
+          </item>
+         </layout>
+        </widget>
+       </item>
+       <item row="4" column="0" colspan="3">
+        <layout class="QHBoxLayout" name="horizontalLayout_15">
+         <item>
+          <widget class="QLabel" name="label_12">
+           <property name="enabled">
+            <bool>true</bool>
+           </property>
+           <property name="minimumSize">
+            <size>
+             <width>90</width>
+             <height>0</height>
+            </size>
+           </property>
+           <property name="maximumSize">
+            <size>
+             <width>90</width>
+             <height>16777215</height>
+            </size>
+           </property>
+           <property name="text">
+            <string>减速度</string>
+           </property>
+          </widget>
+         </item>
+         <item>
+          <widget class="QDoubleSpinBox" name="brake">
+           <property name="enabled">
+            <bool>true</bool>
+           </property>
+           <property name="minimumSize">
+            <size>
+             <width>78</width>
+             <height>0</height>
+            </size>
+           </property>
+           <property name="maximumSize">
+            <size>
+             <width>78</width>
+             <height>16777215</height>
+            </size>
+           </property>
+           <property name="minimum">
+            <double>-5.000000000000000</double>
+           </property>
+           <property name="maximum">
+            <double>0.000000000000000</double>
+           </property>
+           <property name="singleStep">
+            <double>0.200000000000000</double>
+           </property>
+           <property name="value">
+            <double>0.000000000000000</double>
+           </property>
+          </widget>
+         </item>
+         <item>
+          <widget class="QCheckBox" name="aeb_en">
+           <property name="enabled">
+            <bool>false</bool>
+           </property>
+           <property name="text">
+            <string>AEB使能</string>
+           </property>
+          </widget>
+         </item>
+         <item>
+          <widget class="QLabel" name="label_9">
+           <property name="enabled">
+            <bool>false</bool>
+           </property>
+           <property name="text">
+            <string>AEB减速度</string>
+           </property>
+          </widget>
+         </item>
+         <item>
+          <widget class="QDoubleSpinBox" name="aeb_acc">
+           <property name="enabled">
+            <bool>false</bool>
+           </property>
+           <property name="minimum">
+            <double>-16.000000000000000</double>
+           </property>
+           <property name="maximum">
+            <double>0.000000000000000</double>
+           </property>
+          </widget>
+         </item>
+         <item>
+          <spacer name="horizontalSpacer_2">
+           <property name="orientation">
+            <enum>Qt::Horizontal</enum>
+           </property>
+           <property name="sizeHint" stdset="0">
+            <size>
+             <width>40</width>
+             <height>20</height>
+            </size>
+           </property>
+          </spacer>
+         </item>
+        </layout>
+       </item>
+       <item row="0" column="3">
+        <widget class="QGroupBox" name="groupBox_4">
+         <property name="enabled">
+          <bool>true</bool>
+         </property>
+         <property name="title">
+          <string>手刹控制</string>
+         </property>
+         <layout class="QGridLayout" name="gridLayout_2">
+          <item row="0" column="0">
+           <layout class="QVBoxLayout" name="verticalLayout_2">
+            <item>
+             <widget class="QCheckBox" name="hand_brake">
+              <property name="enabled">
+               <bool>false</bool>
+              </property>
+              <property name="text">
+               <string>手刹</string>
+              </property>
+              <property name="checked">
+               <bool>true</bool>
+              </property>
+              <attribute name="buttonGroup">
+               <string notr="true">EPB_buttonGroup</string>
+              </attribute>
+             </widget>
+            </item>
+            <item>
+             <widget class="QCheckBox" name="push_hand_brake">
+              <property name="enabled">
+               <bool>false</bool>
+              </property>
+              <property name="text">
+               <string>松手刹</string>
+              </property>
+              <attribute name="buttonGroup">
+               <string notr="true">EPB_buttonGroup</string>
+              </attribute>
+             </widget>
+            </item>
+           </layout>
+          </item>
+         </layout>
+        </widget>
+       </item>
+       <item row="0" column="2">
+        <widget class="QGroupBox" name="groupBox_7">
+         <property name="enabled">
+          <bool>true</bool>
+         </property>
+         <property name="title">
+          <string>发动机车门锁控制</string>
+         </property>
+         <layout class="QGridLayout" name="gridLayout_4">
+          <item row="0" column="0">
+           <layout class="QVBoxLayout" name="verticalLayout_4">
+            <item>
+             <widget class="QCheckBox" name="engine">
+              <property name="enabled">
+               <bool>false</bool>
+              </property>
+              <property name="text">
+               <string>点火</string>
+              </property>
+              <attribute name="buttonGroup">
+               <string notr="true">engineer_buttonGroup</string>
+              </attribute>
+             </widget>
+            </item>
+            <item>
+             <widget class="QCheckBox" name="stop_engine">
+              <property name="enabled">
+               <bool>false</bool>
+              </property>
+              <property name="text">
+               <string>熄火</string>
+              </property>
+              <property name="checked">
+               <bool>true</bool>
+              </property>
+              <attribute name="buttonGroup">
+               <string notr="true">engineer_buttonGroup</string>
+              </attribute>
+             </widget>
+            </item>
+            <item>
+             <widget class="QCheckBox" name="door_lock">
+              <property name="enabled">
+               <bool>false</bool>
+              </property>
+              <property name="text">
+               <string>车门上锁</string>
+              </property>
+             </widget>
+            </item>
+           </layout>
+          </item>
+         </layout>
+        </widget>
+       </item>
+       <item row="0" column="1">
+        <widget class="QGroupBox" name="groupBox_3">
+         <property name="enabled">
+          <bool>true</bool>
+         </property>
+         <property name="title">
+          <string>挡位控制</string>
+         </property>
+         <layout class="QVBoxLayout" name="verticalLayout">
+          <item>
+           <widget class="QCheckBox" name="gear_D">
+            <property name="enabled">
+             <bool>false</bool>
+            </property>
+            <property name="text">
+             <string>D</string>
+            </property>
+            <attribute name="buttonGroup">
+             <string notr="true">gear_buttonGroup</string>
+            </attribute>
+           </widget>
+          </item>
+          <item>
+           <widget class="QCheckBox" name="gear_N">
+            <property name="enabled">
+             <bool>false</bool>
+            </property>
+            <property name="text">
+             <string>N</string>
+            </property>
+            <attribute name="buttonGroup">
+             <string notr="true">gear_buttonGroup</string>
+            </attribute>
+           </widget>
+          </item>
+          <item>
+           <widget class="QCheckBox" name="gear_R">
+            <property name="enabled">
+             <bool>false</bool>
+            </property>
+            <property name="text">
+             <string>R</string>
+            </property>
+            <attribute name="buttonGroup">
+             <string notr="true">gear_buttonGroup</string>
+            </attribute>
+           </widget>
+          </item>
+          <item>
+           <widget class="QCheckBox" name="gear_P">
+            <property name="enabled">
+             <bool>false</bool>
+            </property>
+            <property name="text">
+             <string>P</string>
+            </property>
+            <property name="checked">
+             <bool>true</bool>
+            </property>
+            <attribute name="buttonGroup">
+             <string notr="true">gear_buttonGroup</string>
+            </attribute>
+           </widget>
+          </item>
+         </layout>
+        </widget>
+       </item>
+       <item row="3" column="0" colspan="3">
+        <layout class="QHBoxLayout" name="horizontalLayout_6">
+         <item>
+          <widget class="QLabel" name="label_acc">
+           <property name="sizePolicy">
+            <sizepolicy hsizetype="Fixed" vsizetype="Preferred">
+             <horstretch>0</horstretch>
+             <verstretch>0</verstretch>
+            </sizepolicy>
+           </property>
+           <property name="minimumSize">
+            <size>
+             <width>90</width>
+             <height>0</height>
+            </size>
+           </property>
+           <property name="maximumSize">
+            <size>
+             <width>90</width>
+             <height>20</height>
+            </size>
+           </property>
+           <property name="text">
+            <string>加速扭矩</string>
+           </property>
+          </widget>
+         </item>
+         <item>
+          <widget class="QDoubleSpinBox" name="targe_acc">
+           <property name="sizePolicy">
+            <sizepolicy hsizetype="Minimum" vsizetype="Minimum">
+             <horstretch>0</horstretch>
+             <verstretch>0</verstretch>
+            </sizepolicy>
+           </property>
+           <property name="minimumSize">
+            <size>
+             <width>78</width>
+             <height>0</height>
+            </size>
+           </property>
+           <property name="maximumSize">
+            <size>
+             <width>78</width>
+             <height>20</height>
+            </size>
+           </property>
+           <property name="decimals">
+            <number>0</number>
+           </property>
+           <property name="minimum">
+            <double>0.000000000000000</double>
+           </property>
+           <property name="maximum">
+            <double>1000.000000000000000</double>
+           </property>
+           <property name="singleStep">
+            <double>10.000000000000000</double>
+           </property>
+           <property name="value">
+            <double>0.000000000000000</double>
+           </property>
+          </widget>
+         </item>
+         <item>
+          <widget class="QLabel" name="label_8">
+           <property name="enabled">
+            <bool>false</bool>
+           </property>
+           <property name="text">
+            <string>限速</string>
+           </property>
+          </widget>
+         </item>
+         <item>
+          <widget class="QSpinBox" name="limit_spd">
+           <property name="enabled">
+            <bool>false</bool>
+           </property>
+           <property name="maximum">
+            <number>255</number>
+           </property>
+          </widget>
+         </item>
+         <item>
+          <spacer name="horizontalSpacer">
+           <property name="orientation">
+            <enum>Qt::Horizontal</enum>
+           </property>
+           <property name="sizeHint" stdset="0">
+            <size>
+             <width>40</width>
+             <height>20</height>
+            </size>
+           </property>
+          </spacer>
+         </item>
+        </layout>
+       </item>
+       <item row="1" column="0" colspan="4">
+        <widget class="QGroupBox" name="groupBox_5">
+         <property name="enabled">
+          <bool>true</bool>
+         </property>
+         <property name="title">
+          <string>声光控制</string>
+         </property>
+         <layout class="QGridLayout" name="gridLayout_3">
+          <item row="0" column="0">
+           <widget class="QCheckBox" name="left_turn">
+            <property name="text">
+             <string>左转向</string>
+            </property>
+            <property name="checked">
+             <bool>false</bool>
+            </property>
+            <property name="tristate">
+             <bool>false</bool>
+            </property>
+           </widget>
+          </item>
+          <item row="0" column="4">
+           <widget class="QCheckBox" name="airc_ac">
+            <property name="enabled">
+             <bool>false</bool>
+            </property>
+            <property name="text">
+             <string>空调AC</string>
+            </property>
+           </widget>
+          </item>
+          <item row="0" column="2">
+           <widget class="QCheckBox" name="speaker">
+            <property name="enabled">
+             <bool>false</bool>
+            </property>
+            <property name="text">
+             <string>喇叭</string>
+            </property>
+           </widget>
+          </item>
+          <item row="0" column="5">
+           <widget class="QCheckBox" name="window">
+            <property name="enabled">
+             <bool>false</bool>
+            </property>
+            <property name="text">
+             <string>车窗上升</string>
+            </property>
+           </widget>
+          </item>
+          <item row="0" column="1">
+           <widget class="QCheckBox" name="pos_light">
+            <property name="enabled">
+             <bool>false</bool>
+            </property>
+            <property name="text">
+             <string>位置灯</string>
+            </property>
+           </widget>
+          </item>
+          <item row="1" column="4">
+           <widget class="QCheckBox" name="airc_recirc">
+            <property name="enabled">
+             <bool>false</bool>
+            </property>
+            <property name="text">
+             <string>空调内循环</string>
+            </property>
+           </widget>
+          </item>
+          <item row="1" column="0">
+           <widget class="QCheckBox" name="right_turn">
+            <property name="text">
+             <string>右转向</string>
+            </property>
+           </widget>
+          </item>
+          <item row="1" column="1">
+           <widget class="QCheckBox" name="dipped_light">
+            <property name="enabled">
+             <bool>false</bool>
+            </property>
+            <property name="text">
+             <string>近光灯</string>
+            </property>
+           </widget>
+          </item>
+          <item row="1" column="2">
+           <widget class="QCheckBox" name="frog">
+            <property name="enabled">
+             <bool>false</bool>
+            </property>
+            <property name="text">
+             <string>雾灯</string>
+            </property>
+           </widget>
+          </item>
+          <item row="2" column="2">
+           <widget class="QCheckBox" name="wiper">
+            <property name="enabled">
+             <bool>false</bool>
+            </property>
+            <property name="text">
+             <string>雨刮</string>
+            </property>
+           </widget>
+          </item>
+          <item row="2" column="1">
+           <widget class="QCheckBox" name="high_beam">
+            <property name="enabled">
+             <bool>false</bool>
+            </property>
+            <property name="text">
+             <string>远光灯</string>
+            </property>
+           </widget>
+          </item>
+          <item row="2" column="0">
+           <widget class="QCheckBox" name="flicker">
+            <property name="enabled">
+             <bool>false</bool>
+            </property>
+            <property name="text">
+             <string>双闪</string>
+            </property>
+           </widget>
+          </item>
+          <item row="2" column="4">
+           <widget class="QCheckBox" name="airc_auto">
+            <property name="enabled">
+             <bool>false</bool>
+            </property>
+            <property name="text">
+             <string>空调auto</string>
+            </property>
+           </widget>
+          </item>
+          <item row="3" column="4">
+           <widget class="QCheckBox" name="airc_off">
+            <property name="enabled">
+             <bool>false</bool>
+            </property>
+            <property name="text">
+             <string>空调off</string>
+            </property>
+           </widget>
+          </item>
+          <item row="3" column="2">
+           <widget class="QCheckBox" name="brake_light">
+            <property name="enabled">
+             <bool>false</bool>
+            </property>
+            <property name="text">
+             <string>制动灯</string>
+            </property>
+           </widget>
+          </item>
+          <item row="3" column="1">
+           <widget class="QCheckBox" name="defrost">
+            <property name="enabled">
+             <bool>false</bool>
+            </property>
+            <property name="text">
+             <string>除霜</string>
+            </property>
+           </widget>
+          </item>
+          <item row="3" column="0">
+           <widget class="QCheckBox" name="reverse_light">
+            <property name="enabled">
+             <bool>false</bool>
+            </property>
+            <property name="text">
+             <string>倒车灯</string>
+            </property>
+           </widget>
+          </item>
+         </layout>
+        </widget>
+       </item>
+       <item row="3" column="3">
+        <widget class="QLabel" name="label">
+         <property name="enabled">
+          <bool>true</bool>
+         </property>
+         <property name="minimumSize">
+          <size>
+           <width>0</width>
+           <height>17</height>
+          </size>
+         </property>
+         <property name="text">
+          <string>当前车速(km/h)</string>
+         </property>
+         <property name="alignment">
+          <set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter</set>
+         </property>
+        </widget>
+       </item>
+       <item row="4" column="3">
+        <widget class="QLabel" name="Vehicle_Speed">
+         <property name="enabled">
+          <bool>true</bool>
+         </property>
+         <property name="sizePolicy">
+          <sizepolicy hsizetype="Preferred" vsizetype="Fixed">
+           <horstretch>0</horstretch>
+           <verstretch>0</verstretch>
+          </sizepolicy>
+         </property>
+         <property name="minimumSize">
+          <size>
+           <width>0</width>
+           <height>17</height>
+          </size>
+         </property>
+         <property name="maximumSize">
+          <size>
+           <width>16777215</width>
+           <height>50</height>
+          </size>
+         </property>
+         <property name="mouseTracking">
+          <bool>false</bool>
+         </property>
+         <property name="tabletTracking">
+          <bool>false</bool>
+         </property>
+         <property name="autoFillBackground">
+          <bool>false</bool>
+         </property>
+         <property name="styleSheet">
+          <string notr="true"/>
+         </property>
+         <property name="frameShape">
+          <enum>QFrame::Box</enum>
+         </property>
+         <property name="text">
+          <string>0</string>
+         </property>
+         <property name="alignment">
+          <set>Qt::AlignCenter</set>
+         </property>
+        </widget>
+       </item>
+      </layout>
+     </widget>
+    </item>
+   </layout>
+  </widget>
+  <widget class="QMenuBar" name="menuBar">
+   <property name="geometry">
+    <rect>
+     <x>0</x>
+     <y>0</y>
+     <width>965</width>
+     <height>22</height>
+    </rect>
+   </property>
+  </widget>
+  <widget class="QToolBar" name="mainToolBar">
+   <attribute name="toolBarArea">
+    <enum>TopToolBarArea</enum>
+   </attribute>
+   <attribute name="toolBarBreak">
+    <bool>false</bool>
+   </attribute>
+  </widget>
+  <widget class="QStatusBar" name="statusBar"/>
+ </widget>
+ <layoutdefault spacing="6" margin="11"/>
+ <resources>
+  <include location="resources.qrc"/>
+ </resources>
+ <connections/>
+ <buttongroups>
+  <buttongroup name="EPB_buttonGroup"/>
+  <buttongroup name="gear_buttonGroup"/>
+  <buttongroup name="engineer_buttonGroup"/>
+ </buttongroups>
+</ui>

src/chassis_test/shenlan_test/mainwindowYUAN.ui

@@ -0,0 +1,82 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<ui version="4.0">
+ <class>MainWindow</class>
+ <widget class="QMainWindow" name="MainWindow">
+  <property name="geometry">
+   <rect>
+    <x>0</x>
+    <y>0</y>
+    <width>400</width>
+    <height>300</height>
+   </rect>
+  </property>
+  <property name="windowTitle">
+   <string>MainWindow</string>
+  </property>
+  <widget class="QWidget" name="centralWidget">
+   <widget class="QLineEdit" name="lineEdit">
+    <property name="geometry">
+     <rect>
+      <x>10</x>
+      <y>70</y>
+      <width>81</width>
+      <height>25</height>
+     </rect>
+    </property>
+    <property name="autoFillBackground">
+     <bool>false</bool>
+    </property>
+    <property name="inputMethodHints">
+     <set>Qt::ImhNone</set>
+    </property>
+    <property name="text">
+     <string>方向盘转角</string>
+    </property>
+    <property name="readOnly">
+     <bool>true</bool>
+    </property>
+   </widget>
+   <widget class="QSpinBox" name="EPS_spinBox">
+    <property name="geometry">
+     <rect>
+      <x>100</x>
+      <y>70</y>
+      <width>61</width>
+      <height>26</height>
+     </rect>
+    </property>
+    <property name="minimum">
+     <number>-430</number>
+    </property>
+    <property name="maximum">
+     <number>430</number>
+    </property>
+    <property name="singleStep">
+     <number>10</number>
+    </property>
+   </widget>
+  </widget>
+  <widget class="QMenuBar" name="menuBar">
+   <property name="geometry">
+    <rect>
+     <x>0</x>
+     <y>0</y>
+     <width>400</width>
+     <height>22</height>
+    </rect>
+   </property>
+  </widget>
+  <widget class="QToolBar" name="mainToolBar">
+   <attribute name="toolBarArea">
+    <enum>TopToolBarArea</enum>
+   </attribute>
+   <attribute name="toolBarBreak">
+    <bool>false</bool>
+   </attribute>
+  </widget>
+  <widget class="QStatusBar" name="statusBar"/>
+ </widget>
+ <layoutdefault spacing="6" margin="11"/>
+ <resources/>
+ <connections/>
+</ui>

src/chassis_test/shenlan_test/resources.qrc

@@ -0,0 +1,6 @@
+<RCC>
+    <qresource prefix="/new/prefix1">
+        <file>background4.png</file>
+        <file>adc.ico</file>
+    </qresource>
+</RCC>

src/chassis_test/shenlan_test/shenlan_test.pro

@@ -0,0 +1,59 @@
+#-------------------------------------------------
+#
+# Project created by QtCreator 2023-04-26T14:20:55
+#
+#-------------------------------------------------
+
+QT       += core gui
+
+greaterThan(QT_MAJOR_VERSION, 4): QT += widgets
+
+QT += network dbus xml widgets
+
+CONFIG += c++11 console
+CONFIG -= app_bundle
+
+TARGET = shenlan_test
+TEMPLATE = app
+
+
+# The following define makes your compiler emit warnings if you use
+# any feature of Qt which has been marked as deprecated (the exact warnings
+# depend on your compiler). Please consult the documentation of the
+# deprecated API in order to know how to port your code away from it.
+DEFINES += QT_DEPRECATED_WARNINGS
+
+# You can also make your code fail to compile if you use deprecated APIs.
+# In order to do so, uncomment the following line.
+# You can also select to disable deprecated APIs only up to a certain version of Qt.
+#DEFINES += QT_DISABLE_DEPRECATED_BEFORE=0x060000    # disables all the APIs deprecated before Qt 6.0.0
+
+INCLUDEPATH += $$PWD/../common
+
+INCLUDEPATH += $$PWD/../../include/msgtype
+
+LIBS += -lprotobuf
+
+!include(../../../include/common.pri ) {
+    error( "Couldn't find the common.pri file!" )
+}
+
+unix:!macx: INCLUDEPATH += $$PWD/.
+unix:!macx: DEPENDPATH += $$PWD/.
+
+SOURCES += \
+        main.cpp \
+        mainwindow.cpp\
+        ../../include/msgtype/decition.pb.cc \
+        ../../include/msgtype/chassis.pb.cc \
+
+HEADERS += \
+        mainwindow.h\
+        ../../include/msgtype/decition.pb.h \
+        ../../include/msgtype/chassis.pb.h \
+
+FORMS += \
+        mainwindow.ui
+
+RESOURCES += \
+    resources.qrc

src/common/common/xodr/OpenDrive/OpenDriveXmlParser.cpp

@@ -3168,7 +3168,7 @@ bool OpenDriveXmlParser::ReadFile(std::string fileName)
 	}
 	
 	//failed to read the file
-	cout<<"Could not read file: "<<fileName<<endl;
+//	cout<<"Could not read file: "<<fileName<<endl;
 	return false;
 }
 //--------------

src/common/common/xodr/xodrfunc/xodrfunc.cpp

@@ -224,6 +224,8 @@ double xodrfunc::GetArcDis(GeometryArc * parc,double x,double y,double & nearx,
 
     double hdgltoa = CalcHdg(QPointF(x,y),QPointF(x_center,y_center));
 
+//    double hdgltoa = CalcHdg(QPointF(x_center,y_center),QPointF(x,y));
+
 
     QPointF arcpoint;
     arcpoint.setX(x_center);arcpoint.setY(y_center);

src/common/ivbacktrace/ivbacktrace.cpp

@@ -274,12 +274,12 @@ void out_stack(char *sig)
 
 void RegisterIVBackTrace()
 {
-    signal(SIGHUP, signal_exit);
-    signal(SIGINT, signal_exit);
-    signal(SIGQUIT, signal_exit);
+//    signal(SIGHUP, signal_exit);
+//    signal(SIGINT, signal_exit);
+//    signal(SIGQUIT, signal_exit);
     signal(SIGABRT, signal_exit);
-    signal(SIGKILL, signal_exit);
-    signal(SIGTERM, signal_exit);
+//    signal(SIGKILL, signal_exit);
+//    signal(SIGTERM, signal_exit);
     signal(SIGSEGV, signal_exit);
 }
 

src/common/modulecomm/modulecomm.pro

@@ -27,7 +27,7 @@ QT += dbus
 DEFINES += USEDBUS
 }
 
-
+QMAKE_CXXFLAGS +=  -g
 
 CONFIG += c++11
 

src/common/modulecomm/shm/procsm.cpp

@@ -124,13 +124,18 @@ procsm::procsm(const char * strsmname,const unsigned int nBufSize,const unsigned
     mpASMPtr = new QSharedMemory(strsmname);
     mpthreadattmon = new std::thread(&procsm::threadAttachMon,this);
 
+//
+
+//    savefatalerror("procsm");
     char strasmname[300];
+    snprintf(strasmname,255,"%s_%lld",strsmname,std::chrono::system_clock::now().time_since_epoch().count());
 
 
     mnMode = nMode;
     if(nMode == ModeWrite)
     {
         int nrtn = CreateASMPTR(strasmname,nBufSize,nMaxPacCount);
+//        int nrtn = CreateASMPTR(mstrsmname,nBufSize,nMaxPacCount);
         if(nrtn<0)
         {
             std::cout<<"CreateASMPTR Fail."<<std::endl;
@@ -143,11 +148,15 @@ procsm::procsm(const char * strsmname,const unsigned int nBufSize,const unsigned
         return;
     }
 
+
+
 //    mpASM = new QSharedMemory(strsmname);
     mpASM = new QSharedMemory(strasmname);
 
     if(nMode == ModeWrite)
     {
+
+
         strncpy(mmodulemsg_type.mstrmsgidname,strsmname,255);
         mmodulemsg_type.mnBufSize = nBufSize;
         mmodulemsg_type.mnMsgBufCount = nMaxPacCount;
@@ -164,6 +173,7 @@ procsm::procsm(const char * strsmname,const unsigned int nBufSize,const unsigned
 #endif
 #endif
 
+        std::cout<<" create asm impl name: "<<strasmname<<std::endl;
         int nrtn = CreateAndAttachASM(strasmname,nBufSize,nMaxPacCount,strsmname);
         if(nrtn <0 )
         {
@@ -172,6 +182,7 @@ procsm::procsm(const char * strsmname,const unsigned int nBufSize,const unsigned
             ivstdcolorout(strerr,iv::STDCOLOR_BOLDRED);
         }
 
+
     }
 
     mbInitComplete = true;
@@ -225,7 +236,10 @@ int procsm::CreateASMPTR(char * strasmname,const unsigned int nBufSize,const uns
         }
         else
         {
-            std::cout<<"Attach Fail."<<std::endl;
+            char strout[256];
+            snprintf(strout,256,"Fatal Error. Create %s, attach fail. %s is exist,but attach fail.",mstrsmname,mstrsmname);
+            std::cout<<strout<<std::endl;
+            savefatalerror(strout);
             return -1;
         }
     }
@@ -233,7 +247,10 @@ int procsm::CreateASMPTR(char * strasmname,const unsigned int nBufSize,const uns
     ASM_PTR * pasm = (ASM_PTR *)mpASMPtr->data();
     if(pasm == NULL)
     {
-        std::cout<<"ASM_PTR is NULL."<<std::endl;
+        char strout[256];
+        snprintf(strout,256,"%s  pasm is NULL.",mstrsmname);
+        std::cout<<strout<<std::endl;
+        savefatalerror(strout);
         return -1;
     }
     if(bSMExit == false)
@@ -276,8 +293,10 @@ int procsm::CreateAndAttachASM(char * strasmname,const unsigned int nBufSize,con
             char * p = (char *)mpASM->data();
             if(p == NULL)
             {
-                strout = "Fatal Error: Create " + std::string(strsmname) + " Fail. data is NULL";
-                ivstdcolorout(strout,iv::STDCOLOR_BOLDRED);
+                char strout2[256];
+                snprintf(strout2,256,"Fatal Error: Exist, attach ok, but data is NULL",strsmname);
+                savefatalerror(strout2);
+                ivstdcolorout(strout2,iv::STDCOLOR_BOLDRED);
                 return -1;
             }
             mpinfo = (procsm_info *)p;
@@ -302,7 +321,10 @@ int procsm::CreateAndAttachASM(char * strasmname,const unsigned int nBufSize,con
                 char * p = (char *)mpASM->data();
                 if(p == NULL)
                 {
-                    strout = "Fatal Error: ReCreate " + std::string(strsmname) + " Fail. data is NULL";
+                    strout = "Fatal Error: ReCreate " + std::string(strsmname) + " OK. but data is NULL";
+                    char strout2[256];
+                    snprintf(strout2,256,"%s",strout.data());
+                    savefatalerror(strout2);
                     ivstdcolorout(strout,iv::STDCOLOR_BOLDRED);
                     return -2;
                 }
@@ -319,7 +341,10 @@ int procsm::CreateAndAttachASM(char * strasmname,const unsigned int nBufSize,con
             }
             else
             {
-                strout = "Fatal Error: ReCreate " + std::string(strsmname) + " Fail.";
+                strout = "Fatal Error: Exist,attach fail, so  ReCreate " + std::string(strsmname) + " ,but Fail.";
+                char strout2[256];
+                snprintf(strout2,256,"%s",strout.data());
+                savefatalerror(strout2);
                 ivstdcolorout(strout,iv::STDCOLOR_BOLDRED);
                 return -5;
             }
@@ -333,6 +358,9 @@ int procsm::CreateAndAttachASM(char * strasmname,const unsigned int nBufSize,con
         {
             strout = "Fatal Error.Create SharedMemory Fail. " + std::string(strsmname) + " not exist. successfully create it, but data is NULL";
             ivstdcolorout(strout,iv::STDCOLOR_BOLDRED);
+            char strout2[256];
+            snprintf(strout2,256,"%s",strout.data());
+            savefatalerror(strout2);
             return -3;
         }
         mpASM->lock();
@@ -379,6 +407,9 @@ int procsm::CreateAndAttachASM(char * strasmname,const unsigned int nBufSize,con
         mbAttach = false;
         strout = "Fatal Error.Create SharedMemory Fail. " + std::string(strsmname) + " Attach Fail.";
         ivstdcolorout(strout,iv::STDCOLOR_BOLDRED);
+        char strout2[256];
+        snprintf(strout2,256,"%s",strout.data());
+        savefatalerror(strout2);
         return -4;
     }
 
@@ -527,6 +558,7 @@ bool procsm::AttachMem()
                     char strerr[256];
                     snprintf(strerr,256,"AttachMem  CreateAndAttachASMFail. error code: %d",nrtn);
                     ivstdcolorout(strerr,iv::STDCOLOR_BOLDRED);
+                    savefatalerror(strerr);
                     return false;
                 }
                 else
@@ -535,6 +567,11 @@ bool procsm::AttachMem()
                 }
 
             }
+            else
+            {
+                mpinfo = NULL;
+                mphead = NULL;
+            }
 
 
             return false;
@@ -545,8 +582,9 @@ bool procsm::AttachMem()
         if(mnMode == ModeWrite)
         {
             char strout[256];
-            snprintf(strout,256,"%s  Attach ASMPtr Fail.",mstrsmname);
+            snprintf(strout,256," when AttachMem, %s  Attach ASMPtr Fail.",mstrsmname);
             ivstdcolorout(" AttachMem: Attach Fail. ",iv::STDCOLOR_BOLDYELLOW);
+            savefatalerror(strout);
         }
         return false;
     }
@@ -648,13 +686,30 @@ int procsm::writemsg(const char *str, const unsigned int nSize)
         }
         else
         {
+            static bool bsave = false;
+            if(bsave == false)
+            {
+                char strout[256];
+                snprintf(strout,256,"%s procsm::writemsg message size is very big.msg size is %d ",mstrsmname,nSize);
+                savefatalerror(strout);
+                bsave = true;
+            }
+
         qDebug("procsm::writemsg message size is very big");
         return -1;
         }
     }
     if(mbAttach == false)
     {
-        std::cout<<mstrsmname<<"ShareMemory Attach fail."<<std::endl;
+        static bool bsave = false;
+        if(bsave == false)
+        {
+            char strout[256];
+            snprintf(strout,256,"%s writemsg, ShareMemory Attach fail. ",mstrsmname);
+            savefatalerror(strout);
+            bsave = true;
+        }
+        std::cout<<mstrsmname<<"  ShareMemory Attach fail."<<std::endl;
         return -1;
     }
     mpASM->lock();
@@ -758,6 +813,12 @@ unsigned int procsm::getcurrentnext()
 {
 
     checkasm();
+
+    if(mpinfo == NULL)
+    {
+        std::cout<<"procsm::getcurrentnext attach asm fail.  "<<std::endl;
+        return 0;
+    }
     unsigned int nNext;
     mpASM->lock();
     nNext = mpinfo->mNext;
@@ -841,7 +902,17 @@ int procsm::readmsg(unsigned int index, char *str, unsigned int nMaxSize,unsigne
           //         qDebug("read pos = %d",(phh+nPos)->mnPos);
                    nRtn = (phh+nPos)->mnLen;
   //                 pdt->setDate(QDate(2030,1,1));
+                   const int nms_msgkeep = 30000; //message must in 3 seconds read.
                    pdt->setMSecsSinceEpoch((phh+nPos)->sendtime/1000000);
+
+                   int64_t nms_now = std::chrono::system_clock::now().time_since_epoch().count()/1000000;
+                   int64_t nms_send = (phh+nPos)->sendtime/1000000;
+                   if(abs(nms_now - nms_send)> nms_msgkeep)
+                   {
+                       nRtn = 0;
+                       std::cout<<" message very old. skip it. diff is: "<<(nms_now - nms_send)<<std::endl;
+                   }
+
 //                   (phh+nPos)->GetDate(pdt);
            //        memcpy(pdt,&((phh+nPos)->mdt),sizeof(QDateTime));
                 }
@@ -853,3 +924,36 @@ int procsm::readmsg(unsigned int index, char *str, unsigned int nMaxSize,unsigne
     return nRtn;
 }
 
+
+void procsm::savefatalerror(char * strerr)
+{
+
+    char strdirname[256];
+    snprintf(strdirname,256,"%s/log",getenv("HOME"));
+    QDir xDir(strdirname);
+    if(!xDir.exists())
+    {
+        xDir.mkdir(strdirname);
+    }
+
+    QFile xFile;
+    int64_t nNow = std::chrono::system_clock::now().time_since_epoch().count();
+    char strfilename[256];
+    snprintf(strfilename,256,"%s/modulecomm_%ld.log",strdirname,nNow);
+
+
+    xFile.setFileName(strfilename);
+    if(xFile.open(QIODevice::ReadWrite))
+    {
+        xFile.write(strerr);
+        xFile.flush();
+    }
+    else
+    {
+        char strout[256];
+        snprintf(strout,256,"save error to file %s fail.",strfilename);
+        ivstdcolorout(strout,iv::STDCOLOR_BOLDRED);
+    }
+
+}
+

src/common/modulecomm/shm/procsm.h

@@ -156,6 +156,8 @@ private:
     void setfailmark();
     bool checkfailmark(); //if have fail mark file,return true
 
+    void savefatalerror(char * strerr);
+
 
 
 };

src/common/modulecomm/shm/procsm_if.cpp

@@ -163,10 +163,12 @@ void procsm_if_readthread::run()
                 if((nRtn == -2) ||(nRtn == -100)||(nRtn == -101))
                 {
                    index = mpPSM->getcurrentnext();
-                   if(nRtn == -100)  //Because New ShareMemory, read this.
-                   {
-                       if((index > 0)&&(bFirstRead == false) )index = index -1;
-                   }
+
+                   //comment next code, skip some message, when change asm.
+//                   if(nRtn == -100)  //Because New ShareMemory, read this.
+//                   {
+//                       if((index > 0)&&(bFirstRead == false) )index = index -1;
+//                   }
                 }
                 else
                 {

src/common/ndt_cpu/ndt_cpu.pro

@@ -25,6 +25,7 @@ unix:INCLUDEPATH += /usr/include/pcl-1.8
 unix:INCLUDEPATH += /usr/include/pcl-1.7
 
 unix:INCLUDEPATH += /usr/include/pcl-1.12
+unix:INCLUDEPATH += /usr/include/pcl-1.10
 
 
 #PKGCONFIG +=pcl

src/common/ndt_gpu/ndt_gpu.pro

@@ -20,9 +20,11 @@ QMAKE_CXXFLAGS += -std=gnu++17
 
 SOURCES +=  \
 
+#INCLUDEPATH += /usr/local/cuda-10.2/targets/aarch64-linux/include/crt
 #INCLUDEPATH += /home/nvidia/eigentem
 unix:INCLUDEPATH += /usr/include/eigen3
 unix:INCLUDEPATH += /usr/include/pcl-1.8
+unix:INCLUDEPATH += /usr/include/pcl-1.10
 
 unix:LIBS += -lboost_thread -lboost_system
 
@@ -53,15 +55,15 @@ LIBS += -L"/usr/local/lib" \
         -lcudart \
         -lcufft
 
-CUDA_SDK = "/usr/local/cuda-10.2/"   # cudaSDK路径
+CUDA_SDK = "/usr/local/cuda/"   # cudaSDK路径
 
-CUDA_DIR = "/usr/local/cuda-10.2/"            # CUDA tookit路径
+CUDA_DIR = "/usr/local/cuda/"            # CUDA tookit路径
 
 SYSTEM_NAME = linux         # 自己系统环境 'Win32', 'x64', or 'Win64'
 
 SYSTEM_TYPE = 64           #操作系统位数 '32' or '64',
 
-CUDA_ARCH = sm_72          # cuda架构, for example 'compute_10', 'compute_11', 'sm_10'
+CUDA_ARCH = sm_72  #sm_72  # xavier 72  orin 87          # cuda架构, for example 'compute_10', 'compute_11', 'sm_10'
 
 NVCC_OPTIONS = --use_fast_math --compiler-options "-fPIC"
 

src/controller/controller_changan_shenlan_v2/controller_changan_shenlan_v2.pro

@@ -40,7 +40,7 @@ include($$PWD/control/control.pri)
 INCLUDEPATH += $$PWD/../controllercommon
 
 
-#DEFINES += TORQUEBRAKETEST
+DEFINES += TORQUEBRAKETEST
 
 #unix:!macx: LIBS += -L/home/yuchuli/qt/lib -lncomgnss -ladcmemshare
 

src/controller/controller_changan_shenlan_v2/main.cpp

@@ -20,34 +20,45 @@
 
 #include <thread>
 
-void * gpacansend;
-void * gpadecition;
-void * gpachassis;
+#ifdef Q_OS_LINUX
+#include <signal.h>
+#endif
+
+static void * gpacansend;
+static void * gpadecition;
+static void * gpachassis;
 
-std::string gstrmemdecition;
-std::string gstrmemcansend;
-std::string gstrmemchassis;
-bool gbSendRun = true;
+static std::string gstrmemdecition;
+static std::string gstrmemcansend;
+static std::string gstrmemchassis;
+static bool gbSendRun = true;
 
-bool gbChassisEPS = false;
+static bool gbChassisEPS = false;
 
-iv::brain::decition gdecition_def;
-iv::brain::decition gdecition;
+static iv::brain::decition gdecition_def;
+static iv::brain::decition gdecition;
 
-QTime gTime;
-int gnLastSendTime = 0;
-int gnLastRecvDecTime = -1000;
-int gnDecitionNum = 0; //when is zero,send default;
+static QTime gTime;
+static int gnLastSendTime = 0;
+static int gnLastRecvDecTime = -1000;
+static int gnDecitionNum = 0; //when is zero,send default;
 const int gnDecitionNumMax = 100;
-int gnIndex = 0;
+static int gnIndex = 0;
 
 static bool gbHaveVehSpd = false;
 static double gfVehSpd = 0.0;
 
-boost::shared_ptr<iv::control::Controller> gcontroller;	//实际车辆控制器
+static bool gbHaveWheelAngle = false;
+static double gfWheelAngle = 0.0;
+
+static double gfWheelSpeedLim = 300; //300 degrees per second
+
+static boost::shared_ptr<iv::control::Controller> gcontroller;	//实际车辆控制器
 
 static QMutex gMutex;
 
+static std::thread * gpsendthread = NULL;
+
 
 // signal: @ACC_LatAngReq    //更改CANid
 #define ECU_1C4_ACC_LatAngReq_CovFactor (0.1)
@@ -108,29 +119,53 @@ typedef struct
 
 typedef struct
 {
+    uint8_t CdcDoor;
+    uint8_t res1;
+    uint8_t res2;
     uint8_t ADS_UDLCTurnLightReq;
-} ECU_36E_t;
+} ECU_25E_t;  //zhuanxiangdeng IDgenghuan
 
 unsigned char byte_1C4[64];//byte_144[8];
 unsigned char byte_24E[64];
-unsigned char byte_36E[64];
+unsigned char byte_25E[32];
 
 ECU_1C4_t _m1C4 = {0,0,0,0};
 ECU_24E_t _m24E = {0,0,0,0,0,0,0,0,0,0,0};
-ECU_36E_t _m36E = {0};
+ECU_25E_t _m25E = {0,0,0,0};
 
 void ExecSend();
 
 void executeDecition(const iv::brain::decition &decition)
 {
 
+
+
+
     static int xieya = 50;
 //     std::cout<<"acc is "<<decition.torque()<<" ang is "<<decition.wheelangle()<<std::endl;
 //     std::cout<<"angle_mode is "<<decition.angle_mode()<<" angle_active is "<<decition.angle_active()<<std::endl;
 //     std::cout<<"brake_type is "<<decition.brake_type()<<" acc_active is "<<decition.acc_active()<<std::endl;
 //     std::cout<<"brake is "<<decition.brake()<<" brake_active is "<<decition.brake_active()<<std::endl;
 //     std::cout<<"auto_mode is "<<decition.auto_mode()<<" rightlamp is "<<decition.rightlamp()<<std::endl;
-    _m1C4.ACC_LatAngReq = ECU_1C4_ACC_LatAngReq_toS(decition.wheelangle());
+
+    double fWheelAngleReq = decition.wheelangle();
+    double fsendinter = 0.02;
+//    if(fabs(fWheelAngleReq - gfWheelAngle)/fsendinter > gfWheelSpeedLim)
+//    {
+//        if(fWheelAngleReq > gfWheelAngle)
+//        {
+//            fWheelAngleReq = gfWheelAngle + fsendinter * gfWheelSpeedLim;
+//        }
+//        else
+//        {
+//            fWheelAngleReq = gfWheelAngle - fsendinter * gfWheelSpeedLim;
+//        }
+
+//    }
+
+//    std::cout<<" wheel req: "<<decition.wheelangle()<<" real send : "<<fWheelAngleReq<<" real whhel:"<<gfWheelAngle<< std::endl;
+
+    _m1C4.ACC_LatAngReq = ECU_1C4_ACC_LatAngReq_toS(fWheelAngleReq);
     //_m1C4.ADS_Reqmode = decition.angle_mode(); //20221102,新车没有此信号
     _m1C4.ACC_MotorTorqueMaxLimitRequest = ECU_1C4_ACC_MotorTorqueMaxLimitRequest_toS(10);
     _m1C4.ACC_LatAngReqActive = decition.angle_active();
@@ -155,9 +190,9 @@ void executeDecition(const iv::brain::decition &decition)
     /*制动过程用的减速度，加速用扭矩*/
     _m24E.ACC_AccTrqReq = ECU_24E_ACC_AccTrqReq_toS(decition.torque());
     _m24E.ACC_AccTrqReqActive = decition.acc_active();
-    if(decition.brake()<(-1.5))
+    if(decition.brake()<(-5.0))
     {
-        _m24E.ACC_ACCTargetAcceleration = ECU_24E_ACC_ACCTargetAcceleration_toS(-1.5);
+        _m24E.ACC_ACCTargetAcceleration = ECU_24E_ACC_ACCTargetAcceleration_toS(-5.0);
     }
     else
         _m24E.ACC_ACCTargetAcceleration = ECU_24E_ACC_ACCTargetAcceleration_toS(decition.brake());
@@ -307,14 +342,14 @@ void executeDecition(const iv::brain::decition &decition)
     //byte_24E[13] = ((_m24E.ACC_AccTrqReq & (0x1FU)<<3))| ((_m24E.ACC_AccTrqReqActive & (0x01U)) << 2);
     byte_24E[13] = (((_m24E.ACC_AccTrqReq & (0x1FU))<<3))| ((_m24E.ACC_AccTrqReqActive & (0x01U)) << 2);
 
-//    if(decition.leftlamp() == true && decition.rightlamp() == false)
-//        _m36E.ADS_UDLCTurnLightReq = 3;
-//    else if(decition.leftlamp() == false && decition.rightlamp() == true)
-//        _m36E.ADS_UDLCTurnLightReq = 4;
-//    else
-//        _m36E.ADS_UDLCTurnLightReq = 0;
+    if(decition.leftlamp() == true && decition.rightlamp() == false)
+        _m25E.ADS_UDLCTurnLightReq = 3;
+    else if(decition.leftlamp() == false && decition.rightlamp() == true)
+        _m25E.ADS_UDLCTurnLightReq = 4;
+    else
+        _m25E.ADS_UDLCTurnLightReq = 0;
 
-//    byte_36E[0] = ((_m36E.ADS_UDLCTurnLightReq & (0x07U)) << 5);
+    byte_25E[3] = ((_m25E.ADS_UDLCTurnLightReq & (0x07U)));
 
 }
 
@@ -322,10 +357,12 @@ void Activate()
 {
     std::this_thread::sleep_for(std::chrono::milliseconds(100));
     iv::brain::decition xdecition;
+    xdecition.set_brake(0.0);
+    xdecition.set_torque(0.0);
 //    for(int j=0;j<100000;j++)
 //    {
         std::cout<<" run "<<std::endl;
-    for(int i = 0; i < 10; i++){
+    for(int i = 0; i < 3; i++){
         xdecition.set_wheelangle(0.0);
         xdecition.set_angle_mode(0);
         xdecition.set_angle_active(0);
@@ -338,7 +375,7 @@ void Activate()
         ExecSend();
         std::this_thread::sleep_for(std::chrono::milliseconds(10));
     }
-    for(int i = 0; i < 10; i++){
+    for(int i = 0; i < 3; i++){
         xdecition.set_wheelangle(0.0);
         xdecition.set_angle_mode(1);
         xdecition.set_angle_active(1);
@@ -357,7 +394,10 @@ void Activate()
 void UnAcitvate()
 {
     iv::brain::decition xdecition;
-    for(int i = 0; i < 10; i++){
+
+    xdecition.set_brake(0.0);
+    xdecition.set_torque(0.0);
+    for(int i = 0; i < 3; i++){
         xdecition.set_wheelangle(0);
         xdecition.set_angle_mode(1);
         xdecition.set_angle_active(1);
@@ -370,7 +410,7 @@ void UnAcitvate()
         ExecSend();
         std::this_thread::sleep_for(std::chrono::milliseconds(10));
     }
-    for(int i = 0; i < 10; i++){
+    for(int i = 0; i < 3; i++){
         xdecition.set_wheelangle(0);
         xdecition.set_angle_mode(0);
         xdecition.set_angle_active(0);
@@ -412,6 +452,12 @@ void UpdateChassis(const char *strdata, const unsigned int nSize, const unsigned
         gbHaveVehSpd = true;
   //      std::cout<<" gf Veh speed : "<<gfVehSpd<<std::endl;
     }
+
+    if(xchassis.has_angle_feedback())
+    {
+        gfWheelAngle = xchassis.angle_feedback();
+        gbHaveWheelAngle = true;
+    }
 }
 
 
@@ -429,6 +475,8 @@ void ListenDeciton(const char * strdata,const unsigned int nSize,const unsigned
         return;
     }
 
+
+
     //    if(xdecition.gear() != 4)
     //    {
     //        qDebug("not D");
@@ -486,19 +534,20 @@ void ExecSend()
     xmsg.set_channel(0);
     xmsg.set_index(gnIndex);
 
-//    xraw.set_id(0x36E);
-//    xraw.set_data(byte_36E,8);
-//    xraw.set_bext(false);
-//    xraw.set_bremote(false);
-//    xraw.set_len(8);
+    xraw.set_id(0x25E);
+    xraw.set_data(byte_25E,32);
+    xraw.set_bext(false);
+    xraw.set_bremote(false);
+    xraw.set_len(32);
 //    if(nCount == 10)
-//    {
-//        iv::can::canraw * pxraw36E = xmsg.add_rawmsg();
-//        pxraw36E->CopyFrom(xraw);
+    if(nCount%2 == 1) //25Ede zhouqi shi 20ms
+    {
+        iv::can::canraw * pxraw25E = xmsg.add_rawmsg();
+        pxraw25E->CopyFrom(xraw);
 //        nCount = 0;
-//    }
-//    xmsg.set_channel(0);
-//    xmsg.set_index(gnIndex);
+    }
+    xmsg.set_channel(0);
+    xmsg.set_index(gnIndex);
 
     gnIndex++;
     xmsg.set_mstime(QDateTime::currentMSecsSinceEpoch());
@@ -559,12 +608,56 @@ void sendthread()
             gMutex.unlock();
             gnDecitionNum--;
         }
+
+#ifdef TORQUEBRAKETEST
+        if(gnothavetb < 10)
+        {
+            iv::controller::torquebrake xtb;
+            gMutextb.lock();
+            xtb.CopyFrom(gtb);
+            gMutextb.unlock();
+            if(xtb.enable())
+            {
+
+                xdecition.set_torque(xtb.torque());
+                xdecition.set_brake(xtb.brake());
+
+                std::cout<<" use xtb. torque: "<<xtb.torque()<<" brake: "<<xtb.brake()<<std::endl;
+
+//                gcontroller->control_torque(xtb.torque());
+//                gcontroller->control_brake(xtb.brake());
+                //            qDebug("use tb value torque is %f brake is %f",xtb.torque(),xtb.brake());
+            }
+            else
+            {
+                //            qDebug("torquebrake not enable.");
+            }
+            gnothavetb++;
+        }
+
+#endif
         executeDecition(xdecition);
         if(gbChassisEPS == false) ExecSend();
         std::this_thread::sleep_for(std::chrono::milliseconds(10));
     }
+    UnAcitvate();
 }
 
+#ifdef Q_OS_LINUX
+void sig_int(int signo)
+{
+    gbSendRun = false;
+    gpsendthread->join();
+
+    iv::modulecomm::Unregister(gpacansend);
+    iv::modulecomm::Unregister(gpachassis);
+    iv::modulecomm::Unregister(gpadecition);
+
+    std::cout<<" controller exit."<<std::endl;
+    exit(0);
+}
+#endif
+
 int main(int argc, char *argv[])
 {
     RegisterIVBackTrace();
@@ -619,5 +712,12 @@ int main(int argc, char *argv[])
 
     std::thread xthread(sendthread);
 
+    gpsendthread = &xthread;
+
+#ifdef Q_OS_LINUX
+   signal(SIGINT, sig_int);
+   signal(SIGTERM,sig_int);
+#endif
+
     return a.exec();
 }

src/decition/common/common/car_status.h

@@ -27,6 +27,8 @@
 #include "common/tracepointstation.h"
 
 #include "groupmsg.pb.h"
+#include "cameraobject.pb.h"
+#include "cameraobjectarray.pb.h"
 #include <QMutex>
 
 #define RADAR_OBJ_NUM 64
@@ -264,6 +266,33 @@ namespace iv {
         double camera_x_adjust; //camera2vehicle coordinate adjust at x-dim
         double camera_y_adjust; //camera2vehicle coordinate adjust at y-dim
         double laneline_speed; // laneline driver speed
+
+        //debug cheliang juece
+        double ang_debug=0.0;
+        double torque_or_acc=0.0;
+        double brake=0.0;
+
+
+        double mfbasepitch = 0.0;
+        bool mbUseRampAssit = false;
+
+        bool mbUseDynamics = true;
+        bool mbLimitSpeed = true;
+        double mfMaxAcc = 3.0;
+        double mfChassisMaxBrake = 6.0; //Chassis Limit Max Brake
+        double mfPerceptionMax = 40.0; // Max 40 meters Obs
+        double mfMaxSpeed = 80.0; //Max Speed is calculate, mfMaxSpeed = sqrt(2.0 * mfChassisMaxBrake * (mfPerceptionMax - 5.0)) * 3.6;
+
+        //20230814,交通标志牌识别数据更新计时
+        QTime mTrafficSignUpdateTimer;
+        //std::vector<iv::vision::cameraobjectarray> xtrafficsign;
+        double iTrafficsignSpeed = 200;//5,10 ,0
+        int itrafficsignTurn = 200; //0,左转，1 右转
+        float objectCon=0.85;
+        float objectWidth =20;
+
+        double tlStopDis = 5.0;
+
     };
     typedef boost::serialization::singleton<iv::CarStatus> CarStatusSingleton;//非线程安全，注意多线程加锁，单例模式
 }

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/decition/adc_adapter/ge3_adapter.cpp

@@ -115,6 +115,8 @@ iv::decition::Decition iv::decition::Ge3Adapter::getAdapterDeciton(GPS_INS now_g
     (*decition)->wheel_angle=min((float)500.0,(*decition)->wheel_angle);
 
 
+
+
     return *decition;
 }
 

src/decition/decition_brain_sf_changan_shenlan/ADS_decision.xml

@@ -0,0 +1,49 @@
+<xml>	
+	<node name="ADCIntelligentVehicle-Replay">
+		<param name="vin" value="LMWHP1S28J1005878" />
+		<param name="iccid" value="898600MFSSYYG1234018" />
+		<param name="vehType" value="shenlan" />
+		<param name="server" value="60.247.58.116" />
+		<param name="port" value="5600" />
+		<param name="id" value="1" />
+		<param name="group" value="true1" />
+        <param name="speed" value="false" />
+
+		<param name="roadmode0" value="10" />
+		<param name="roadmode5" value="6" />
+		<param name="roadmode11" value="6" />
+		<param name="roadmode13" value="8" />
+		<param name="roadmode14" value="11" />
+		<param name="roadmode15" value="11" />
+		<param name="roadmode16" value="8" />
+		<param name="roadmode17" value="8" />
+		<param name="roadmode18" value="8" />
+		<param name="zhuchetime" value="10" />
+		<param name="epsoff" value="0" />
+        <param name="parklat" value="39.1364494" />
+        <param name="parklng" value="117.0868970" />
+        <param name="parkheading" value="347.6" />
+		<param name="parktype" value="0" />
+		<param name="lightlon" value="118.0866011"/>
+		<param name="lightlat" value="39.1362522" />
+		<param name="LidarRotation" value="95" />
+		<param name="LidarRangeUnit" value="5.0" />
+		
+		<param name="vehWidth" value="3.0" />
+		<param name="avoidObs" value="false" />
+		<param name="avoidInRoad" value="false" />
+
+		<param name="log" value="true2" />
+		<param name="socfDis" value="15" />
+		<param name="aocfDis" value="25" />
+		<param name="aocfTimes" value="3" />
+		<param name="camera_x_adjust" value="0" />
+		<param name="camera_y_adjust" value="6.8" />
+		<param name="laneline_speed" value="7" />
+
+
+		<param name="objectCon" value="0.85" />
+		<param name="objectWidth" value="30" />
+	</node>
+</xml>
+

src/decition/decition_brain_sf_changan_shenlan/decition/adc_adapter/changan_shenlan_adapter.cpp

@@ -22,6 +22,10 @@ iv::decition::ChanganShenlanAdapter::~ChanganShenlanAdapter(){
 iv::decition::Decition iv::decition::ChanganShenlanAdapter::getAdapterDeciton(GPS_INS now_gps_ins, std::vector<Point2D>  trace , float dSpeed, float obsDistance ,
                                                                               float  obsSpeed,float accAim,float accNow  ,bool changingDangWei,Decition *decition){
 
+    bool bUseDynamics = true;//false;
+
+    if(ServiceCarStatus.mbUseDynamics)bUseDynamics = true;
+    else bUseDynamics = false;
 
     float controlSpeed=0;
     float controlBrake=0;
@@ -53,11 +57,34 @@ iv::decition::Decition iv::decition::ChanganShenlanAdapter::getAdapterDeciton(GP
     }
     else
     {
+
+
         controlBrake = 0;
-        if(lastTorque==0){
-            controlSpeed=100;
-        }else{
-            controlSpeed = lastTorque+(accAim-accNow)*500*0.1;
+        if(bUseDynamics == false)
+        {
+            if(lastTorque==0){
+                controlSpeed=100;
+            }else{
+                controlSpeed = lastTorque+(accAim-accNow)*500*0.1;
+            }
+        }
+        else
+        {
+            double fVehWeight = 1800;
+            double fg = 9.8;
+            double fRollForce = 50;
+            const double fRatio = 2.5;
+            double fNeed = fRollForce + fVehWeight*accAim;
+            if(ServiceCarStatus.mbUseRampAssit)
+            {
+                if(now_gps_ins.ins_pitch_angle>(ServiceCarStatus.mfbasepitch + 1.0))
+                {
+                    double fpitch = now_gps_ins.ins_heading_angle - ServiceCarStatus.mfbasepitch;
+                    double fpitchforce = fVehWeight* fg * sin(fpitch*M_PI/180.0);
+                    fNeed = fNeed + fpitchforce;
+                }
+            }
+            controlSpeed = fNeed/fRatio;
         }
     }
 
@@ -70,6 +97,7 @@ iv::decition::Decition iv::decition::ChanganShenlanAdapter::getAdapterDeciton(GP
             if(abs(dSpeed-realSpeed)<2 && controlBrake>0){
                 controlBrake=0;
             }
+            controlSpeed = controlSpeed *400;
         }
     }
 
@@ -79,16 +107,21 @@ iv::decition::Decition iv::decition::ChanganShenlanAdapter::getAdapterDeciton(GP
     controlSpeed = limitSpeed(realSpeed, controlBrake, dSpeed, controlSpeed);
 
     if(DecideGps00::minDecelerate<0){
-        controlBrake = max((0-DecideGps00::minDecelerate)*30,controlBrake);
+        controlBrake = max((0-DecideGps00::minDecelerate)*20,controlBrake);
         controlSpeed=0;
     }
     controlBrake=min(controlBrake,100.0f);
     controlBrake=max(controlBrake,0.0f);
 
-        (*decition)->brake = 0 - controlBrake/25;
+        (*decition)->brake = 0 - controlBrake/20;
         (*decition)->torque= controlSpeed;
         lastTorque=(*decition)->torque;
 
+   if((*decition)->brake < (ServiceCarStatus.mfChassisMaxBrake*(-1.0)))
+   {
+       (*decition)->brake = ServiceCarStatus.mfChassisMaxBrake*(-1.0);
+   }
+
 //    (*decition)->accelerator = 0 - controlBrake/25;
 //    (*decition)->niuju_y = controlSpeed;
 //    lastTorque = (*decition)->niuju_y;
@@ -126,8 +159,8 @@ iv::decition::Decition iv::decition::ChanganShenlanAdapter::getAdapterDeciton(GP
     (*decition)->angle_mode = 2; //横向控制激活，和上一条同时满足才执行横向请求角度
     (*decition)->auto_mode = 3; //3为自动控制模式
 
-    (*decition)->wheel_angle=max((float)-500.0,float((*decition)->wheel_angle+ServiceCarStatus.mfEPSOff));
-    (*decition)->wheel_angle=min((float)500.0,(*decition)->wheel_angle);
+        (*decition)->wheel_angle=max((float)-430.0,float((*decition)->wheel_angle+ServiceCarStatus.mfEPSOff));
+        (*decition)->wheel_angle=min((float)430.0,(*decition)->wheel_angle);
 
 
     return *decition;
@@ -182,8 +215,22 @@ float iv::decition::ChanganShenlanAdapter::limitSpeed(float realSpeed, float con
         controlSpeed=0;
     }
 
+    if(ServiceCarStatus.mbUseDynamics == false)
+        controlSpeed=min((float)1000,controlSpeed);
+    else
+        controlSpeed=min((float)3000.0,controlSpeed);
+    controlSpeed=max((float)0.0,controlSpeed);
+    return controlSpeed;
+
     if(realSpeed<10){
-        controlSpeed=min((float)400.0,controlSpeed);
+        if(realSpeed<5)
+        {
+           controlSpeed=min((float)1000.0,controlSpeed);  //20230425,低速扭矩加大，保证让车辆动起来，解决上坡问题
+        }
+        else
+        {
+            controlSpeed=min((float)400.0,controlSpeed);
+        }
     }else if(realSpeed<30){
         controlSpeed =min((float)600.0,controlSpeed);
     }
@@ -194,7 +241,7 @@ float iv::decition::ChanganShenlanAdapter::limitSpeed(float realSpeed, float con
     }
     if(dSpeed <= 3)
     {
-        controlSpeed = min(controlSpeed,20.0f);
+        controlSpeed = min(controlSpeed,400.0f);
     }
 
     controlSpeed=min((float)1200.0,controlSpeed);

src/decition/decition_brain_sf_changan_shenlan/decition/adc_adapter/hunter_adapter.cpp

@@ -0,0 +1,304 @@
+#include <decition/adc_adapter/hunter_adapter.h>
+#include <common/constants.h>
+#include <common/car_status.h>
+#include <math.h>
+#include <iostream>
+#include <fstream>
+
+#include "ivlog.h"
+extern iv::Ivlog * givlog;
+
+using namespace std;
+
+
+iv::decition::HunterAdapter::HunterAdapter(){
+    adapter_id=10;
+    adapter_name="hunter";
+}
+iv::decition::HunterAdapter::~HunterAdapter(){
+
+}
+
+//hunter 只用控制转角和速度，速度大于0前进，速度小于0后退，驻车是在完全停车的时候再用
+
+iv::decition::Decition iv::decition::HunterAdapter::getAdapterDeciton(GPS_INS now_gps_ins, std::vector<Point2D>  trace , float dSpeed, float obsDistance ,
+                                                                        float  obsSpeed,float accAim,float accNow  ,bool changingDangWei,Decition *decition){
+    accAim=min(0.7f,accAim);
+
+    float controlSpeed=0;
+    float controlBrake=0;
+    float u = 0- accAim;
+    float realSpeed = now_gps_ins.speed;
+    float ttc = 0-(obsDistance/obsSpeed);
+    bool turn_around=false;
+    if(now_gps_ins.roadMode==14||now_gps_ins.roadMode==15||now_gps_ins.roadMode==16||now_gps_ins.roadMode==17){
+        turn_around=true;
+    }
+
+    if(ttc<0){
+        ttc=15;
+    }
+//    if (accAim < 0)
+//    {
+//        controlSpeed=0;
+//        controlBrake=u; //102
+////        if(obsDistance>0 && obsDistance<10)
+//        if(obsDistance>0 && obsDistance<6)
+//        {
+//            controlBrake= u*1.0;     //1.5    zj-1.2
+//            controlSpeed=0;
+//        }
+////        if(abs(dSpeed-realSpeed)<1 &&(obsDistance>40 || obsDistance<0)&&ttc>8 && dSpeed>0 && !turn_around)
+//        if(ttc<5)
+//        {
+//            controlBrake=0;
+//            controlSpeed=0;
+////            controlSpeed=max(0.0,realSpeed-1.0);
+//        }
+////        else if(abs(dSpeed-realSpeed)<3 &&(obsDistance>40 || obsDistance<0)&&ttc>8 && dSpeed>0 && turn_around
+////                 && dSpeed>0 && lastBrake==0)
+//        else if(ttc<10)
+//        {
+//            controlBrake=0;
+//            controlSpeed=max(0.0,realSpeed-2.0);
+//        }
+////        else if(abs(realSpeed)<15 &&(obsDistance>40 || obsDistance<0)&&ttc>8 && dSpeed>0 && turn_around)
+//        else
+//        {
+//            controlBrake=0;
+//            controlSpeed=max(0.0,realSpeed-1.0);
+//        }
+
+////       else if(abs(dSpeed-realSpeed)>=5 &&abs(dSpeed-realSpeed)<10&&(obsDistance>40 || obsDistance<0)&&ttc>8
+////                  && !turn_around )
+////        {
+////            controlBrake=min(controlBrake,0.3f);
+////            controlSpeed=0;
+////        }
+////        else if(abs(dSpeed-realSpeed)>=10 &&abs(dSpeed-realSpeed)<15&&(obsDistance>40 || obsDistance<0)&&ttc>8
+////                && dSpeed>0 && !turn_around )
+////        {
+////            controlBrake=min(controlBrake,0.5f);
+////            controlSpeed=0;
+////        }
+
+
+//        //0110
+//        if(changingDangWei){
+//            controlBrake=max(0.5f,controlBrake);
+//        }
+
+//        //斜坡刹车加大 lsn 0217
+//        if (abs(now_gps_ins.ins_pitch_angle)>2.5 &&(controlBrake>0||dSpeed==0)){
+//            controlBrake=max(2.0f,controlBrake);
+//        }
+//        //斜坡刹车加大 end
+//    }
+//    else
+//    {
+//        controlBrake = 0;
+
+//        if(abs(dSpeed-realSpeed)<=2.0)
+//        {
+//            controlSpeed= dSpeed;
+//        }
+//        else if(abs(dSpeed-realSpeed)>2.0)
+//        {
+//             controlSpeed = min(realSpeed+1,dSpeed);
+//        }
+
+         controlSpeed= dSpeed;
+
+
+//        controlSpeed= dSpeed;
+
+        // 斜坡加大油门   0217 lsn//斜坡的hunter 不考虑，20210913，cxw
+
+//    }
+
+    controlSpeed= dSpeed;
+//    controlSpeed=max(controlSpeed,5.0f); //对车子进行限速，车子最大速度是5km/h
+    if(controlSpeed>5.0)
+    {
+        controlSpeed=5.0;
+    }
+
+    //0227 10m nei xianzhi shache
+    //障碍物距离在3~10米之间，让速度慢慢降到1，当障碍物距离小于3了，直接停车
+    if(obsDistance<10 &&obsDistance>3){
+        controlSpeed=max(1.0,realSpeed-0.5);
+        controlBrake=max(controlBrake,0.6f);//0.8   zj-0.6
+    }
+    else if(obsDistance<=3 &&obsDistance>0)
+    {
+        controlSpeed=0;
+        controlBrake=max(controlBrake,0.6f);//0.8   zj-0.6
+    }
+
+    if(DecideGps00::minDecelerate<0)  //在到达地图终点的时候，停车
+    {
+        controlBrake = max((0-DecideGps00::minDecelerate),controlBrake);
+        controlSpeed=0;
+    }
+
+//    if(DecideGps00::minDecelerate==-0.4){
+//        controlBrake =0.4;
+//    }
+
+    controlBrake=limitBrake(realSpeed,controlBrake,dSpeed,obsDistance,ttc);
+    controlSpeed = limitSpeed(realSpeed, controlBrake, dSpeed, controlSpeed);
+
+    double reverse_speed=-(realSpeed+8)/3.6;//avoid veh in the reverse road
+//    if((obsDistance>8.0)&&(obsSpeed<reverse_speed)){
+//        controlBrake =0;
+//        controlSpeed =0;
+//    }
+
+
+    (*decition)->brake = controlBrake*9;//9     zj-6//对于hunter车辆，brake 没有用
+    if(controlSpeed==0.0)
+    {
+        givlog->debug("brain_decition","controlSpeed: %f", controlSpeed);
+    }
+    (*decition)->torque =controlSpeed;//hunter目标速度大于0前进，小于0后退，hunter只有速度，角度和驻车控制
+
+    lastBrake= (*decition)->brake;
+    lastTorque=(*decition)->torque;
+
+
+    givlog->debug("brain_decition","brake: %f,obsDistance: %f,obsSpeed: %f,reverse_speed: %f",
+                            (*decition)->brake,obsDistance,obsSpeed,reverse_speed,ttc);
+
+    givlog->debug("brain_decition","dSpeed: %f,realspeed: %f,brake: %f,torque: %f",
+                            dSpeed,realSpeed,(*decition)->brake,(*decition)->torque);
+
+
+
+
+    (*decition)->grade=1;
+    (*decition)->mode=1;
+    (*decition)->speak=0;
+    (*decition)->handBrake=1;  // 0: laqi 1: shifang
+    (*decition)->bright=false;
+    (*decition)->engine=0;
+
+    (*decition)->dangweiEnable=true;
+    (*decition)->angleEnable=true;
+    (*decition)->speedEnable=true;
+    (*decition)->brakeEnable=true;
+    (*decition)->air_enable = false;
+    (*decition)->driveMode=1;
+    (*decition)->brakeType=0;
+    (*decition)->angSpeed=60;
+    (*decition)->topLight=0;
+
+
+    if(ServiceCarStatus.bocheMode ||ServiceCarStatus.daocheMode){
+        (*decition)->dangWei=2;
+        (*decition)->backLight=1;
+    }
+    //1220
+    else{
+        (*decition)->dangWei=4;
+        (*decition)->backLight=0;
+    }
+
+
+
+    if((*decition)->brake>0){
+        (*decition)->brakeLight=1;
+    }else{
+        (*decition)->brakeLight=0;
+    }
+
+
+    if((*decition)->leftlamp){
+        (*decition)->directLight=1;
+    }else if((*decition)->rightlamp){
+         (*decition)->directLight=2;
+    }else{
+        (*decition)->directLight=0;
+    }
+
+    (*decition)->handBrake=false;
+    (*decition)->wheel_angle+=ServiceCarStatus.mfEPSOff;
+
+    (*decition)->wheel_angle=max((float)-870.0,(*decition)->wheel_angle);
+    (*decition)->wheel_angle=min((float)870.0,(*decition)->wheel_angle);
+
+
+
+
+        //dangweiTrasfer
+//             if((decition_gps->dangWei!=ServiceCarStatus.dangwei_st && ServiceCarStatus.dangwei_st!=0)){
+//                 decition_gps->dangWei=2;
+//                 decition_gps->torque=0;
+//             }
+     lastDangWei= (*decition)->dangWei;
+
+    (*decition)->topLight=0; //1116
+    (*decition)->nearLight=0;
+    (*decition)->farLight=0;
+    (*decition)->door=3;
+
+std::cout<<"==========================================="<<std::endl;
+     std::cout<<"dSpeed:"<<dSpeed<<"   realSpeed:"<<realSpeed<<"   acc:"<<accAim<<"  torque_st:"<<ServiceCarStatus.torque_st
+             <<"  decideTorque:"<<(*decition)->torque <<"  decideBrake:"<<(*decition)->brake
+    <<std::endl;
+std::cout<<"========================================="<<std::endl;
+
+
+   (*decition)->accelerator=  (*decition)->torque ;
+
+//将ttc时间和障碍物存储到log中方便后期数据分析
+
+
+    return *decition;
+}
+
+
+
+float iv::decition::HunterAdapter::limitBrake(float realSpeed, float controlBrake,float dSpeed,float obsDistance , float ttc){
+
+    //刹车限制
+
+    //刹车限制
+
+    float vs =realSpeed*3.6;
+    int BrakeIntMax = 10;
+    if (vs < 10.0 / 3.6) BrakeIntMax = 4;
+    else if (vs < 20.0 / 3.6) BrakeIntMax = 6;
+
+    //`
+    if(ttc>10){
+        BrakeIntMax = 2;
+    }else if(ttc>6){
+        BrakeIntMax = 3;
+    }else if(ttc>3){
+        BrakeIntMax = 4;
+    }else {
+        BrakeIntMax = 5;
+    }
+    if(obsDistance>0 && obsDistance<10){
+        BrakeIntMax=max(BrakeIntMax,3);
+    }
+
+    if (controlBrake > BrakeIntMax) controlBrake = BrakeIntMax;
+
+
+
+
+    controlBrake=min(5.0f,controlBrake);
+    controlBrake=max(0.0f,controlBrake);
+    return controlBrake;
+
+}
+
+float iv::decition::HunterAdapter::limitSpeed(float realSpeed, float controlBrake,float dSpeed,float controlSpeed ){
+
+    controlSpeed=min((float)100.0,controlSpeed);
+    controlSpeed=max((float)0.0,controlSpeed);
+    return controlSpeed;
+}
+
+

src/decition/decition_brain_sf_changan_shenlan/decition/adc_adapter/hunter_adapter.h

@@ -0,0 +1,40 @@
+#ifndef HUNTER_ADAPTER_H
+#define HUNTER_ADAPTER_H
+#include <common/gps_type.h>
+#include <decition/decition_type.h>
+#include <common/obstacle_type.h>
+#include <vector>
+#include <decition/adc_tools/gnss_coordinate_convert.h>
+#include <decition/adc_adapter/base_adapter.h>
+#include <decition/adc_tools/transfer.h>
+#include <decition/decide_gps_00.h>
+
+
+namespace iv {
+     namespace decition {
+         class HunterAdapter: public BaseAdapter {
+                    public:
+
+                        float lastTorque;
+                        float lastBrake;
+                        int lastDangWei=0;
+
+
+                        HunterAdapter();
+                        ~HunterAdapter();
+
+                        iv::decition::Decition getAdapterDeciton(GPS_INS now_gps_ins, std::vector<Point2D>  trace , float dSpeed, float obsDistacne ,
+                                                                                 float  obsSpeed,float acc,float accNow  ,bool changingDangWei,Decition *decition);
+
+                        float limitBrake(float realSpeed, float controlBrake,float dSpeed,float obsDistance , float ttc);
+                        float limitSpeed(float realSpeed, float controlBrake,float dSpeed,float controlSpeed );
+
+                    private:
+                        int seizingCount=0;
+
+            };
+
+    }
+}
+
+#endif // HUNTER_ADAPTER_H

src/decition/decition_brain_sf_changan_shenlan/decition/adc_controller/pid_controller.cpp

@@ -49,7 +49,52 @@ iv::decition::Decition iv::decition::PIDController::getControlDecition(GPS_INS n
 }
 
 
+double iv::decition::PIDController::CalcKappa(std::vector<Point2D> trace,int preindex)
+{
+    if(trace.size() == 0)return 0;
+    if(preindex>=trace.size())return 0;
+    if(preindex == 0)return 0;
+    double denominator = 2 * trace[preindex].x *(-1);
+    double numerator = pow(trace[preindex].x,2) + pow(trace[preindex].y,2);
+    double fRadius = 1e9;
+    if(fabs(denominator)>0)
+    {
+        fRadius = numerator/denominator;
+    }
+    else
+    {
+        fRadius = 1e9;
+    }
+    if(fRadius == 0)return  0;
+    assert(fRadius != 0);
+    return 1.0/fRadius;
 
+}
+
+double iv::decition::PIDController::ExtendPreviewDistance(std::vector<Point2D> & trace,double & PreviewDistance)
+{
+    double fAllowableError = 0.3;
+    int i = 0;
+    int nsize = static_cast<int>(trace.size());
+    double fPreviewAllow = 0;
+    for(i=0;i<(nsize-1);i++)
+    {
+        if(fabs(trace[i].x)>fAllowableError)
+        {
+            break;
+        }
+    }
+    if((i>=0)&&(i<nsize))
+    {
+        fPreviewAllow = fabs(trace[i].y);
+    }
+    if(fPreviewAllow > PreviewDistance)
+    {
+        PreviewDistance = fPreviewAllow;
+    }
+    return fPreviewAllow;
+
+}
 
 float iv::decition::PIDController::getPidAngle(float realSpeed, std::vector<Point2D> trace, int roadMode){
     double ang = 0;
@@ -71,9 +116,21 @@ float iv::decition::PIDController::getPidAngle(float realSpeed, std::vector<Poin
     if(ServiceCarStatus.msysparam.mvehtype=="ge3" || ServiceCarStatus.msysparam.mvehtype=="vv7"){
         realSpeed > 40 ? PreviewDistance = max(6.0, realSpeed *0.6) : PreviewDistance = max(4.0, realSpeed *0.3);
     }else{
-        realSpeed > 40 ? PreviewDistance = max(6.0, realSpeed *0.6) : PreviewDistance = max(6.0, realSpeed *0.5);
+        realSpeed > 40 ? PreviewDistance = max(6.0, realSpeed *0.6) : PreviewDistance = max(4.0, realSpeed *0.3);
     }
 
+    if(ServiceCarStatus.msysparam.mvehtype=="hunter")
+    {
+        PreviewDistance = 3.0;
+    }
+
+    bool bExtendPreview = true;
+    if(bExtendPreview)
+    {
+        ExtendPreviewDistance(trace,PreviewDistance);
+    }
+
+
     //    if(ServiceCarStatus.msysparam.mvehtype=="bus"){
     //        KEang = 14, KEPos = 7, DEang = 0, DEPos = 0,IEpos=0,IEang=0;
     //        realSpeed > 40 ? PreviewDistance = max(6.0, realSpeed *0.6) : PreviewDistance = max(6.0, realSpeed *0.5);
@@ -90,6 +147,15 @@ float iv::decition::PIDController::getPidAngle(float realSpeed, std::vector<Poin
     std::vector<Point2D> farTrace;
 
     int nsize = trace.size();
+
+    if(nsize == 0)
+    {
+        std::cout<<" in pid controller. no trace , angle is 0."<<std::endl;
+        return 0;
+    }
+
+    assert(nsize != 0);
+
     for (int i = 1; i < nsize-1; i++)
     {
         sumdis += GetDistance(trace[i - 1], trace[i]);
@@ -99,6 +165,26 @@ float iv::decition::PIDController::getPidAngle(float realSpeed, std::vector<Poin
             break;
         }
     }
+
+    if((sumdis>1.0) &&(gpsIndex == 0))gpsIndex = nsize -1;
+
+
+    bool bUseAutowareKappa = true;
+    double wheel_base = 2.9;
+    double wheelratio = 13.6;
+
+    if(ServiceCarStatus.msysparam.mvehtype == "hunter")
+    {
+        wheel_base = 0.6;
+        wheelratio = 21;
+    }
+
+    if(bUseAutowareKappa)
+    {
+        double kappa = CalcKappa(trace,gpsIndex);
+        return (-1.0)*kappa * wheel_base * wheelratio * 180.0/M_PI;
+    }
+
     if(gpsIndex == 0)   gpsIndex = nsize -1;
 
     EPos = trace[gpsIndex].x;
@@ -308,7 +394,7 @@ float iv::decition::PIDController::limiteU(float u,float ttc){
     }
     else
     {
-        if (u < -1.5) u = -1.5;
+        if (u <(ServiceCarStatus.mfMaxAcc *(-1.0))) u = ServiceCarStatus.mfMaxAcc *(-1.0);
     }
     if (u >= 0 && lastU <= 0)
     {

src/decition/decition_brain_sf_changan_shenlan/decition/adc_controller/pid_controller.h

@@ -54,9 +54,13 @@ namespace iv {
                         float  computeU(float obsDistance, float ds,float vs,float vt,float vh,float vl,float ttc,float ttcr);
                         float  limiteU(float u ,float ttc);
 
+                        double CalcKappa(std::vector<Point2D> trace,int preindex);
+
 
                     private:
 
+                        double ExtendPreviewDistance(std::vector<Point2D> & trace,double & PreviewDistance);
+
                     };
 
         }

src/decition/decition_brain_sf_changan_shenlan/decition/adc_tools/compute_00.cpp

@@ -233,7 +233,133 @@ int iv::decition::Compute00::getMapDelta(std::vector<GPSData> MapTrace){
         doubledata[j][3]=delta_sum/20;
     }
 }
+namespace iv {
+namespace decition {
+extern double gplanbrakeacc;
+extern double gplanacc;
+}
+}
+//extern double iv::decition::gplanbrakeacc;
+
+void iv::decition::Compute00::SmoothACC(std::vector<iv::GPSData> & MapTrace,std::vector<std::vector<double>> & vdoubledata)
+{
+    double acca = iv::decition::gplanacc;
+    int nsize = static_cast<int>(vdoubledata.size());
+    int i;
+    for(i=1;i<(nsize-1);i++)
+    {
+        if((vdoubledata[i+1][4] - vdoubledata[i][4])>1.0)
+        {
+            int j;
+            for(j=i;j<(nsize -1);j++)
+            {
+                double fdis = sqrt(pow(MapTrace[j]->gps_x - MapTrace[j+1]->gps_x,2)+pow(MapTrace[j]->gps_y - MapTrace[j+1]->gps_y,2));
+                double vpre = vdoubledata[j][4]/3.6;
+                double fspeed = sqrt((fdis + 0.5*vpre*vpre/acca)*acca/0.5) *3.6;
+                if(fspeed>vdoubledata[j+1][4])
+                {
+                    break;
+                }
+                vdoubledata[j+1][4] = fspeed;
+            }
+            i = j+1;
+        }
+    }
+
+}
+
+
+
+void iv::decition::Compute00::SmoothHighData(std::vector<iv::GPSData> & MapTrace,std::vector<std::vector<double>> & vdoubledata,unsigned int i)
+{
+    if(i==0)return;
+
+//    0.5*v1*v1/a - 0.5*v2*v2/a = dis;
+//    v1*v1 = (dis + 0.5*v2*v2/a)*a/0.5
+
+    int j;
+    double brakea = 3.0*iv::decition::gplanbrakeacc;//iv::decition::gplanbrakeacc;
+    if(brakea>5.0)brakea = 5.0;
+    int startvalue = static_cast<int>(i-1);
+
+    double fdispoint = sqrt(pow(MapTrace[i]->gps_x - MapTrace[i-1]->gps_x,2)+pow(MapTrace[i]->gps_y - MapTrace[i-1]->gps_y,2));
+    if(fdispoint < 0.1)fdispoint = 0.1;
+    int nprebrake = static_cast<int>(10.0/fdispoint);
+    if((MapTrace[i]->roadMode == 14) || (MapTrace[i]->roadMode == 15))
+    {
+        nprebrake = static_cast<int>(5.0/fdispoint);
+    }
+    else
+    {
+        nprebrake = static_cast<int>(3.0/fdispoint);
+    }
+    for(j=startvalue;j>=0;j--)
+    {
+        if(vdoubledata[j][4] < vdoubledata[j+1][4])
+        {
+            break;
+        }
+        if((startvalue - j) < nprebrake)
+        {
+            vdoubledata[j][4] = vdoubledata[j+1][4];
+            continue;
+        }
+        double fdis = sqrt(pow(MapTrace[j]->gps_x - MapTrace[j+1]->gps_x,2)+pow(MapTrace[j]->gps_y - MapTrace[j+1]->gps_y,2));
+        double vpre = vdoubledata[j+1][4]/3.6;
+        double fspeed = sqrt((fdis + 0.5*vpre*vpre/brakea)*brakea/0.5) *3.6;
+        if(fspeed>vdoubledata[j][4])
+        {
+            break;
+        }
+        vdoubledata[j][4] = fspeed;
+    }
 
+}
+
+void iv::decition::Compute00::SmoothData(std::vector<iv::GPSData> & MapTrace,std::vector<std::vector<double>> & vdoubledata,unsigned int i)
+{
+    if(i==0)return;
+
+//    0.5*v1*v1/a - 0.5*v2*v2/a = dis;
+//    v1*v1 = (dis + 0.5*v2*v2/a)*a/0.5
+
+    int j;
+    double brakea = iv::decition::gplanbrakeacc;//iv::decition::gplanbrakeacc;
+    int startvalue = static_cast<int>(i-1);
+
+    double fdispoint = sqrt(pow(MapTrace[i]->gps_x - MapTrace[i-1]->gps_x,2)+pow(MapTrace[i]->gps_y - MapTrace[i-1]->gps_y,2));
+    if(fdispoint < 0.1)fdispoint = 0.1;
+    int nprebrake = static_cast<int>(10.0/fdispoint);
+    if((MapTrace[i]->roadMode == 14) || (MapTrace[i]->roadMode == 15))
+    {
+        nprebrake = static_cast<int>(5.0/fdispoint);
+    }
+    else
+    {
+        nprebrake = static_cast<int>(3.0/fdispoint);
+    }
+    for(j=startvalue;j>=0;j--)
+    {
+        if(vdoubledata[j][4] < vdoubledata[j+1][4])
+        {
+            break;
+        }
+        if((startvalue - j) < nprebrake)
+        {
+            vdoubledata[j][4] = vdoubledata[j+1][4];
+            continue;
+        }
+        double fdis = sqrt(pow(MapTrace[j]->gps_x - MapTrace[j+1]->gps_x,2)+pow(MapTrace[j]->gps_y - MapTrace[j+1]->gps_y,2));
+        double vpre = vdoubledata[j+1][4]/3.6;
+        double fspeed = sqrt((fdis + 0.5*vpre*vpre/brakea)*brakea/0.5) *3.6;
+        if(fspeed>vdoubledata[j][4])
+        {
+            break;
+        }
+        vdoubledata[j][4] = fspeed;
+    }
+
+}
 
 /*int iv::decition::Compute00::getPlanSpeed(std::vector<GPSData> MapTrace)
 {
@@ -358,6 +484,7 @@ int iv::decition::Compute00::getMapDelta(std::vector<GPSData> MapTrace){
 }*/
 int iv::decition::Compute00::getPlanSpeed(std::vector<GPSData> MapTrace)
 {
+    int nSmoothMode = 1;
     int mode0to5count=0,mode0to18count=0,mode0to5countSum=0,mode0to18countSum=0,mode18count=0,mode18countSum=0,mode0to5flash=0,mode18flash=0;
     double straightSpeed=min(ServiceCarStatus.mroadmode_vel.mfmode0,60.0);
     double straightCurveSpeed=min(ServiceCarStatus.mroadmode_vel.mfmode5,40.0);
@@ -366,97 +493,239 @@ int iv::decition::Compute00::getPlanSpeed(std::vector<GPSData> MapTrace)
     getMapDelta(MapTrace);
     for(int i=0;i<doubledata.size();i++)
     {
-        if((MapTrace[i]->mfCurvature>-0.02)&&(MapTrace[i]->mfCurvature<0.02)){
-            MapTrace[i]->roadMode=5;
-        }else if(((MapTrace[i]->mfCurvature>=0.02)&&(MapTrace[i]->mfCurvature<=0.16))||((MapTrace[i]->mfCurvature>=-0.16)&&(MapTrace[i]->mfCurvature<=-0.02))){
-            MapTrace[i]->roadMode=18;
-        }else if(((MapTrace[i]->mfCurvature>0.16))||((MapTrace[i]->mfCurvature<-0.16))){
-            MapTrace[i]->roadMode=14;
+
+        if((MapTrace[i]->roadMode==14)||(MapTrace[i]->roadMode==15))
+        {
+           int  a = 1;
+        }
+        else
+        {
+            if((MapTrace[i]->mfCurvature>-0.02)&&(MapTrace[i]->mfCurvature<0.02)){
+                MapTrace[i]->roadMode=5;
+            }else if(((MapTrace[i]->mfCurvature>=0.02)&&(MapTrace[i]->mfCurvature<=0.16))||((MapTrace[i]->mfCurvature>=-0.16)&&(MapTrace[i]->mfCurvature<=-0.02))){
+                MapTrace[i]->roadMode=18;
+            }else if(((MapTrace[i]->mfCurvature>0.16))||((MapTrace[i]->mfCurvature<-0.16))){
+                MapTrace[i]->roadMode=14;
+            }
         }
     }
-                for(int i=0;i<MapTrace.size();i++)
-                {
-                    if(MapTrace[i]->roadMode==0){
-                        doubledata[i][4]=straightSpeed;
-                        mode0to5count++;
-                        //mode0to18count++;
-                        mode18count=0;
-                        mode0to5flash=mode0to5count;
-                    }else if(MapTrace[i]->roadMode==5){
-                        doubledata[i][4]=straightCurveSpeed;
-                        mode0to5count++;
-                        //mode0to18count++;
-                        mode18count=0;
-                        mode0to5flash=mode0to5count;
-                    }else if(MapTrace[i]->roadMode==18){
-                        mode0to5countSum=mode0to5count;
-                        doubledata[i][4]=Curve_SmallSpeed;
 
-                        double brake_distance_double=(pow((straightCurveSpeed/3.6),2)-pow((Curve_SmallSpeed/3.6),2))*10/0.6;
-                        int brake_distance=(int)brake_distance_double;
-                        int step_count=0;
-                        if((i>brake_distance)&&(mode0to5countSum>brake_distance))
-                        {
-                            double step_speed=(straightCurveSpeed-Curve_SmallSpeed)/brake_distance;
+    for(int i=1;i<MapTrace.size()-1;i++)
+    {
+         if((MapTrace[i-1]->roadMode==5)&&(MapTrace[i+1]->roadMode==5))
+         {
+          if(MapTrace[i]->roadMode!=5)
+           MapTrace[i]->roadMode=5;
+
+         }
+         else if ((MapTrace[i-1]->roadMode==18)&&(MapTrace[i+1]->roadMode==18))
+         {
+          if(MapTrace[i]->roadMode==5)
+           MapTrace[i]->roadMode=18;
+          if(MapTrace[i]->roadMode==14)
+           MapTrace[i]->roadMode=18;
+         }
+            else if ((MapTrace[i-1]->roadMode==14)&&(MapTrace[i+1]->roadMode==14))
+         {
+          if(MapTrace[i]->roadMode==5)
+           MapTrace[i]->roadMode=14;
+                 if(MapTrace[i]->roadMode==18)
+           MapTrace[i]->roadMode=14;
+         }
+         else
+         {
+
+         }
+    }
+    if(nSmoothMode == 0)
+        {
 
-                            for(int j=i;j>i-brake_distance;j--){
-                                doubledata[j][4]=min((Curve_SmallSpeed+step_count*step_speed),straightCurveSpeed);
-                                step_count++;
+                    for(int i=0;i<MapTrace.size();i++)
+                    {
+                        if(MapTrace[i]->roadMode==0){
+                            doubledata[i][4]=straightSpeed;
+                            if(MapTrace[i]->speed>0)
+                            {
+         //                       double fSpeed = MapTrace[i]->speed;
+                                doubledata[i][4] = MapTrace[i]->speed*3.6;
                             }
-                        }else if(mode0to5countSum>0){
-                            for(int j=i;j>=i-mode0to5countSum;j--){
-                                doubledata[j][4]=Curve_SmallSpeed;
-                                step_count++;
+                            mode0to5count++;
+                            //mode0to18count++;
+                            mode18count=0;
+                            mode0to5flash=mode0to5count;
+                        }else if(MapTrace[i]->roadMode==5){
+                            doubledata[i][4]=straightCurveSpeed;
+                            if(MapTrace[i]->speed>0)
+                            {
+         //                       double fSpeed = MapTrace[i]->speed;
+                                doubledata[i][4] = MapTrace[i]->speed*3.6;
                             }
-                        }else{
+                            mode0to5count++;
+                            //mode0to18count++;
+                            mode18count=0;
+                            mode0to5flash=mode0to5count;
+                        }else if(MapTrace[i]->roadMode==18){
+                            mode0to5countSum=mode0to5count;
                             doubledata[i][4]=Curve_SmallSpeed;
-                        }
-                        mode0to5count=0;
-                        //mode0to18count++;
-                        mode18count++;
-                        mode18flash=mode18count;
-                    }else if(MapTrace[i]->roadMode==14){
-                        mode0to18countSum=mode0to5flash+mode18flash;
-                        mode18countSum=mode18count;
-                        double brake_distanceLarge_double=(pow((Curve_SmallSpeed/3.6),2)-pow((Curve_LargeSpeed/3.6),2))*10/0.6;
-                        double brake_distance0to18_double=(pow((straightCurveSpeed/3.6),2)-pow((Curve_LargeSpeed/3.6),2))*10/0.6;
-                        int brake_distanceLarge=(int)brake_distanceLarge_double;
-                        int brake_distance0to18=(int)brake_distance0to18_double;
-                        int step_count=0;
-                        doubledata[i][4]=Curve_LargeSpeed;
 
-                        if(mode18countSum>brake_distanceLarge)
-                        {
-                                double step_speed=(Curve_SmallSpeed-Curve_LargeSpeed)/brake_distanceLarge;
+                            double brake_distance_double=(pow((straightCurveSpeed/3.6),2)-pow((Curve_SmallSpeed/3.6),2))*5/0.6; //1228
+                            int brake_distance=(int)brake_distance_double;
+                            int step_count=0;
+                            if((i>brake_distance)&&(mode0to5countSum>brake_distance))
+                            {
+                                double step_speed=(straightCurveSpeed-Curve_SmallSpeed)/brake_distance;
+
+                                for(int j=i;j>i-brake_distance;j--){
+                                    doubledata[j][4]=min((Curve_SmallSpeed+step_count*step_speed),straightCurveSpeed);
 
-                                for(int j=i;j>i-brake_distanceLarge;j--){
-                                    doubledata[j][4]=min((Curve_LargeSpeed+step_count*step_speed),Curve_SmallSpeed);
                                     step_count++;
                                 }
-
-                        }else if(mode0to18countSum>brake_distance0to18){
-
-                                double step_speed=(straightCurveSpeed-Curve_LargeSpeed)/brake_distance0to18;
-
-                                for(int j=i;j>i-brake_distance0to18;j--){
-                                    doubledata[j][4]=min((Curve_LargeSpeed+step_count*step_speed),straightCurveSpeed);
+                            }else if(mode0to5countSum>0){
+                                double step_speed=(straightCurveSpeed-Curve_SmallSpeed)/brake_distance;//1228
+                                for(int j=i;j>=i-mode0to5countSum;j--){
+                                    doubledata[j][4]=Curve_SmallSpeed;//min((Curve_SmallSpeed+step_count*step_speed),straightCurveSpeed);//1228Curve_SmallSpeed;
                                     step_count++;
                                 }
-                        }else if(mode0to18countSum>0){
-                            for(int j=i;j>=i-mode0to18countSum;j--){
-                                doubledata[j][4]=Curve_LargeSpeed;
-                                step_count++;
+                            }else{
+                                doubledata[i][4]=Curve_SmallSpeed;
                             }
-                        }else{
-                                doubledata[i][4]=Curve_LargeSpeed;
+                            mode0to5count=0;
+                            //mode0to18count++;
+                            mode18count++;
+                            mode18flash=mode18count;
+                        }else if((MapTrace[i]->roadMode==14)||(MapTrace[i]->roadMode==15)){
+                            mode0to18countSum=mode0to5flash+mode18flash;
+                            mode18countSum=mode18count;
+                            double brake_distanceLarge_double=(pow((Curve_SmallSpeed/3.6),2)-pow((Curve_LargeSpeed/3.6),2))*5/0.6; //1228
+                            double brake_distance0to18_double=(pow((straightCurveSpeed/3.6),2)-pow((Curve_LargeSpeed/3.6),2))*5/0.6;
+                            int brake_distanceLarge=(int)brake_distanceLarge_double;
+                            int brake_distance0to18=(int)brake_distance0to18_double;
+                            int step_count=0;
+                            doubledata[i][4]=Curve_LargeSpeed;
+
+                            if(mode18countSum>brake_distanceLarge)
+                            {
+                                    double step_speed=(Curve_SmallSpeed-Curve_LargeSpeed)/brake_distanceLarge;
+
+                                    for(int j=i;j>i-brake_distanceLarge;j--){
+                                        doubledata[j][4]=min((Curve_LargeSpeed+step_count*step_speed),Curve_SmallSpeed);
+                                        step_count++;
+                                    }
+
+                            }else if(mode0to18countSum>brake_distance0to18){
+
+                                    double step_speed=(straightCurveSpeed-Curve_LargeSpeed)/brake_distance0to18;
+
+                                    for(int j=i;j>i-brake_distance0to18;j--){
+                                        if(MapTrace[j]->roadMode==18)//20230106,fangzhi 18moshi de sudu dayu yuzhi
+                                            continue;
+                                        doubledata[j][4]=min((Curve_LargeSpeed+step_count*step_speed),straightCurveSpeed);
+                                        step_count++;
+                                    }
+                            }else if(mode0to18countSum>0){
+                                double step_speed=(straightCurveSpeed-Curve_LargeSpeed)/brake_distance0to18;//1228
+                                for(int j=i;j>=i-mode0to18countSum;j--){
+                                    doubledata[j][4]=Curve_LargeSpeed;//min((Curve_LargeSpeed+step_count*step_speed),straightCurveSpeed);//1228Curve_LargeSpeed;
+                                    step_count++;
+                                }
+                            }else{
+                                    doubledata[i][4]=Curve_LargeSpeed;
+                            }
+
+                            mode0to5count=0;
+                            mode18count=0;
+                            mode0to5flash=0;
+                            mode18flash=0;
                         }
+                    }
 
-                        mode0to5count=0;
-                        mode18count=0;
-                        mode0to5flash=0;
-                        mode18flash=0;
+
+        }
+    else
+    {
+        for(int i=0;i<MapTrace.size();i++)
+        {
+            if(MapTrace[i]->roadMode==0){
+                doubledata[i][4]=straightSpeed;
+                if(MapTrace[i]->speed>0)
+                {
+//                       double fSpeed = MapTrace[i]->speed;
+                    doubledata[i][4] = MapTrace[i]->speed*3.6;
+
+                }
+
+
+                if(i>0)
+                {
+                    if((doubledata[i-1][4] - doubledata[i][4]) > 5.0)
+                    {
+                        SmoothHighData(MapTrace,doubledata,i);
+                    }
+                }
+
+            }else if(MapTrace[i]->roadMode==5){
+                doubledata[i][4]=straightCurveSpeed;
+                if(MapTrace[i]->speed>0)
+                {
+//                       double fSpeed = MapTrace[i]->speed;
+                    doubledata[i][4] = MapTrace[i]->speed*3.6;
+
+                }
+
+
+                if(i>0)
+                {
+                    if((doubledata[i-1][4] - doubledata[i][4]) > 5.0)
+                    {
+                        SmoothHighData(MapTrace,doubledata,i);
                     }
                 }
+
+            }else if(MapTrace[i]->roadMode==18){
+
+                if(fabs(MapTrace[i]->mfCurvature)>0.06)
+                {
+                    doubledata[i][4]=(Curve_SmallSpeed + Curve_LargeSpeed)/2.0;
+                }
+                else
+                    doubledata[i][4]=Curve_SmallSpeed;
+
+                if((MapTrace[i]->speed>0) &&(doubledata[i][4]>(MapTrace[i]->speed*3.6)))
+                {
+//                       double fSpeed = MapTrace[i]->speed;
+                    doubledata[i][4] = MapTrace[i]->speed*3.6;
+                }
+                if(i>0)
+                {
+                    if(MapTrace[i-1]->roadMode != MapTrace[i]->roadMode)
+                    {
+                        SmoothData(MapTrace,doubledata,i);
+                    }
+                }
+
+
+            }else if((MapTrace[i]->roadMode==14)||(MapTrace[i]->roadMode==15)){
+
+                doubledata[i][4]=Curve_LargeSpeed;
+                if((MapTrace[i]->speed>0) &&(doubledata[i][4]>(MapTrace[i]->speed*3.6)))
+                {
+//                       double fSpeed = MapTrace[i]->speed;
+                    doubledata[i][4] = MapTrace[i]->speed*3.6;
+                }
+                if(i>0)
+                {
+                    if(MapTrace[i-1]->roadMode != MapTrace[i]->roadMode)
+                    {
+                        SmoothData(MapTrace,doubledata,i);
+                    }
+                }
+
+            }
+        }
+
+        SmoothACC(MapTrace,doubledata);
+    }
+
 //
                 /*for(int i=0;i<MapTrace.size();i++){
                     //将数据写入文件开始
@@ -1010,6 +1279,7 @@ iv::Point2D iv::decition::Compute00::getLidarObsPoint(std::vector<Point2D> gpsTr
 iv::Point2D iv::decition::Compute00::getLidarObsPointXY(std::vector<Point2D> gpsTrace,std::vector<Point2D> gpsTraceLeft,std::vector<Point2D> gpsTraceRight, iv::LidarGridPtr lidarGridPtr) {
 
     iv::Point2D obsPoint(-1, -1);
+    obsPoint.s = -1;
     float xiuzheng=0;
     if(!ServiceCarStatus.useMobileEye){
         xiuzheng=0-ServiceCarStatus.msysparam.lidarGpsXiuzheng;
@@ -1923,7 +2193,7 @@ double iv::decition::Compute00::bocheCompute(GPS_INS nowGps, GPS_INS aimGps) {
     double x_2 = 0.0, y_2 = 0.0;
     double steering_angle;
     double l = 2.950;
-    double r =6;
+    double r =7.0;  //6.0
     double x_o, y_o, x_o_1, y_o_1, x_o_2, y_o_2, x_3, y_3;
     double x_t_n, y_t_n, x_t_f, y_t_f;//近切点和远切点
     double x_t_1, y_t_1, x_t_2, y_t_2;//圆形1的切点

src/decition/decition_brain_sf_changan_shenlan/decition/adc_tools/compute_00.h

@@ -37,6 +37,10 @@ namespace iv {
             static int getDesiredSpeed(std::vector<GPSData> gpsMap);
             static int getMapDelta(std::vector<GPSData> MapTrace);
             static int getPlanSpeed(std::vector<GPSData> MapTrace);
+            static void SmoothACC(std::vector<iv::GPSData> & MapTrace,std::vector<std::vector<double>> & vdoubledata);
+            static void SmoothData(std::vector<iv::GPSData> & MapTrace,std::vector<std::vector<double>> & vdoubledata,unsigned int i);
+
+            static void SmoothHighData(std::vector<iv::GPSData> & MapTrace,std::vector<std::vector<double>> & vdoubledata,unsigned int i);
 
 
             static double getDecideAngle(std::vector<Point2D>  gpsTrace, double realSpeed);

src/decition/decition_brain_sf_changan_shenlan/decition/brain.cpp

@@ -28,6 +28,8 @@ extern std::string gstrmembraincarstate;
 namespace iv {
 namespace decition {
         iv::decition::BrainDecition * gbrain;
+        double gplanbrakeacc=0.3;
+        double gplanacc=1.0;
 
         void ListenTraceMap(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
         {
@@ -66,7 +68,11 @@ namespace decition {
         {
             gbrain->GetFusion(strdata,nSize);
         }
-    }
+        void ListenTrafficSign(const char *strdata, const unsigned int nSize, const unsigned int index, const QDateTime *dt, const char *strmemname) //20230814,交通标志牌识别数据更新
+        {
+            gbrain->GetTrafficSign(strdata,nSize);
+        }
+}
 
 }
 
@@ -94,6 +100,7 @@ iv::decition::BrainDecition::BrainDecition()
 
     mpaVechicleState = iv::modulecomm::RegisterSend(gstrmembrainstate.data(),10000,10);
     mpaCarstate      = iv::modulecomm::RegisterSend(gstrmembraincarstate.data(),10000,10);
+    mpaBrainRunState = iv::modulecomm::RegisterSend("brainrunstate",10000,1);
 
     mpfusion         = iv::modulecomm::RegisterRecv("li_ra_fusion",ListenFusion);
 
@@ -113,6 +120,9 @@ iv::decition::BrainDecition::BrainDecition()
 
     mpv2r            = iv::modulecomm::RegisterRecv("v2r_send",iv::decition::ListenV2r); //20211009,cxw
 
+    mpaTrafficSign   = iv::modulecomm::RegisterRecv("signarray",iv::decition::ListenTrafficSign); //20230814,交通标志牌识别数据更新
+
+
     ModuleFun funchassis =std::bind(&iv::decition::BrainDecition::UpdateChassis,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
     mpaChassis = iv::modulecomm::RegisterRecvPlus(gstrmemchassis.data(),funchassis);
 
@@ -148,6 +158,8 @@ iv::decition::BrainDecition::~BrainDecition()
     iv::modulecomm::Unregister(mpaVechicleState);
     iv::modulecomm::Unregister(mpvbox);
     iv::modulecomm::Unregister(mpa);
+    iv::modulecomm::Unregister(mpaTrafficSign);
+    iv::modulecomm::Unregister(mpv2r);
 
     delete eyes;
     std::cout<<"Brain Unialize."<<std::endl;
@@ -186,6 +198,8 @@ void iv::decition::BrainDecition::run() {
     ServiceCarStatus.msysparam.mstriccid = xp.GetParam("iccid","11111111111111111111");
     ServiceCarStatus.msysparam.mvehtype= xp.GetParam("vehType","ge3");
 
+    std::string strvehtype = ServiceCarStatus.msysparam.mvehtype;
+
     ServiceCarStatus.msysparam.vehWidth = atof(xp.GetParam("vehWidth","2.1").data());
     ServiceCarStatus.msysparam.vehLenth = atof(xp.GetParam("vehLenth","4.6").data());
 
@@ -366,6 +380,84 @@ void iv::decition::BrainDecition::run() {
     ServiceCarStatus.aocfDis = atof(aocfDis.data());
     ServiceCarStatus.aocfTimes = atof(aocfTimes.data());
 
+    //20230814,jiaotong biaozhipaishibie
+
+    std::string objectCon = xp.GetParam("objectCon","0.80");
+    std::string objectWidth = xp.GetParam("objectWidth","29");
+    ServiceCarStatus.objectCon = atof(objectCon.data());
+    ServiceCarStatus.objectWidth = atof(objectWidth.data());
+
+
+    std::string angdebug = xp.GetParam("angDebug","0.0");
+    std::string tordebug = xp.GetParam("torqueDebug","0.0");
+    std::string brkdebug = xp.GetParam("brakeDebug","3");
+
+
+
+    ServiceCarStatus.ang_debug = atof(angdebug.data());
+    ServiceCarStatus.torque_or_acc = atof(tordebug.data());
+    ServiceCarStatus.brake = atof(brkdebug.data());
+
+    std::string strbasepitch = xp.GetParam("basepitch","0.0");
+    ServiceCarStatus.mfbasepitch = atof(strbasepitch.data());
+
+    ServiceCarStatus.mbUseRampAssit = xp.GetParam("UseRamp",false);
+
+    gplanbrakeacc = fabs(xp.GetParam("planbrakeacc",0.3));
+    if(gplanbrakeacc <= 0.01)gplanbrakeacc = 0.01;
+
+    gplanacc = fabs(xp.GetParam("planacc",1.0));
+    if(gplanacc <0.1)gplanacc = 0.1;
+
+
+    decitionGps00->mstopbrake = fabs(xp.GetParam("stopbrake",0.6));
+    if(decitionGps00->mstopbrake < 0.1)decitionGps00->mstopbrake = 0.1;
+
+
+//    bool mbUseDynamics = true;
+//    bool mbLimitSpeed = true;
+//    double mfMaxAcc = 3.0;
+//    double mfChassisMaxBrake = 6.0; //Chassis Limit Max Brake
+//    double mfPerceptionMax = 40.0; // Max 40 meters Obs
+//    double mfMaxSpeed = 80.0; //Max Speed is calculate, mfMaxSpeed = sqrt(2.0 * mfChassisMaxBrake * (mfPerceptionMax - 5.0)) * 3.6;
+
+    ServiceCarStatus.mbUseDynamics = xp.GetParam("UseDynamics",true);
+    if(ServiceCarStatus.msysparam.mvehtype=="shenlan")
+        ServiceCarStatus.mbLimitSpeed = xp.GetParam("LimitSpeed",true);
+    else
+        ServiceCarStatus.mbLimitSpeed = xp.GetParam("LimitSpeed",false);
+    ServiceCarStatus.mfMaxAcc = xp.GetParam("MaxAcc",3.0);
+    if(ServiceCarStatus.mfMaxAcc<0.0)ServiceCarStatus.mfMaxAcc = 0.1;
+    ServiceCarStatus.mfChassisMaxBrake = xp.GetParam("ChassisMaxBrake",6.0);
+    if(ServiceCarStatus.msysparam.mvehtype=="shenlan")
+        ServiceCarStatus.mfChassisMaxBrake = xp.GetParam("ChassisMaxBrake",1.5);
+    ServiceCarStatus.mfPerceptionMax = xp.GetParam("PerceptionMax",40);
+    if(ServiceCarStatus.mfPerceptionMax <= 6.0)ServiceCarStatus.mfPerceptionMax = 6.0;
+    if(ServiceCarStatus.mfChassisMaxBrake < 0.1)ServiceCarStatus.mfChassisMaxBrake = 0.1;
+    ServiceCarStatus.mfMaxSpeed = sqrt(2.0 * ServiceCarStatus.mfChassisMaxBrake * (ServiceCarStatus.mfPerceptionMax - 5.0)) * 3.6;
+
+    if((ServiceCarStatus.msysparam.mvehtype=="shenlan") &&(ServiceCarStatus.mbLimitSpeed))
+    {
+        ServiceCarStatus.mfMaxSpeed = 20.0;
+        ServiceCarStatus.mfChassisMaxBrake = 1.5;
+    }
+
+
+    if(ServiceCarStatus.msysparam.mvehtype=="hunter")
+    {
+        ServiceCarStatus.mfMaxSpeed = 5.0;
+    }
+
+    //add by lyp, for setting distance to trafficlight stopline 
+    std::string tlStopDis = xp.GetParam("tlStopDis","5");
+    ServiceCarStatus.tlStopDis = atof(tlStopDis.data());
+    //max tlStopDis is 10
+    if(ServiceCarStatus.tlStopDis > 10)
+        ServiceCarStatus.tlStopDis = 10;
+
+    std::cout<<" Max Acc: "<<ServiceCarStatus.mfMaxAcc<<" PerceptionMax: "<<ServiceCarStatus.mfPerceptionMax
+            <<" Chassis Max Brake: "<<ServiceCarStatus.mfChassisMaxBrake<<" Max Speed: "<<ServiceCarStatus.mfMaxSpeed<<std::endl;
+
     mstrmemmap_index = xp.GetParam("msg_mapindex","map_index");
 
 
@@ -427,7 +519,7 @@ void iv::decition::BrainDecition::run() {
                 }
                 else
                 {
-                    decition_gps->brake = 6;//保证在进入自动驾驶的时候车辆能够泄压,否则进入自动驾驶车辆无法加速
+                    decition_gps->brake = -2;//保证在进入自动驾驶的时候车辆能够泄压,否则进入自动驾驶车辆无法加速
                 }
                 decition_gps->torque=0.0;
                 decition_gps->acc_active = 0;//ACC扭矩请求激活（为1激活，为0不激活），不激活后不执行扭矩请求，对于深蓝没什么用，因为控制模块直接给的1
@@ -436,7 +528,47 @@ void iv::decition::BrainDecition::run() {
                 decition_gps->angle_active = 0;//横向控制激活模式
                 decition_gps->angle_mode = 0; //横向控制激活，和上一条同时满足才执行横向请求角度
                 decition_gps->auto_mode = 0; //3为自动控制模式
-                decition_gps->wheel_angle = 0;
+               // ServiceCarStatus.ang_debug=ServiceCarStatus.ang_debug*15.9;//如果xml中配置的是车轮转角
+                ServiceCarStatus.ang_debug=max((double)-430.0,ServiceCarStatus.ang_debug);
+                ServiceCarStatus.ang_debug=min((double)430.0,ServiceCarStatus.ang_debug);
+                decition_gps->wheel_angle = ServiceCarStatus.ang_debug;//0;
+
+
+                double dSpeed=200;
+                //20230814,交通标志识别添加 start
+                if(ServiceCarStatus.mTrafficSignUpdateTimer.elapsed()>10*1000)
+                 {
+                    ServiceCarStatus.iTrafficsignSpeed=200;
+                    ServiceCarStatus.itrafficsignTurn=200;
+                 }
+
+                if(ServiceCarStatus.itrafficsignTurn==2)
+                {
+                    decition_gps->leftlamp = true;
+                    decition_gps->rightlamp = false;
+                }
+                else if(ServiceCarStatus.itrafficsignTurn==3)
+                {
+                    decition_gps->leftlamp = false;
+                    decition_gps->rightlamp = true;
+                }
+                else
+                {
+                    decition_gps->leftlamp = false;
+                    decition_gps->rightlamp = false;;
+                }
+                std::cout<<"  TrafficSignleft,2 is turnleft:"<<ServiceCarStatus.itrafficsignTurn<<std::endl;
+                std::cout<<"  TrafficSignright,3 is turnright:"<<ServiceCarStatus.itrafficsignTurn<<std::endl;
+                std::cout<<"  TrafficSigndSpeed:"<<ServiceCarStatus.iTrafficsignSpeed<<std::endl;
+                dSpeed=min(dSpeed,ServiceCarStatus.iTrafficsignSpeed);
+                //20230814,交通标志识别添加 end
+                std::cout<<"===================v2x========================"<<std::endl;
+                //print V2X honglvdeng xinxi
+                std::cout<<"    iTrafficeLightTime:"<<ServiceCarStatus.iTrafficeLightTime
+                         <<"    iTrafficeLightLat:"<<ServiceCarStatus.iTrafficeLightLat
+                         <<"    iTrafficeLightLon:"<<ServiceCarStatus.iTrafficeLightLon
+                         <<"    LightType:"<<ServiceCarStatus.iTrafficeLightColor
+                         <<std::endl;
             }
             else
             {
@@ -466,6 +598,8 @@ void iv::decition::BrainDecition::run() {
 
         }
 
+        givlog->debug("decition_brain"," ServiceCarStatus mbRunPause is : %d",ServiceCarStatus.mbRunPause);
+
         ServiceCarStatus.mbBrainCtring = true;
         obs_lidar_ = obs_radar_ = NULL;
 
@@ -478,6 +612,8 @@ void iv::decition::BrainDecition::run() {
         eyes->getSensorObstacle(obs_radar_, obs_camera_,gps_data_, obs_lidar_grid_);	//从传感器线程临界区交换数据
 
 
+
+
         ServiceLidar.copylidarperto(lidar_per);
 
 
@@ -628,6 +764,10 @@ void iv::decition::BrainDecition::run() {
             ShareBrainstate(xbs);
             ShareDecition(decition_gps);
 
+            iv::brain::brainrunstate xbr;
+            xbr.set_decision_period(decision_period);
+            ShareBrainRunState(xbr);
+
             iv::brain::carstate car_xbs;
             car_xbs.set_mstate(ServiceCarStatus.mvehState);
             car_xbs.set_mavoidx(ServiceCarStatus.mavoidX);
@@ -851,13 +991,18 @@ void iv::decition::BrainDecition::ShareDecition(iv::decition::Decition decition)
     xdecition.set_angle_mode(decition->angle_mode);
     xdecition.set_angle_active(decition->angle_active);
     xdecition.set_auto_mode(decition->auto_mode);
+    xdecition.set_leftlamp(decition->leftlamp);
+    xdecition.set_rightlamp(decition->rightlamp);
     if(ServiceCarStatus.msysparam.mvehtype=="shenlan")
     {
         std::cout<<"===================shareDecition========================"<<std::endl;
         std::cout<<"  torque_st:"<<ServiceCarStatus.torque_st
                 <<"  decideTorque:"<<decition->torque <<"  decideBrake:"<<decition->brake
+                <<"  decideAngle:"<<decition->wheel_angle
                 <<"  decideAngActive:"<<decition->angle_active
                 <<"  decideAutoMode:"<<decition->auto_mode
+               <<"  decideLeftTurn:"<<decition->leftlamp
+               <<"  decideRightTurn:"<<decition->rightlamp
                 <<std::endl;
         std::cout<<"========================================="<<std::endl;
     }
@@ -892,6 +1037,15 @@ void iv::decition::BrainDecition::ShareDecition(iv::decition::Decition decition)
         {
 
             iv::modulecomm::ModuleSendMsg(mpaDecition,str,nsize);
+//            else
+
+        }
+        else
+        {
+            if(ServiceCarStatus.msysparam.mvehtype=="shenlan")
+            {
+                iv::modulecomm::ModuleSendMsg(mpaDecition,str,nsize);
+            }
         }
     }
     else
@@ -900,8 +1054,25 @@ void iv::decition::BrainDecition::ShareDecition(iv::decition::Decition decition)
     }
 }
 
-void iv::decition::BrainDecition::ShareBrainstate(iv::brain::brainstate xbs)
+void iv::decition::BrainDecition::ShareBrainRunState(iv::brain::brainrunstate &xbs)
 {
+    int nsize = xbs.ByteSize();
+    char * str = new char[nsize];
+    std::shared_ptr<char> pstr;
+    pstr.reset(str);
+    if(xbs.SerializeToArray(str,nsize))
+    {
+        iv::modulecomm::ModuleSendMsg(mpaBrainRunState,str,nsize);
+    }
+    else
+    {
+        std::cout<<"iv::decition::BrainDecition::ShareBrainstate serialize error."<<std::endl;
+    }
+}
+
+void iv::decition::BrainDecition::ShareBrainstate(iv::brain::brainstate & xbs)
+{
+
     int nsize = xbs.ByteSize();
     char * str = new char[nsize];
     std::shared_ptr<char> pstr;
@@ -1125,6 +1296,136 @@ void iv::decition::BrainDecition::UpdateFusion(std::shared_ptr<iv::fusion::fusio
 
 }
 
+//20230814,交通标志识别
+void iv::decition::BrainDecition::GetTrafficSign(const char *pdata, const int ndatasize)
+{
+    std::shared_ptr<iv::vision::cameraobjectarray> xtrafficsign (new iv::vision::cameraobjectarray);
+
+    if(ndatasize<1)return;
+
+    if(false == xtrafficsign->ParseFromArray(pdata,ndatasize))
+    {
+        std::cout<<" Listen traffic sign fail."<<std::endl;
+        return;
+    }
+    else{
+        //std::cout<<"srfront byte size:  "<<radarobjvec.ByteSize()<<std::endl;
+    }
+
+    ServiceCarStatus.mTrafficSignUpdateTimer.start();
+//    ServiceCarStatus.iTrafficsignSpeed=200.0;
+//    ServiceCarStatus.itrafficsignTurn=200.0;
+
+    iv::decition::BrainDecition::UpdateTrafficSign(xtrafficsign);
+}
+
+void iv::decition::BrainDecition::UpdateTrafficSign(std::shared_ptr<iv::vision::cameraobjectarray> xtrafficsign)
+{
+    mMutexVison_.lock();
+
+    xtrafficsign.swap(mvision_obs_);
+    int flag;
+    int objectID=0;
+    float objectCon=ServiceCarStatus.objectCon;  //W>30
+    float objectW=ServiceCarStatus.objectWidth;  //W>0.85
+    for(int i =0;i<mvision_obs_->obj_size();i++)//{"Speed10","Speed5","Left","Right","Nopassing","Park"}共六类
+    {
+        if(mvision_obs_->obj(i).con()>=objectCon)
+        {
+            if(mvision_obs_->obj(i).w()>=objectW)
+            {
+                objectID=i;
+                //objectCon =mvision_obs_->obj(i).con();
+                objectW = mvision_obs_->obj(i).w();
+            }
+        }
+    }
+    if(mvision_obs_->obj(objectID).type()=="Speed10")
+        flag=0;
+    else if(mvision_obs_->obj(objectID).type()=="Speed5")
+        flag=1;
+    else if(mvision_obs_->obj(objectID).type()=="Left")
+        flag=2;
+    else if(mvision_obs_->obj(objectID).type()=="Right")
+        flag=3;
+    else if(mvision_obs_->obj(objectID).type()=="Nopassing")
+        flag=4;
+    else if(mvision_obs_->obj(objectID).type()=="Park")
+        flag=5;
+    else
+        flag =200;
+
+    if(flag==0)
+    {
+        ServiceCarStatus.iTrafficsignSpeed=10.0;
+        ServiceCarStatus.itrafficsignTurn=200;
+    }
+    else if(flag==1)
+    {
+        ServiceCarStatus.iTrafficsignSpeed=5.0;
+        ServiceCarStatus.itrafficsignTurn=200;
+    }
+    else if(flag==4)
+    {
+        ServiceCarStatus.iTrafficsignSpeed=0.0;
+        ServiceCarStatus.itrafficsignTurn=200;
+    }
+    else if(flag==5)
+    {
+        ServiceCarStatus.iTrafficsignSpeed=0.0;
+        ServiceCarStatus.itrafficsignTurn=200;
+    }
+    else if(flag==2)
+    {
+        ServiceCarStatus.iTrafficsignSpeed=200.0;
+        ServiceCarStatus.itrafficsignTurn=2;
+    }
+    else if(flag==3)
+    {
+        ServiceCarStatus.iTrafficsignSpeed=200.0;
+        ServiceCarStatus.itrafficsignTurn=3;
+    }
+    else
+    {
+        ServiceCarStatus.iTrafficsignSpeed=200.0;
+        ServiceCarStatus.itrafficsignTurn=200;
+    }
+
+//      if((flag==0)||(flag==1)||(flag==4)|(flag==5))
+//      {
+//          switch(flag)
+//          {
+//              case 0:
+//                    ServiceCarStatus.iTrafficsignSpeed=min(10.0 , ServiceCarStatus.iTrafficsignSpeed);
+//              case 1:
+//                    ServiceCarStatus.iTrafficsignSpeed=min(5.0,ServiceCarStatus.iTrafficsignSpeed);
+//              case 4:
+//                    ServiceCarStatus.iTrafficsignSpeed=min(0.0,ServiceCarStatus.iTrafficsignSpeed);
+//              case 5:
+//                    ServiceCarStatus.iTrafficsignSpeed=min(0.0,ServiceCarStatus.iTrafficsignSpeed);
+//              default:
+//                    ServiceCarStatus.iTrafficsignSpeed=200.0;
+//          }
+//      }
+//      else if((flag==2)||(flag==3))
+//      {
+//          if(flag==2)
+//          {
+//               ServiceCarStatus.itrafficsignTurn=2;
+//          }
+//           else if(flag==3)
+//          {
+//               ServiceCarStatus.itrafficsignTurn=3;
+//          }
+//      }
+
+//    }
+    //givlog->debug("decition_brain","flag is : %d",flag);
+    //givlog->debug("decition_brain","itrafficsignTurn is : %d",ServiceCarStatus.itrafficsignTurn);
+    //givlog->debug("decition_brain","iTrafficsignSpeed is : %f",ServiceCarStatus.iTrafficsignSpeed);
+
+    mMutexVison_.unlock();
+}
 
 void iv::decition::BrainDecition::UpdateGroupInfo(const char *pdata, const int ndatasize){
 
@@ -1192,7 +1493,7 @@ void iv::decition::BrainDecition::UpdateV2r(const char *pdata, const int ndatasi
        ServiceCarStatus.mbRunPause = group_message.mbpause();
 
 //      std::cout<<"enter  for butn %d  "<<ServiceCarStatus.mbRunPause <<std::endl;
-       //givlog->debug("decition_brain","mbRunPause is : %d",group_message.mbpause());
+       givlog->debug("decition_brain","group_message mbRunPause is : %d",group_message.mbpause());
     }
     if(group_message.has_speedlimit())
     {

src/decition/decition_brain_sf_changan_shenlan/decition/brain.h

@@ -48,6 +48,9 @@
 #include "v2r.pb.h"
 #include "carstate.pb.h"
 #include "groupmsg.pb.h"
+#include "cameraobject.pb.h"
+#include "cameraobjectarray.pb.h"
+#include "brainrunstate.pb.h"
 
 #include "fanyaapi.h"
 
@@ -132,12 +135,15 @@ namespace iv {
             void * mpaObsTraceLeft;
             void * mpaObsTraceRight;
             void * mpaCarstate;
-             void * mpv2r;//20211009,cxw
+            void * mpv2r;//20211009,cxw
+            void * mpaTrafficSign;//20230814,交通标志牌识别数据更新计时
+            void * mpaBrainRunState;
 
 
             void ShareDecition(iv::decition::Decition decition);
-            void ShareBrainstate(iv::brain::brainstate xbs);
+            void ShareBrainstate(iv::brain::brainstate & xbs);
             void ShareBraincarstate(iv::brain::carstate xbs);
+            void ShareBrainRunState(iv::brain::brainrunstate & xbs);
 
         private:
             void * mpaHMI;
@@ -162,10 +168,14 @@ namespace iv {
             void GetFusion(const char* pdata, const int ndatasize);
             void UpdateFusion(std::shared_ptr<iv::fusion::fusionobjectarray> fusion_obs);
             void UpdateV2r(const char *pdata, const int ndatasize);
+            void GetTrafficSign(const char* pdata, const int ndatasize);//20230814,交通标志牌识别
+            void UpdateTrafficSign(std::shared_ptr<iv::vision::cameraobjectarray> xtrafficsign);//20230814,交通标志牌识别数据更新计时
 
         private:
             std::shared_ptr<iv::fusion::fusionobjectarray>  mfusion_obs_;
             iv::LidarGridPtr fusion_ptr_ = NULL;
+            std::shared_ptr<iv::vision::cameraobjectarray>  mvision_obs_;//20230814,交通标志牌识别数据更新计时
+            QMutex mMutexVison_;//20230814,交通标志牌识别数据更新计时
             QMutex mMutex_;
 
 

src/decition/decition_brain_sf_changan_shenlan/decition/decide_gps_00.cpp

@@ -28,7 +28,7 @@ extern iv::Ivlog * givlog;
 #define AVOID_NEW 1
 iv::decition::DecideGps00::DecideGps00()
 {
-
+    std::cout<<" init gps00"<<std::endl;
 }
 iv::decition::DecideGps00::~DecideGps00()
 {
@@ -188,7 +188,7 @@ std::vector<std::vector<double>> doubledata;//大地坐标系下x,y,phi,delta，
 
 enum VehState { normalRun, park, readyPark, stopObs, avoidObs, preAvoid, avoiding, backOri,preBack,
                 waitAvoid ,shoushaTest,justRun, changingRoad, chaoche, chaocheBack, reverseCar,reversing, reverseCircle, reverseDirect,reverseArr,
-                dRever,dRever0,dRever1,dRever2,dRever3,dRever4} vehState,lastVehState;
+                dRever,dRever0,dRever1,dRever2,dRever3,dRever4,dReverTZD,dReverTZDing} vehState,lastVehState;
 
 
 std::vector<iv::Point2D>  gpsTraceAim; //记录变道后的目标路径，若车辆经frenet规划行驶至该路径上后，就关闭frenet规划
@@ -315,7 +315,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
         }
 
         double dis =  GetDistance(*gpsMapLine[0], *gpsMapLine[gpsMapLine.size()-1]);
-        if(dis<15){
+        if(dis<1){
             circleMode=true;
         }else{
             circleMode=false;
@@ -347,6 +347,13 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
     ServiceCarStatus.mavoidX=avoidXNew;
 
 
+
+    if(front_car_decide_avoid == false)    //geely logic yizhi
+    {
+        obstacle_avoid_flag =  0;
+    }
+
+
     if(ServiceCarStatus.msysparam.gpsOffset_y!=0 || ServiceCarStatus.msysparam.gpsOffset_x!=0){
         GPS_INS gpsOffset = Coordinate_UnTransfer(ServiceCarStatus.msysparam.gpsOffset_x, ServiceCarStatus.msysparam.gpsOffset_y, now_gps_ins);
         now_gps_ins.gps_x=gpsOffset.gps_x;
@@ -417,6 +424,13 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
     secSpeed = realspeed / 3.6;
 
 
+    if(ServiceCarStatus.mnParkType==2)//guosai pingtai yanzheng ceting
+    {
+        if(ServiceCarStatus.bocheMode &&(vehState != dReverTZD))
+        {
+            vehState = dReverTZD;
+        }
+    }
 
 
     //sidePark
@@ -438,6 +452,8 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
             ServiceCarStatus.bocheEnable=2;
         }
 
+
+
         if(ServiceCarStatus.bocheMode && vehState!=dRever &&  vehState!=dRever0  &&  vehState!=dRever1 && vehState!=dRever2
                 &&  vehState!=dRever3 && vehState!=dRever4 &&  vehState!=reverseArr )
         {
@@ -503,6 +519,41 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
         }
     }
 
+    if(ServiceCarStatus.mnParkType == 2)
+        ServiceCarStatus.bocheEnable=1;
+
+
+    if(vehState == dReverTZD)
+    {
+         Point2D pt = Coordinate_Transfer(now_gps_ins.gps_x,now_gps_ins.gps_y, aim_gps_ins);
+         if(pt.y<0.5)
+         {
+             if(abs(realspeed)>0.1)
+             {
+                 vehState = reverseArr;
+             }
+             gps_decition->wheel_angle = 0;
+             gps_decition->speed = dSpeed;
+             obsDistance=-1;
+             minDecelerate=min(minDecelerate,-0.5f);
+             phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
+             //	gps_decition->accelerator = 0;
+             return gps_decition;
+         }
+         else
+         {
+             gps_decition->wheel_angle = 0;
+             gps_decition->speed = dSpeed;
+             obsDistance=-1;
+             dSpeed = 2;
+             dSecSpeed = dSpeed / 3.6;
+             gps_decition->speed = dSpeed;
+             phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
+             //	gps_decition->accelerator = 0;
+             return gps_decition;
+         }
+    }
+
     // ChuiZhiTingChe
     if (vehState == reverseCar)
     {
@@ -513,7 +564,12 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
     if (vehState == reversing)
     {
         double dis = GetDistance(now_gps_ins, Compute00().nearTpoint);
-        if (dis<2.0)//0.5
+        iv::Point2D pt = iv::decition::Coordinate_Transfer(now_gps_ins.gps_x,now_gps_ins.gps_y,aim_gps_ins);
+        iv::Point2D ptnear = iv::decition::Coordinate_Transfer(Compute00().nearTpoint.gps_x,Compute00().nearTpoint.gps_y,aim_gps_ins);
+
+         double fdistonear = fabs(pt.x - ptnear.x);
+         if(fdistonear<0.5)  //将吉利项目中的停车逻辑移植过来
+//        if (dis<2.0)//0.5
         {
             vehState = reverseCircle;
             qiedianCount = true;
@@ -699,6 +755,8 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
         }
         else
         {
+            controlAng = Compute00().dBocheAngle*16.5;
+            gps_decition->wheel_angle = 0 - controlAng;
             if (qiedianCount && trumpet()<1000)
             {
                 //                gps_decition->brake = 10;
@@ -718,8 +776,8 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
                 phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
             }
 
-            controlAng = Compute00().dBocheAngle*16.5;
-            gps_decition->wheel_angle = 0 - controlAng;
+
+
             cout<<"farTpointDistance================"<<dis<<endl;
             return gps_decition;
         }
@@ -785,6 +843,9 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
             cout<<"到达远切点+++++++++++++++++++++++++++++++++"<<endl;
         }
         else {
+
+            controlAng = 0-Compute00().dBocheAngle*16.5;
+            gps_decition->wheel_angle = 0 - controlAng*0.95;
             if (qiedianCount && trumpet()<1000)
             {
                 //                gps_decition->brake = 10;
@@ -804,8 +865,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
                 phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
             }
 
-            controlAng = 0-Compute00().dBocheAngle*16.5;
-            gps_decition->wheel_angle = 0 - controlAng*0.95;
+
             cout<<"farTpointDistance================"<<dis<<endl;
             return gps_decition;
         }
@@ -933,32 +993,76 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
 
     double acc_end = 0.1;
     static double distoend=1000;
-    if((ServiceCarStatus.table_look_up_on==true)&&(hapo_Adapter->IsINterpolationOK()))
+//    if((ServiceCarStatus.table_look_up_on==true)&&(hapo_Adapter->IsINterpolationOK()))
+//    {
+//        if(PathPoint+1000>gpsMapLine.size())
+//        {
+//            distoend=computeDistToEnd(gpsMapLine,PathPoint);
+//        }
+//        else
+//        {
+//            distoend=1000;
+//        }
+//        if(!circleMode && distoend<50)
+//        {
+//            double nowspeed = realspeed/3.6;
+//            if((distoend<10)||(distoend<(nowspeed*nowspeed)+2))
+//            {
+//                if(distoend == 0.0)
+//                    distoend = 0.09;
+//                acc_end = (-1.0)*nowspeed*nowspeed/(2.0*distoend);
+//                if(acc_end<(-3.0))
+//                    acc_end = -3.0;
+//            }
+
+//            if((distoend < 2.1)||(PathPoint>=gpsMapLine.size()-2))
+//                acc_end = -0.5;
+//        }
+//    }
+    int useaccend = 1;
+    double mstopbrake=0.6;
+    if(((ServiceCarStatus.table_look_up_on==true)&&(hapo_Adapter->IsINterpolationOK()))||(useaccend == 1))
     {
-        if(PathPoint+1000>gpsMapLine.size())
-        {
-            distoend=computeDistToEnd(gpsMapLine,PathPoint);
-        }
-        else
-        {
-            distoend=1000;
-        }
-        if(!circleMode && distoend<50)
-        {
-            double nowspeed = realspeed/3.6;
-            if((distoend<10)||(distoend<(nowspeed*nowspeed)+2))
-            {
-                if(distoend == 0.0)
-                    distoend = 0.09;
-                acc_end = (-1.0)*nowspeed*nowspeed/(2.0*distoend);
-                if(acc_end<(-3.0))
-                    acc_end = -3.0;
-            }
+                if(PathPoint+1000>gpsMapLine.size()){
+                    distoend=computeDistToEnd(gpsMapLine,PathPoint);
+                }else{
+                    distoend=1000;
+                }
+                if(!circleMode && distoend<100){
+                        if(distoend<3.0)
+                        {
+                            std::cout<<"distoend:  "<<distoend<<std::endl;
+                        }
+                        double nowspeed = realspeed/3.6;
+                        double brakedis = 100;
+                        double stopbrake = mstopbrake;
+                        if(nowspeed>10)
+                        {
+                            brakedis = (nowspeed*nowspeed)/(2*2.0);
+                        }
+                        else
+                        {
+                            brakedis = (nowspeed*nowspeed)/(2*stopbrake)+3;
+                        }
+                        if((distoend<3)||(distoend<brakedis))
+                        {
+                            if(distoend == 0.0)distoend = 0.09;
+                            acc_end = (-1.0)*nowspeed*nowspeed/(2.0*distoend);
+                            if((acc_end<(-3.0))&&(nowspeed<10))acc_end = -3.0;
+                            dSecSpeed = sqrt(2.0 * fabs(acc_end) * distoend);
+                            dSpeed = dSecSpeed * 3.6;
+                        }
 
-            if((distoend < 2.1)||(PathPoint>=gpsMapLine.size()-2))
-                acc_end = -0.5;
-        }
-    }else{
+                        if((distoend <= 0.1)||(PathPoint>=gpsMapLine.size()-2))acc_end = (stopbrake + 0.1)*(-1.0);
+
+
+                        if(acc_end<0)minDecelerate = acc_end;
+
+                        givlog->debug("decition_brain","acc_end: %f",acc_end);
+
+                }
+    }
+    else{
         //                if(!circleMode && PathPoint>gpsMapLine.size()-brake_distance){
         //                        minDecelerate=-0.5;
         //                        std::cout<<"map finish brake"<<std::endl;
@@ -969,6 +1073,8 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
             static int BrakePoint=-1;
             static bool final_brake=false;
             static double distance_to_end=1000;
+            static bool enterTofinal=false;  //20230417,shenlan
+            static double enterTofinal_speed=200;
             if(PathPoint+forecast_final_point>gpsMapLine.size())
             {
                 distance_to_end=computeDistToEnd(gpsMapLine,PathPoint);
@@ -993,6 +1099,8 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
                 final_brake_lock=false;
                 brake_mode=false;
                 BrakePoint=-1;
+                enterTofinal=false;  //20230417,shenlan
+                enterTofinal_speed=200;
             }
             if(final_brake==true)
             {
@@ -1002,12 +1110,35 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
                 }
                 else
                 {
-                    dSpeed=min(dSpeed, 3.0);
+                    if(enterTofinal==false)  //20230417,shenlan)
+                    {
+                        enterTofinal_speed=realspeed;
+                        enterTofinal=true;
+                    }
+                    if(ServiceCarStatus.msysparam.mvehtype=="shenlan")
+                    {
+                       dSpeed=min(enterTofinal_speed, 3.0);
+                    }
+                    else
+                    {
+                       dSpeed=min(dSpeed, 3.0);
+                    }
                     final_brake_lock=true;
                     brake_mode=true;
-                    if(distance_to_end<0.8)
+                    if(ServiceCarStatus.msysparam.mvehtype=="shenlan")
+                    {
+                        if(distance_to_end<2.0)
+                        {
+                            minDecelerate=-0.7;
+                        }
+
+                    }
+                    else
                     {
-                        minDecelerate=-0.7;
+                        if(distance_to_end<0.8)
+                        {
+                            minDecelerate=-0.7;
+                        }
                     }
                 }
             }
@@ -1486,6 +1617,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);
@@ -1555,7 +1701,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
         }
         else
         {
-            vehState==normalRun;
+            vehState=normalRun;
         }
     }
 
@@ -1745,6 +1891,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
         {
             vehState=normalRun;
             obstacle_avoid_flag=0;
+           // avoidXNewPreVector.clear();//20230526,huifu
         }
     }
 
@@ -1826,13 +1973,13 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
     }
     else
     {
-        if(PathPoint+400<gpsMapLine.size()){
+        if(PathPoint+200<gpsMapLine.size()){   //400gaiwei 200
             int road_permit_sum=0;
-            for(int k=PathPoint;k<PathPoint+400;k++){
+            for(int k=PathPoint;k<PathPoint+200;k++){
                 if((gpsMapLine[k]->mbnoavoid==false))//&&(gpsMapLine[k]->roadSum>1))
                     road_permit_sum++;
             }
-            if(road_permit_sum>=400)
+            if(road_permit_sum>=200)
                 road_permit_avoid=true;
         }
     }
@@ -1842,9 +1989,17 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
     //shiyanbizhang 0929
     if(obsDistance>0&& (obsDistance<ServiceCarStatus.aocfDis)&&(obs_speed_for_avoid>-8.0)&&(obs_speed_for_avoid<3.0)&&(realspeed>0.01)//&& (avoid_speed_flag==true)        //
             &&(vehState==normalRun||vehState==backOri || vehState==avoiding) //&&(ObsTimeStart==-1)
-            && (gpsMapLine[PathPoint]->runMode==1)&& (road_permit_avoid==true)&&(gpsMapLine[PathPoint]->mode2!=3000)&&(gpsMapLine[PathPoint+300]->mode2!=3000)&&(gpsMapLine[PathPoint+100]->mode2!=3000)&&(gpsMapLine[PathPoint+300]->mode2!=23)){
+            && (gpsMapLine[PathPoint]->runMode==1)&& (road_permit_avoid==true)&&(gpsMapLine[PathPoint]->mode2!=3000)){
         //        ObsTimeStart=GetTickCount();
-        ObsTimeEnd+=1.0;
+        if((PathPoint + 300)<gpsMapLine.size())
+        {
+            if((gpsMapLine[PathPoint+300]->mode2!=3000)&&(gpsMapLine[PathPoint+100]->mode2!=3000)&&(gpsMapLine[PathPoint+300]->mode2!=23))
+            {
+                ObsTimeEnd+=1.0;
+            }
+
+        }
+
         //dSpeed = min(6.0,dSpeed);
         cout<<"\n====================preAvoid time count start==================\n"<<endl;
     }
@@ -1936,44 +2091,198 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
         double* avoidRight_p=&avoidRight_value;
         computeAvoidBoundary(roadOri,roadSum,gpsMapLine[PathPoint]->mfLaneWidth,ServiceCarStatus.msysparam.vehWidth,avoidLeft_p,avoidRight_p);
         avoidXNewPre=computeAvoidXDistance(lidarGridPtr,now_gps_ins, gpsMapLine,avoidLeft_value,avoidRight_value,avoidXNewLast);
+
+        givlog->debug("decition_brain","======avoidXNewPre==========:%f",avoidXNewPre);
+
+        //const int nAvoidPreCount = 100;
         if(avoidXNewPreVector.size()<5){
-            avoidXNewPreVector.push_back(avoidXNewPre);
-        }else{
-            if(avoidXNewPreVector[0]!=avoidXNewLast){
-                for(unsigned int i=1;i<avoidXNewPreVector.size();i++){
-                    if(avoidXNewPreVector[i]!=avoidXNewPreVector[0]){
-                        avoidXNewPreFilter=avoidXNewLast;
-                        break;
-                    }
-                    if(i==avoidXNewPreVector.size()-1)
-                        avoidXNewPreFilter=avoidXNewPreVector[0];
-                }
-            }
-            avoidXNewPreVector.clear();
-        }
-        if(avoidXNewPreFilter!=avoidXNewLast){
-            avoidXNew=avoidXNewPre;
-            if(avoidXNew<0)
-                turnLampFlag=-1;
-            else if(avoidXNew>0)
-                turnLampFlag=1;
-            else
-                turnLampFlag=0;
+                   avoidXNewPreVector.push_back(avoidXNewPre);
+               }else{
+                   if(avoidXNewPreVector[0]!=avoidXNewLast){
+                       for(unsigned int i=1;i<avoidXNewPreVector.size();i++){
+                           if(avoidXNewPreVector[i]!=avoidXNewPreVector[0]){
+                               avoidXNewPreFilter=avoidXNewLast;
+                               break;
+                           }
+                           if(i==avoidXNewPreVector.size()-1)
+                               avoidXNewPreFilter=avoidXNewPreVector[0];
+                       }
+                   }
+                   avoidXNewPreVector.clear();
+               }
+               if(avoidXNewPreFilter!=avoidXNewLast){
+                   avoidXNew=avoidXNewPre;
+                   if(avoidXNew<0)
+                       turnLampFlag=-1;
+                   else if(avoidXNew>0)
+                       turnLampFlag=1;
+                   else
+                       turnLampFlag=0;
+
+                   gpsTraceNow.clear();
+                   gpsTraceAvoidXY.clear();
+                   givlog->debug("decition_brain","avoidXNew: %f,avoidXNewLast: %f",
+                                 avoidXNew,avoidXNewLast);
+                   //gpsTraceAvoidXY = getGpsTraceOffsetAvoid(gpsTraceOri, avoidXNew,now_gps_ins);
+                   gpsTraceAvoidXY = splinePlanner->chooseRoadBySpline(now_gps_ins,gpsMapLine,PathPoint,obsSpline,avoidXNew,avoidXNewLast);
+                   vehState = avoiding;
+                   startAvoidGpsIns = now_gps_ins;
+                   obstacle_avoid_flag=1;
+                   hasCheckedAvoidLidar = false;
+                   avoidXNewLast=avoidXNew;
+                   avoidXNewPreFilter=avoidXNew;
+               }
+           }
 
-            gpsTraceNow.clear();
-            gpsTraceAvoidXY.clear();
-            givlog->debug("decition_brain","avoidXNew: %f,avoidXNewLast: %f",
-                          avoidXNew,avoidXNewLast);
-            //gpsTraceAvoidXY = getGpsTraceOffsetAvoid(gpsTraceOri, avoidXNew,now_gps_ins);
-            gpsTraceAvoidXY = splinePlanner->chooseRoadBySpline(now_gps_ins,gpsMapLine,PathPoint,obsSpline,avoidXNew,avoidXNewLast);
-            vehState = avoiding;
-            startAvoidGpsIns = now_gps_ins;
-            obstacle_avoid_flag=1;
-            hasCheckedAvoidLidar = false;
-            avoidXNewLast=avoidXNew;
-            avoidXNewPreFilter=avoidXNew;
-        }
-    }
+//        if(avoidXNewPreVector.size()<nAvoidPreCount){
+//            avoidXNewPreVector.push_back(avoidXNewPre);
+//        }
+//        else
+//        {
+//            avoidXNewPreVector.erase(avoidXNewPreVector.begin());
+//            avoidXNewPreVector.push_back(avoidXNewPre);
+//        }
+
+//        if(vehState == preAvoid)
+//        {
+//            avoidXNew = avoidXNewPre;
+//        }
+//        else
+//        {
+//            if(fabs(avoidXNewLast)>0.1)
+//            {
+
+//                bool bOriginSafe = true;
+
+//                if(avoidXNewPreVector.size()<nAvoidPreCount)
+//                {
+//                    bOriginSafe = false;
+//                }
+//                else
+//                {
+//                    unsigned int j;
+//                    unsigned int nSize = avoidXNewPreVector.size();
+//                    for(j=0;j<nSize;j++)
+//                    {
+//                        if(fabs(avoidXNewPreVector[j])>0.1)
+//                        {
+//                            bOriginSafe = false;
+//                            break;
+//                        }
+//                    }
+//                }
+//                if(bOriginSafe)
+//                {
+//                    avoidXNew = 0;
+// //                   avoidXNewPreVector.clear();
+//                }
+//                else
+//                {
+//                    unsigned int j;
+//                    unsigned int nSize = avoidXNewPreVector.size();
+//                    if(avoidXNewPreVector.size()==nAvoidPreCount)
+//                    {
+//                        double filter = 0;
+//                        for(j=0;j<nSize;j++)
+//                        {
+//                            if(fabs(avoidXNewPreVector[j])>0.1)
+//                            {
+//                                if(fabs(filter)<0.1)filter = avoidXNewPreVector[j];
+//                                else
+//                                {
+//                                    if(fabs(filter - avoidXNewPreVector[j])>0.1)
+//                                    {
+//                                        filter = 0;
+//                                        break;
+//                                    }
+//                                }
+//                            }
+//                        }
+//                        if(fabs(filter)<0.1)
+//                        {
+//                            avoidXNew = avoidXNewLast;
+//                        }
+//                        else
+//                        {
+//                            if(fabs(filter - avoidXNewLast)>=2.0)
+//                            {
+//                                avoidXNew = filter;
+//                            }
+//                            else
+//                            {
+//                                avoidXNew = avoidXNewLast;
+//                            }
+//                        }
+//                    }
+//                    else
+//                    {
+//                        avoidXNew = avoidXNewLast;
+//                    }
+//                }
+//            }
+//        }
+
+
+//        if(avoidXNew<0)
+//            turnLampFlag=-1;
+//        else if(avoidXNew>0)
+//            turnLampFlag=1;
+//        else
+//            turnLampFlag=0;
+
+//        if(fabs(avoidXNew - avoidXNewLast)>0.1)
+//        {
+//            gpsTraceNow.clear();
+//            gpsTraceAvoidXY.clear();
+//            givlog->debug("decition_brain","avoidXNew: %f,avoidXNewLast: %f",
+//                          avoidXNew,avoidXNewLast);
+//            //gpsTraceAvoidXY = getGpsTraceOffsetAvoid(gpsTraceOri, avoidXNew,now_gps_ins);
+//            gpsTraceAvoidXY = splinePlanner->chooseRoadBySpline(now_gps_ins,gpsMapLine,PathPoint,obsSpline,avoidXNew,avoidXNewLast);
+//            vehState = avoiding;
+//            startAvoidGpsIns = now_gps_ins;
+//            obstacle_avoid_flag=1;
+//            hasCheckedAvoidLidar = false;
+//            avoidXNewLast=avoidXNew;
+//        }
+
+//        if(avoidXNewPreVector.size()<5){
+//            avoidXNewPreVector.push_back(avoidXNewPre);
+//        }else{
+//            if(avoidXNewPreVector[0]!=avoidXNewLast){
+//                for(unsigned int i=1;i<avoidXNewPreVector.size();i++){
+//                    if(avoidXNewPreVector[i]!=avoidXNewPreVector[0]){
+//                        avoidXNewPreFilter=avoidXNewLast;
+//                        break;
+//                    }
+//                    if(i==avoidXNewPreVector.size()-1)
+//                        avoidXNewPreFilter=avoidXNewPreVector[0];
+//                }
+//            }
+//            avoidXNewPreVector.clear();
+//        }
+//        if(avoidXNewPreFilter!=avoidXNewLast){
+//            avoidXNew=avoidXNewPre;
+//            if(avoidXNew<0)
+//                turnLampFlag=-1;
+//            else if(avoidXNew>0)
+//                turnLampFlag=1;
+//            else
+//                turnLampFlag=0;
+
+//            gpsTraceNow.clear();
+//            gpsTraceAvoidXY.clear();
+//            givlog->debug("decition_brain","avoidXNew: %f,avoidXNewLast: %f",
+//                          avoidXNew,avoidXNewLast);
+//            //gpsTraceAvoidXY = getGpsTraceOffsetAvoid(gpsTraceOri, avoidXNew,now_gps_ins);
+//            gpsTraceAvoidXY = splinePlanner->chooseRoadBySpline(now_gps_ins,gpsMapLine,PathPoint,obsSpline,avoidXNew,avoidXNewLast);
+//            vehState = avoiding;
+//            startAvoidGpsIns = now_gps_ins;
+//            obstacle_avoid_flag=1;
+//            hasCheckedAvoidLidar = false;
+//            avoidXNewLast=avoidXNew;
+//            avoidXNewPreFilter=avoidXNew;
+//        }
+//    }
 //20230407,
 //    if((vehState == preAvoid)||((avoidXNew!=0)))
 //  //    if((vehState == preAvoid)||(avoidXNew!=0))
@@ -2479,13 +2788,37 @@ else
         }
     }
 
-    if(ServiceCarStatus.msysparam.mvehtype=="shenlan")
+
+
+    if(obsDistance == 0)obsDistance = -1;
+
+    //20230814,交通标志识别添加 start
+    if(ServiceCarStatus.mTrafficSignUpdateTimer.elapsed()>10*1000)
+     {
+        ServiceCarStatus.iTrafficsignSpeed=200;
+        ServiceCarStatus.itrafficsignTurn=200;
+     }
+
+    if(ServiceCarStatus.itrafficsignTurn==2)
     {
-        if(dSpeed>10.0)
-        {
-            dSpeed=10.0;   //shenlan bisai xiansu 10
-        }
+        gps_decition->leftlamp = true;
+        gps_decition->rightlamp = false;
+    }
+    else if(ServiceCarStatus.itrafficsignTurn==3)
+    {
+        gps_decition->leftlamp = false;
+        gps_decition->rightlamp = true;
+    }
+    else
+    {
+        ;
     }
+    dSpeed=min(dSpeed,ServiceCarStatus.iTrafficsignSpeed);
+    //20230814,交通标志识别添加 end
+
+
+    dSpeed = min(dSpeed,ServiceCarStatus.mfMaxSpeed); //Speed Limit By Max Speed
+
 
     phaseSpeedDecition(gps_decition, secSpeed, obsDistance, obsSpeed,now_gps_ins);
 
@@ -2669,6 +3002,8 @@ else
                          else
                              obsDistance_log=obsDistance;
                          QDateTime dt2=QDateTime::currentDateTime();
+                         double disToEndTrace =  GetDistance(now_gps_ins, *gpsMapLine[end_tracepoint]);
+                         double disToParkPoint =  GetDistance(now_gps_ins, aim_gps_ins);
                          outfile <<dt2.time().hour()<<":"<<dt2.time().minute()<<":"<<dt2.time().second()<<":"<<dt2.time().msec()<<","
                                  <<"Decide_Dspeed"  << ","  <<setprecision(2)<<dSpeed<< ","
                                  <<"Decide_ACC"  << ","  <<setprecision(2)<<gps_decition->torque<< ","
@@ -2676,6 +3011,8 @@ else
                                  <<"Vehicle_RealSpd"<< ","  <<setprecision(2)<<now_gps_ins.speed<< ","
   //                             <<"readyParkMode"<< ","<<readyParkMode<< ","
   //                             <<"readyZhucheMode"<< ","<<readyZhucheMode<< ","
+                                 <<"trace_disToEndTrace"<< ","<<disToEndTrace<< ","
+                                 <<"disToParkPoint"<< ","<<disToParkPoint<< ","
                                  <<"OBS_Distance"<< ","<<obsDistance_log<< ","
                                  <<"OBS_Speed"<< ","<<obs_speed_for_avoid<< ","
                                  <<"obsSpeed_fusion"<<","<<obsSpeed<<","
@@ -2696,6 +3033,12 @@ else
                                  <<"Vehicle_GPS_lng"<< ","<< setprecision(10)<<now_gps_ins.gps_lng<< ","
                                  <<"Vehicle_GPS_X"<< ","<< setprecision(10)<<now_gps_ins.gps_x<< ","
                                  <<"Vehicle_GPS_Y"<< ","<< setprecision(10)<<now_gps_ins.gps_y<< ","
+
+                                <<"aim_GPS_lat"<< ","<< setprecision(10)<<aim_gps_ins.gps_lat<< ","
+                                <<"aim_GPS_lng"<< ","<< setprecision(10)<<aim_gps_ins.gps_lng<< ","
+                                <<"aim_GPS_X"<< ","<< setprecision(10)<<aim_gps_ins.gps_x<< ","
+                                <<"aim_GPS_Y"<< ","<< setprecision(10)<<aim_gps_ins.gps_y<< ","
+
                                  <<"MAP_GPS_heading"<< ","<< setprecision(10)<<gpsMapLine[PathPoint]->ins_heading_angle<< ","
                                  <<"MAP_GPS_X"<< ","<< setprecision(10)<<gpsMapLine[PathPoint]->gps_x<< ","
                                  <<"MAP_GPS_Y"<< ","<< setprecision(10)<<gpsMapLine[PathPoint]->gps_y<< ","
@@ -2751,6 +3094,7 @@ void iv::decition::DecideGps00::initMethods(){
     hapo_Adapter = new HapoAdapter();
     sightseeing_Adapter = new SightseeingAdapter();
     changanshenlan_Adapter = new ChanganShenlanAdapter();
+    hunter_Adapter = new HunterAdapter();
 
 
     mpid_Controller.reset(pid_Controller);
@@ -2762,6 +3106,7 @@ void iv::decition::DecideGps00::initMethods(){
     mhapo_Adapter.reset(hapo_Adapter);
     msightseeing_Adapter.reset(sightseeing_Adapter);
     mchanganshenlan_Adapter.reset(changanshenlan_Adapter);
+    mhunter_Adapter.reset(hunter_Adapter);
 
     frenetPlanner = new FrenetPlanner();
     //    laneChangePlanner = new LaneChangePlanner();
@@ -2802,6 +3147,9 @@ void iv::decition::DecideGps00::phaseSpeedDecition(iv::decition::Decition  decit
     else if(ServiceCarStatus.msysparam.mvehtype=="shenlan"){
         changanshenlan_Adapter->getAdapterDeciton(gpsIns, gpsTraceNow, dSpeed, obsDis,obsSpeed,  decition->accelerator,ServiceCarStatus.mfCalAcc  ,changingDangWei, &decition);
     }
+    else if(ServiceCarStatus.msysparam.mvehtype=="hunter"){
+        hunter_Adapter->getAdapterDeciton(gpsIns, gpsTraceNow, dSpeed, obsDis,obsSpeed,  decition->accelerator,ServiceCarStatus.mfCalAcc  ,changingDangWei, &decition);
+    }
 }
 
 
@@ -2814,13 +3162,19 @@ std::vector<iv::Point2D> iv::decition::DecideGps00::getGpsTrace(iv::GPS_INS now_
     int tracesize=800;
     if(ServiceCarStatus.msysparam.mvehtype=="shenlan")
     {
-        tracesize=400;
+        if(gpsMapLine.size()<500) tracesize = 150;
+        else tracesize = 400;
     }
     else
     {
         tracesize=800;
     }
 
+    if(ServiceCarStatus.msysparam.mvehtype=="hunter")
+    {
+        tracesize=100;//400;
+    }
+
     if (gpsMapLine.size() > tracesize && PathPoint >= 0)
     {
         int aimIndex;
@@ -3707,6 +4061,7 @@ void iv::decition::DecideGps00::computeObsOnRoadXY( iv::GPS_INS now_gps_ins,iv::
     }
     else {
         //obsPoint = Compute00().getLidarObsPoint(gpsTrace, lidarGridPtr);
+
         obsPoint = Compute00().getLidarObsPointXY(gpsTrace,gpsTraceLeft,gpsTraceRight,lidarGridPtr);
         float lidarXiuZheng=0;
         if(!ServiceCarStatus.useMobileEye){
@@ -3722,6 +4077,10 @@ void iv::decition::DecideGps00::computeObsOnRoadXY( iv::GPS_INS now_gps_ins,iv::
         obsFrenet.s=obsFrenetMid.s-now_s_d.s;
         lidarDistance = obsFrenet.s;//obsPoint.y + lidarXiuZheng;   //激光距离推到车头  gjw20191110 lidar
         //    lidarDistance=-1;
+//        if(obsPoint.s < 0)
+//        {
+//           lidarDistance = -1;
+//        }
         if (lidarDistance<0)
         {
             lidarDistance = -1;
@@ -4213,7 +4572,7 @@ float  iv::decition::DecideGps00::ComputeTrafficLightSpeed(int traffic_light_col
     return  traffic_speed;
 }
 
-float  iv::decition::DecideGps00::ComputeTrafficLightSpeed(int traffic_light_color, int traffic_light_time,  const std::vector<GPSData> gpsMapLine,int traffic_light_pathpoint,
+float  iv::decition::DecideGps00:: ComputeTrafficLightSpeed(int traffic_light_color, int traffic_light_time,  const std::vector<GPSData> gpsMapLine,int traffic_light_pathpoint,
                                                            int pathpoint,float secSpeed,float dSpeed){
     float traffic_speed=200;
     float traffic_dis=0;
@@ -4250,7 +4609,7 @@ float  iv::decition::DecideGps00::ComputeTrafficLightSpeed(int traffic_light_col
 //        traffic_speed=0;
 //    }
 
-    if(traffic_light_color==2 && traffic_dis<10){  //cxw,1xhonglvdeng gaibian
+    if(traffic_light_color==2 && traffic_dis<ServiceCarStatus.tlStopDis){  //cxw,1xhonglvdeng gaibian
         traffic_speed=0;
 //        return  traffic_speed;
 //        if(traffic_dis>3.0)
@@ -4303,20 +4662,13 @@ float  iv::decition::DecideGps00::ComputeTrafficLightSpeed(int traffic_light_col
 
 
     default:
+        passEnable=true; //20230413
         break;
     }
 
     if(!passEnable){
-        if(traffic_dis<5){
+        if(traffic_dis<ServiceCarStatus.tlStopDis){
             traffic_speed=0;
-        }else if(traffic_dis<10){
-            traffic_speed=5;
-        }else if(traffic_dis<20){
-            traffic_speed=15;
-        }else if(traffic_dis<30){
-            traffic_speed=25;
-        }else if(traffic_dis<50){
-            traffic_speed=30;
         }
     }
     return traffic_speed;
@@ -4582,18 +4934,40 @@ float iv::decition::DecideGps00::ComputeV2XTrafficLightSpeed(iv::TrafficLight tr
         traffic_time = trafficLight.straightTime;
     }
 
-    passThrough = computeTrafficPass(nearTrafficDis,traffic_color,traffic_time,dSecSpeed);
-    if(passThrough)
-    {
+//    passThrough = computeTrafficPass(nearTrafficDis,traffic_color,traffic_time,dSecSpeed);
+//    if(passThrough)
+//    {
+//        return trafficSpeed;
+//    }
+//    else
+//    {
+//        trafficSpeed = computeTrafficSpeedLimt(nearTrafficDis);
+//        if(nearTrafficDis < 6)
+//        {
+//            float decelerate = 0 - ( secSpeed * secSpeed * 0.5 / nearTrafficDis);
+//            minDecelerate = min(minDecelerate,decelerate);
+//        }
+//        return trafficSpeed;
+//    }
+    double fTrafficBrake = 1.0;  //移植吉利项目的v2x
+    passThrough=computeTrafficPass(nearTrafficDis,traffic_color,traffic_time,secSpeed,dSecSpeed);
+    if(passThrough){
         return trafficSpeed;
-    }
-    else
-    {
-        trafficSpeed = computeTrafficSpeedLimt(nearTrafficDis);
-        if(nearTrafficDis < 6)
-        {
-            float decelerate = 0 - ( secSpeed * secSpeed * 0.5 / nearTrafficDis);
-            minDecelerate = min(minDecelerate,decelerate);
+    }else{
+        trafficSpeed=computeTrafficSpeedLimt(nearTrafficDis);
+        if(nearTrafficDis<(dSecSpeed*dSecSpeed/(2*fTrafficBrake))){
+            if(fabs(nearTrafficDis)<1.0)
+            {
+                minDecelerate = -1.0 * fTrafficBrake;
+            }
+            else
+            {
+                if((0.5*secSpeed*secSpeed/fTrafficBrake) > (nearTrafficDis*0.9))
+                {
+                    float decelerate =0-( secSpeed*secSpeed*0.5/(nearTrafficDis-0.5));
+                    minDecelerate=min(minDecelerate,decelerate);
+                }
+            }
         }
         return trafficSpeed;
     }
@@ -4601,9 +4975,12 @@ float iv::decition::DecideGps00::ComputeV2XTrafficLightSpeed(iv::TrafficLight tr
     return trafficSpeed;
 }
 
-bool iv::decition::DecideGps00::computeTrafficPass(float trafficDis,int trafficColor,float trafficTime,float dSecSpeed)
+//bool iv::decition::DecideGps00::computeTrafficPass(float trafficDis,int trafficColor,float trafficTime,float dSecSpeed)
+bool iv::decition::DecideGps00::computeTrafficPass(float trafficDis,int trafficColor,float trafficTime,float realSecSpeed,float dSecSpeed)
 {
     float passTime=0;
+    double fAcc = 0.6;
+    if(dSecSpeed == 0)return true;
     if (trafficColor == 2 || trafficColor == 3)
     {
         return false;
@@ -4617,6 +4994,16 @@ bool iv::decition::DecideGps00::computeTrafficPass(float trafficDis,int trafficC
         passTime=trafficDis/dSecSpeed;
         if(passTime+1< trafficTime)
         {
+            double passTime2= trafficTime - 0.1;
+            double disthru = realSecSpeed * passTime2 + 0.5 * fAcc * pow(passTime2,2);
+            if((disthru -1.0)< trafficDis )
+            {
+                if((realSecSpeed<3.0)&&(trafficDis>10))
+                {
+                    return true;
+                }
+                return false;
+            }
             return true;
         }
         else
@@ -4629,6 +5016,16 @@ bool iv::decition::DecideGps00::computeTrafficPass(float trafficDis,int trafficC
 float iv::decition::DecideGps00::computeTrafficSpeedLimt(float trafficDis)
 {
     float limit=200;
+    double fBrake = 1.0;
+    if(trafficDis < 1.0)
+    {
+        limit =0;
+    }
+    else
+    {
+        limit = 3.6*sqrt(2.0* fabs(fBrake) * (trafficDis-1.0) );
+    }
+    return limit;
     if(trafficDis<10)
     {
         limit = 0;

src/decition/decition_brain_sf_changan_shenlan/decition/decide_gps_00.h

@@ -17,6 +17,7 @@
 #include <decition/adc_adapter/hapo_adapter.h>
 #include <decition/adc_adapter/changan_shenlan_adapter.h>
 #include <decition/adc_adapter/sightseeing_adapter.h>
+#include <decition/adc_adapter/hunter_adapter.h>
 #include "perception/perceptionoutput.h"
 #include "ivlog.h"
 #include <memory>
@@ -157,6 +158,7 @@ public:
     BaseAdapter *hapo_Adapter;
     BaseAdapter *sightseeing_Adapter;
     BaseAdapter *changanshenlan_Adapter;
+    BaseAdapter *hunter_Adapter;
 
     std::shared_ptr<BaseController> mpid_Controller;
     std::shared_ptr<BaseAdapter> mge3_Adapter;
@@ -167,6 +169,7 @@ public:
     std::shared_ptr<BaseAdapter> mhapo_Adapter;
     std::shared_ptr<BaseAdapter> msightseeing_Adapter;
     std::shared_ptr<BaseAdapter> mchanganshenlan_Adapter;
+    std::shared_ptr<BaseAdapter> mhunter_Adapter;
 
 	FrenetPlanner *frenetPlanner;
     SplinePlanner *splinePlanner;
@@ -176,6 +179,9 @@ public:
 //    std::shared_ptr<BasePlanner> mlaneChangePlanner;
 
 
+    double mstopbrake = 0.6;
+
+
     Decition getDecideFromGPS(GPS_INS gpsIns, const std::vector<GPSData> navigation_data, iv::LidarGridPtr lidarGridPtr,
                               std::vector<iv::Perception::PerceptionOutput> lidar_per,iv::TrafficLight trafficLight);
 
@@ -241,7 +247,8 @@ public:
                                    int pathpoint,float secSpeed,float dSpeed,bool circleMode);
     float getTrafficLightSpeed(int traffic_light_color, int traffic_light_time, float dis);
 
-    bool computeTrafficPass(float trafficDis,int trafficColor,float trafficTime,float dSecSpeed);
+    //bool computeTrafficPass(float trafficDis,int trafficColor,float trafficTime,float dSecSpeed);
+    bool computeTrafficPass(float trafficDis,int trafficColor,float trafficTime,float realSecSpeed,float dSecSpeed);
     float computeTrafficSpeedLimt(float trafficDis);
     float ComputeTrafficLightSpeed(int traffic_light_color, int traffic_light_time,  const std::vector<GPSData> gpsMapLine,int traffic_light_pathpoint,
                                                                int pathpoint,float secSpeed,float dSpeed);

src/decition/decition_brain_sf_changan_shenlan/decition/decide_gps_00_copy.cpp

@@ -0,0 +1,5406 @@
+#include <decition/decide_gps_00.h>
+#include <decition/adc_tools/compute_00.h>
+#include <decition/adc_tools/gps_distance.h>
+#include <decition/decition_type.h>
+#include <decition/adc_tools/transfer.h>
+#include <decition/obs_predict.h>
+#include <common/constants.h>
+#include <common/car_status.h>
+#include <math.h>
+#include <iostream>
+#include <fstream>
+#include <control/can.h>
+#include <time.h>
+#include <decition/adc_controller/base_controller.h>
+#include <decition/adc_controller/pid_controller.h>
+#include <decition/adc_planner/lane_change_planner.h>
+#include <decition/adc_planner/frenet_planner.h>
+#include <QTime>
+#include <iomanip>
+
+using namespace std;
+
+
+#include "ivlog.h"
+extern iv::Ivlog * givlog;
+
+#define PI (3.1415926535897932384626433832795)
+#define AVOID_NEW 1
+iv::decition::DecideGps00::DecideGps00()
+{
+    std::cout<<" init gps00"<<std::endl;
+}
+iv::decition::DecideGps00::~DecideGps00()
+{
+
+}
+
+float iv::decition::DecideGps00::xiuzhengCs=0;
+
+int iv::decition::DecideGps00::PathPoint = -1;
+double iv::decition::DecideGps00::minDis = iv::MaxValue;
+double iv::decition::DecideGps00::maxAngle = iv::MinValue;
+int iv::decition::DecideGps00::lastGpsIndex = 0;
+double iv::decition::DecideGps00::controlSpeed = 0;
+double iv::decition::DecideGps00::controlBrake = 0;
+double iv::decition::DecideGps00::controlAng = 0;
+double iv::decition::DecideGps00::Iv = 0;
+double iv::decition::DecideGps00::lastU = 0;
+double iv::decition::DecideGps00::lastVt = 0;
+double iv::decition::DecideGps00::lastEv = 0;
+
+int iv::decition::DecideGps00::gpsLineParkIndex = 0;
+int iv::decition::DecideGps00::gpsMapParkIndex = 0;
+double iv::decition::DecideGps00::lastDistance = MaxValue;
+double iv::decition::DecideGps00::lastPreObsDistance = MaxValue;
+double iv::decition::DecideGps00::obsDistance = -1;
+double iv::decition::DecideGps00::obsSpeed=0;
+double iv::decition::DecideGps00::obsDistanceAvoid = -1;
+double iv::decition::DecideGps00::lastDistanceAvoid = -1;
+
+double iv::decition::DecideGps00::lidarDistance = -1;
+double iv::decition::DecideGps00::myesrDistance = -1;
+double iv::decition::DecideGps00::lastLidarDis = -1;
+
+bool iv::decition::DecideGps00::parkMode = false;
+bool iv::decition::DecideGps00::readyParkMode = false;
+
+bool iv::decition::DecideGps00::zhucheMode = false;
+bool iv::decition::DecideGps00::readyZhucheMode = false;
+bool iv::decition::DecideGps00::hasZhuched = false;
+
+double iv::decition::DecideGps00::lastLidarDisAvoid = -1;
+double iv::decition::DecideGps00::lidarDistanceAvoid = -1;
+
+int iv::decition::DecideGps00::finishPointNum = -1;
+int iv::decition::DecideGps00::zhuchePointNum = 0;
+
+///kkcai, 2020-01-03
+bool iv::decition::DecideGps00::isFirstRun = true;
+//////////////////////////////////////////////
+
+float iv::decition::DecideGps00::minDecelerate=0;
+//bool iv::decition::DecideGps00::startingFlag = true;//起步标志，在起步时需要进行frenet规划。
+
+double offset_real = 0;
+double realspeed;
+double dSpeed;
+double dSecSpeed;
+double secSpeed;
+double ac;
+
+
+iv::Point2D obsPoint(-1, -1);
+iv::Point2D obsPointAvoid(-1, -1);
+
+
+int esrIndex = -1;
+int esrIndexAvoid = -1;
+double obsSpeedAvoid = 0;
+
+double esrDistance = -1;
+double esrDistanceAvoid = -1;
+int obsLostTime = 0;
+int obsLostTimeAvoid = 0;
+
+// double avoidTime = 0;
+
+double avoidXNewPre=0,avoidXNewPreFilter=0;//20230213,障碍物检测精度由1米换成0.5米
+//vector<int> avoidXNewPreVector;
+//int avoidXNew=0;
+//int avoidXNewLast=0;
+vector<double> avoidXNewPreVector;
+double avoidXNew=0;
+double avoidXNewLast=0;  //20230213,障碍物检测精度由1米换成0.5米
+double avoidX =0;
+int    turnLampFlag=0;  //  if <0:left , if >0:right
+float  roadWidth = 3.5;
+int roadSum =10;
+int roadNow = 0;
+int roadOri =0;
+int roadPre = -1;
+int lastRoadOri = 0;
+
+int lightTimes = 0;
+
+
+int lastRoadNum=1;
+
+int stopCount = 0;
+
+int gpsMissCount = 0;
+
+bool rapidStop = false;
+
+bool hasTrumpeted = false;
+
+
+bool changeRoad=false;
+//实验手刹
+bool handFirst = true;
+double handTimeSpan = 0;
+double handStartTime = 0;
+double handLastTime = 0;
+bool handPark = false;
+long handParkTime=10000;
+
+//喇叭
+bool trumpetFirstCount = true;
+double trumpetTimeSpan = 0;
+double trumpetStartTime = 0;
+double trumpetLastTime = 0;
+
+//过渡
+bool transferFirstCount = true;
+double transferTimeSpan = 0;
+double transferStartTime = 0;
+double transferLastTime = 0;
+
+bool busMode = false;
+bool parkBesideRoad = false;
+
+bool chaocheMode = false;
+bool specialSignle = false;
+
+double offsetX = 0;
+
+double steerSpeed=9000;
+
+bool transferPieriod;
+bool transferPieriod2;
+double traceDevi;
+
+bool startingFlag = true;  //起步标志，在起步时需要进行frenet规划。
+bool useFrenet = false;    //标志是否启用frenet算法避障
+bool useOldAvoid = true;   //标志是否用老方法避障
+bool front_car_decide_avoid=true;  //前后车距大于30米，后车允许根据自己障碍物情况避障，否则只能听从前车
+bool road_permit_avoid=false;  //true: 道路允许避障，false:道路不允许避障
+
+//数据存储功能 ,20210903,cxw
+bool file_cnt_add_en =false;  //用于提示是否需要将文件计数值增加
+bool file_cnt_add=false;
+bool map_start_point = true;
+bool first_start_en=true;  //自主巡迹数据存储用
+int  start_tracepoint;
+int  end_tracepoint;
+
+std::vector<std::vector<double>> doubledata;//大地坐标系下x,y,phi,delta，速度规划用变量
+
+enum VehState { normalRun, park, readyPark, stopObs, avoidObs, preAvoid, avoiding, backOri,preBack,
+                waitAvoid ,shoushaTest,justRun, changingRoad, chaoche, chaocheBack, reverseCar,reversing, reverseCircle, reverseDirect,reverseArr,
+                dRever,dRever0,dRever1,dRever2,dRever3,dRever4,dReverTZD,dReverTZDing} vehState,lastVehState;
+
+
+std::vector<iv::Point2D>  gpsTraceAim; //记录变道后的目标路径，若车辆经frenet规划行驶至该路径上后，就关闭frenet规划
+std::vector<iv::Point2D>  gpsTraceAvoid,gpsTraceNow,gpsTraceBack,gpsTracePre,gpsTraceOri,gpsTraceRear,gpsTraceNowLeft,gpsTraceNowRight,gpsTraceAvoidXY,gpsTraceMapOri;
+
+std::vector<double> esrDisVector(roadSum, -1);
+std::vector<double> lidarDisVector(roadSum, -1);
+std::vector<double> obsDisVector(roadSum,-1);
+std::vector<double> obsSpeedVector(roadSum, 0);
+
+std::vector<int> obsLostTimeVector(roadSum, 0);
+
+std::vector<double> lastLidarDisVector(roadSum, -1);
+std::vector<double> lastDistanceVector(roadSum, -1);
+
+std::vector<iv::Point2D> traceOriLeft,traceOriRight;
+
+bool qiedianCount = false;
+
+static int obstacle_avoid_flag=0;
+static int front_car_id=-1;
+static int front_car_vector_id=-1;
+static bool final_brake_lock=false,brake_mode=false;
+std::vector<iv::Point2D>  obsSpline;
+//日常展示
+
+#include <QDateTime>
+
+iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_gps_ins,
+                                                                   const std::vector<GPSData> gpsMapLine,
+                                                                   iv::LidarGridPtr lidarGridPtr,
+                                                                   std::vector<iv::Perception::PerceptionOutput> lidar_per,
+                                                                   iv::TrafficLight trafficLight)
+{
+    Decition gps_decition(new DecitionBasic);
+
+    bool bgroupgrpc = false;
+    qint64 ngrpcvalid = 3000;
+    iv::group::groupinfo xgroupgrpcinfo;
+    if((QDateTime::currentMSecsSinceEpoch() - ServiceCarStatus.mgroupgrpcupdatetime ) < ngrpcvalid)
+    {
+        ServiceCarStatus.mMutexgroupgrpc.lock();
+        xgroupgrpcinfo.CopyFrom(ServiceCarStatus.mgroupgrpcinfo);
+        ServiceCarStatus.mMutexgroupgrpc.unlock();
+        bgroupgrpc = true;
+    }
+    if((bgroupgrpc==true)&&(ServiceCarStatus.curID>1)){
+        for(int i=0;i<xgroupgrpcinfo.mvehinfo_size();i++){
+            if((xgroupgrpcinfo.mutable_mvehinfo(i)->intragroupid()+1)==ServiceCarStatus.curID){
+                front_car_id=xgroupgrpcinfo.mutable_mvehinfo(i)->intragroupid();
+                front_car_vector_id=i;
+            }
+        }
+    }else{
+        front_car_id=-1;
+    }
+
+    if(front_car_id>0){
+        if(xgroupgrpcinfo.mutable_mvehinfo(front_car_vector_id)->has_mgpsimu()){
+            front_car.front_car_ins.gps_lat=xgroupgrpcinfo.mutable_mvehinfo(front_car_vector_id)->mgpsimu().lat();
+            front_car.front_car_ins.gps_lng=xgroupgrpcinfo.mutable_mvehinfo(front_car_vector_id)->mgpsimu().lon();
+            front_car.front_car_ins.ins_heading_angle=xgroupgrpcinfo.mutable_mvehinfo(front_car_vector_id)->mgpsimu().heading();
+            front_car.front_car_ins.speed=xgroupgrpcinfo.mutable_mvehinfo(front_car_vector_id)->mgpsimu().speed();
+            GaussProjCal(front_car.front_car_ins.gps_lng,front_car.front_car_ins.gps_lat, &front_car.front_car_ins.gps_x, &front_car.front_car_ins.gps_y);
+            front_car.front_car_dis=GetDistance(now_gps_ins,front_car.front_car_ins);
+        }else{
+            front_car.front_car_dis=500;
+        }
+        if(xgroupgrpcinfo.mutable_mvehinfo(front_car_vector_id)->has_mcarstate()){
+            front_car.vehState=xgroupgrpcinfo.mutable_mvehinfo(front_car_vector_id)->mcarstate().mstate();
+            int avoidX_record=xgroupgrpcinfo.mutable_mvehinfo(front_car_vector_id)->mcarstate().mavoidx();
+            if(front_car.vehState!=0&&avoidX_record!=0)
+                front_car.avoidX=avoidX_record;
+            else
+                front_car.avoidX=0;
+            front_car.obs_distance=xgroupgrpcinfo.mutable_mvehinfo(front_car_vector_id)->mcarstate().mobsdistance();
+            front_car.obs_speed=xgroupgrpcinfo.mutable_mvehinfo(front_car_vector_id)->mcarstate().mobsspeed();
+        }
+        if(ServiceCarStatus.txt_log_on==true)
+        {
+            givlog->debug("decition_brain","front_car_id: %d,FrontState: %d,FrontAvoidX: %d,FrontDis: %f,FrontObs: %f",
+                      front_car_id,front_car.vehState,front_car.avoidX,front_car.front_car_dis,front_car.obs_distance);
+        }
+    }
+
+    if(front_car_id>0){
+        if(front_car.front_car_dis>35){
+            front_car_decide_avoid=true;
+        }else if((front_car.front_car_dis<=35)&&(front_car.avoidX==0)){
+            front_car_decide_avoid=false;
+        }else if((front_car.front_car_dis<=35)&&(front_car.avoidX!=0)){
+            front_car_decide_avoid=true;
+        }
+    }else{
+        front_car_decide_avoid=true;
+    }
+
+
+    static int file_num;//log文件存储计数
+    if (isFirstRun)
+    {
+        file_num=0;
+        initMethods();
+        vehState = normalRun;
+        startAvoid_gps_ins = now_gps_ins;
+        init();
+        PathPoint = Compute00().getFirstNearestPointIndex(now_gps_ins, gpsMapLine,
+                                                          DecideGps00::lastGpsIndex,
+                                                          DecideGps00::minDis,
+                                                          DecideGps00::maxAngle);
+        DecideGps00::lastGpsIndex = PathPoint;
+
+
+        if(ServiceCarStatus.speed_control==true){
+            Compute00().getPlanSpeed(gpsMapLine);
+        }
+
+        double dis =  GetDistance(*gpsMapLine[0], *gpsMapLine[gpsMapLine.size()-1]);
+        if(dis<1){
+            circleMode=true;
+        }else{
+            circleMode=false;
+        }
+
+        getV2XTrafficPositionVector(gpsMapLine);
+
+        ServiceCarStatus.bocheMode=false;
+        ServiceCarStatus.daocheMode=false;
+        parkMode=false;
+        readyParkMode=false;
+        finishPointNum=-1;
+        roadNowStart=true;
+        isFirstRun = false;
+        obstacle_avoid_flag=0;
+        avoidXNew=0;
+        avoidXNewLast=0;
+        avoidXNewPre=0;
+        avoidXNewPreFilter=0;
+        gpsMapLine[gpsMapLine.size()-1]->frenet_s=0;
+
+        if((ServiceCarStatus.txt_log_on==true)&&(ServiceCarStatus.msysparam.mvehtype=="hapo"))
+        {
+
+            givlog->debug("decition_brain_bool","DoorTimeStart: %d,current: %d",0,0);
+        }
+    }
+
+    ServiceCarStatus.mvehState=vehState;
+    ServiceCarStatus.mavoidX=avoidXNew;
+
+    changingDangWei=false;
+    minDecelerate=0;
+
+    busMode = ServiceCarStatus.busmode;
+    nearStation=false;
+
+    gps_decition->leftlamp = false;
+    gps_decition->rightlamp = false;
+
+    xiuzhengCs=0;
+    is_arrivaled = false;
+
+    realspeed = now_gps_ins.speed;//km/h
+    secSpeed = realspeed / 3.6;//m/s
+
+
+    if(front_car_decide_avoid == false)    //geely logic yizhi
+    {
+        obstacle_avoid_flag =  0;
+    }
+
+
+    if(ServiceCarStatus.msysparam.gpsOffset_y!=0 || ServiceCarStatus.msysparam.gpsOffset_x!=0){
+        GPS_INS gpsOffset = Coordinate_UnTransfer(ServiceCarStatus.msysparam.gpsOffset_x, ServiceCarStatus.msysparam.gpsOffset_y, now_gps_ins);
+        now_gps_ins.gps_x=gpsOffset.gps_x;
+        now_gps_ins.gps_y=gpsOffset.gps_y;
+        GaussProjInvCal(now_gps_ins.gps_x,  now_gps_ins.gps_y, &now_gps_ins.gps_lng, &now_gps_ins.gps_lat);
+    }
+
+
+    if(now_gps_ins.gps_lat<0||now_gps_ins.gps_lat>90)
+    {
+        gps_decition->wheel_angle = 0.2;
+        gps_decition->speed = dSpeed;
+
+        gps_decition->accelerator = -0.5;
+        gps_decition->torque = 0.0;
+//        gps_decition->brake=10;
+        if(ServiceCarStatus.msysparam.mvehtype=="shenlan")  //长安深蓝决策发的brake是负值的时候才刹车，利用的是扭矩
+        {
+          gps_decition->brake=-10;
+        }
+        else
+        {
+           gps_decition->brake=10;
+        }
+        givlog->debug("decition_brain","gps_lat is error,f%",now_gps_ins.gps_lat); //20230315
+        return gps_decition;
+    }
+
+    if(ServiceCarStatus.daocheMode){
+
+        now_gps_ins.ins_heading_angle=now_gps_ins.ins_heading_angle+180;
+        if( now_gps_ins.ins_heading_angle>=360)
+            now_gps_ins.ins_heading_angle= now_gps_ins.ins_heading_angle-360;
+    }
+    //    qDebug("heading is %g",now_gps_ins.ins_heading_angle);
+
+/****************************self park logic begin************************************************/
+    aim_gps_ins.gps_lat= ServiceCarStatus.mfParkLat;
+    aim_gps_ins.gps_lng= ServiceCarStatus.mfParkLng;
+    aim_gps_ins.ins_heading_angle= ServiceCarStatus.mfParkHeading;
+    GaussProjCal(aim_gps_ins.gps_lng,aim_gps_ins.gps_lat, &aim_gps_ins.gps_x, &aim_gps_ins.gps_y);
+
+    //sidePark
+    if(ServiceCarStatus.mnParkType==1)
+    {
+        if( vehState!=dRever &&  vehState!=dRever0  &&  vehState!=dRever1 &&  vehState!=dRever2
+                &&  vehState!=dRever3 && vehState!=dRever4 && vehState!=reverseArr )
+        {
+            int bocheEN=Compute00().bocheDirectCompute(now_gps_ins, aim_gps_ins);
+            if(bocheEN==1){
+                ServiceCarStatus.bocheEnable=1;
+            }else if(bocheEN==0){
+                ServiceCarStatus.bocheEnable=0;
+            }
+        }
+        else
+        {
+            ServiceCarStatus.bocheEnable=2;
+        }
+
+
+
+        if(ServiceCarStatus.bocheMode && vehState!=dRever &&  vehState!=dRever0  &&  vehState!=dRever1 && vehState!=dRever2
+                &&  vehState!=dRever3 && vehState!=dRever4 &&  vehState!=reverseArr )
+        {
+            if(abs(realspeed)>0.1)
+            {
+                dSpeed=0;
+                minDecelerate=min(minDecelerate,-0.5f);
+                phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
+                gps_decition->wheel_angle=0;
+                return gps_decition;
+            }else
+            {
+                if(trumpet()>3000)
+                {
+                    trumpetFirstCount=true;
+                    vehState=dRever;
+                }
+            }
+        }
+
+        if (vehState == dRever)
+        {
+            GaussProjCal(aim_gps_ins.gps_lng, aim_gps_ins.gps_lat, &aim_gps_ins.gps_x, &aim_gps_ins.gps_y);
+            Compute00().bocheDirectCompute(now_gps_ins, aim_gps_ins);
+            vehState = dRever0;
+            qiedianCount=true;
+            std::cout<<"enter side boche mode"<<std::endl;
+        }
+        if (vehState == dRever0)
+        {
+            double dis = GetDistance(now_gps_ins, Compute00().dTpoint0);
+            if (dis<2.0)
+            {
+                vehState = dRever1;
+                qiedianCount = true;
+                cout<<"到达近切点+++++++++++++++++++++++++++++++++"<<endl;
+            }
+            else
+            {
+                controlAng = 0;
+                dSpeed = 2;
+                dSecSpeed = dSpeed / 3.6;
+                gps_decition->wheel_angle = 0;
+                gps_decition->speed = dSpeed;
+                obsDistance=-1;
+                phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
+                //	gps_decition->accelerator = 0;
+                return gps_decition;
+            }
+        }
+
+        if (vehState == dRever1)
+        {
+            double dis = GetDistance(now_gps_ins, Compute00().dTpoint1);
+            if(dis<2.0)
+            {
+                vehState = dRever2;
+                qiedianCount = true;
+                cout<<"到达远切点+++++++++++++++++++++++++++++++++"<<endl;
+            }
+            else
+            {
+                controlAng = Compute00().dBocheAngle*16.5;
+                gps_decition->wheel_angle = 0 - controlAng;
+                if (qiedianCount && trumpet()<1000)
+                {
+                    //                gps_decition->brake = 10;
+                    //                gps_decition->torque = 0;
+                    dSpeed=0;
+                    minDecelerate=min(minDecelerate,-0.5f);
+                    phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
+                }
+                else
+                {
+                    qiedianCount = false;
+                    trumpetFirstCount = true;
+                    dSpeed = 2;
+                    dSecSpeed = dSpeed / 3.6;
+                    gps_decition->speed = dSpeed;
+                    obsDistance=-1;
+                    phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
+                }
+
+
+
+                cout<<"farTpointDistance================"<<dis<<endl;
+                return gps_decition;
+            }
+        }
+
+
+        if (vehState == dRever2)
+        {
+            double dis = GetDistance(now_gps_ins, Compute00().dTpoint2);
+            Point2D pt1 = Coordinate_Transfer(now_gps_ins.gps_x,now_gps_ins.gps_y, aim_gps_ins);
+            Point2D pt2 = Coordinate_Transfer(Compute00().dTpoint2.gps_x,Compute00().dTpoint2.gps_y, aim_gps_ins);
+            double xx= (pt1.x-pt2.x);
+            // if(xx>0)
+            if(xx>-0.5)
+            {
+                GaussProjCal(aim_gps_ins.gps_lng, aim_gps_ins.gps_lat, &aim_gps_ins.gps_x, &aim_gps_ins.gps_y);
+                Point2D ptt = Coordinate_Transfer(now_gps_ins.gps_x,now_gps_ins.gps_y, aim_gps_ins);
+                double xxx=ptt.x;
+                double yyy =ptt.y;
+                vehState = dRever3;
+                qiedianCount = true;
+                cout<<"到达远切点+++++++++++++++++++++++++++++++++"<<endl;
+                cout<<"xxx+++++++++++++++++++++++++++++++++"<<xxx<<endl;
+                cout<<"yyy+++++++++++++++++++++++++++++++++"<<yyy<<endl;
+            }
+            else
+            {
+                if (qiedianCount && trumpet()<1000)
+                {
+                    /*  gps_decition->brake = 10;
+                    gps_decition->torque = 0;  */
+                    dSpeed=0;
+                    minDecelerate=min(minDecelerate,-0.5f);
+                    phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
+                }
+                else
+                {
+                    qiedianCount = false;
+                    trumpetFirstCount = true;
+
+                    dSpeed = 2;
+                    dSecSpeed = dSpeed / 3.6;
+                    gps_decition->speed = dSpeed;
+                    obsDistance=-1;
+                    phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
+                }
+
+                gps_decition->wheel_angle = 0 ;
+                cout<<"farTpointDistance================"<<dis<<endl;
+                return gps_decition;
+            }
+        }
+
+
+        if (vehState == dRever3)
+        {
+            double dis = GetDistance(now_gps_ins, Compute00().dTpoint3);
+            double angdis =abs(now_gps_ins.ins_heading_angle - aim_gps_ins.ins_heading_angle);
+            if((((angdis<5)||(angdis>355)))&&(dis<10.0))
+            {
+                vehState = dRever4;
+                qiedianCount = true;
+                cout<<"到达远切点+++++++++++++++++++++++++++++++++"<<endl;
+            }
+            else {
+
+                controlAng = 0-Compute00().dBocheAngle*16.5;
+                gps_decition->wheel_angle = 0 - controlAng*0.95;
+                if (qiedianCount && trumpet()<1000)
+                {
+                    //                gps_decition->brake = 10;
+                    //                gps_decition->torque = 0;
+                    dSpeed=0;
+                    minDecelerate=min(minDecelerate,-0.5f);
+                    phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
+                }
+                else
+                {
+                    qiedianCount = false;
+                    trumpetFirstCount = true;
+                    dSpeed = 2;
+                    dSecSpeed = dSpeed / 3.6;
+                    gps_decition->speed = dSpeed;
+                    obsDistance=-1;
+                    phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
+                }
+
+
+                cout<<"farTpointDistance================"<<dis<<endl;
+                return gps_decition;
+            }
+        }
+
+        if (vehState == dRever4)
+        {
+            //	double dis = GetDistance(now_gps_ins, aim_gps_ins);
+            Point2D pt = Coordinate_Transfer(now_gps_ins.gps_x,now_gps_ins.gps_y, aim_gps_ins);
+
+            if ((pt.y)<0.5)
+            {
+                qiedianCount=true;
+                vehState=reverseArr;
+                cout<<"到达泊车点+++++++++++++++++++++++++++++++++"<<endl;
+                //            gps_decition->accelerator = -3;
+                //            gps_decition->brake =10 ;
+                dSpeed=0;
+                minDecelerate=min(minDecelerate,-0.5f);
+                phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
+
+                gps_decition->wheel_angle=0;
+                return gps_decition;
+            }
+            else
+            {
+                //if (qiedianCount && trumpet()<0)
+                if (qiedianCount && trumpet()<1000)
+                {
+                    dSpeed=0;
+                    minDecelerate=min(minDecelerate,-0.5f);
+                    phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
+                }
+                else
+                {
+                    qiedianCount = false;
+                    trumpetFirstCount = true;
+                    dSpeed = 2;
+                    dSecSpeed = dSpeed / 3.6;
+                    gps_decition->speed = dSpeed;
+                    obsDistance=-1;
+                    phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
+                }
+                controlAng = 0;
+                gps_decition->wheel_angle = 0;
+                return gps_decition;
+            }
+        }
+    }
+
+
+    //chuizhiPark
+    if(ServiceCarStatus.mnParkType==0)
+    {
+        if( vehState!=reverseCar &&  vehState!=reversing  &&  vehState!=reverseCircle &&  vehState!=reverseDirect &&  vehState!=reverseArr )
+        {
+            int bocheEN=Compute00().bocheCompute(now_gps_ins, aim_gps_ins);
+            if(bocheEN==1)
+            {
+                ServiceCarStatus.bocheEnable=1;
+            }
+            else if(bocheEN==0)
+            {
+                ServiceCarStatus.bocheEnable=0;
+            }
+        }
+        else
+        {
+            ServiceCarStatus.bocheEnable=2;
+        }
+
+        if(ServiceCarStatus.bocheMode && vehState!=reverseCar &&  vehState!=reversing  &&  vehState!=reverseCircle
+                &&  vehState!=reverseDirect&&  vehState!=reverseArr )
+        {
+            if(abs(realspeed)>0.1)
+            {
+                dSpeed=0;
+                minDecelerate=min(minDecelerate,-0.5f);
+                phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
+                gps_decition->wheel_angle=0;
+                return gps_decition;
+            }
+            else
+            {
+                if(trumpet()>3000)
+                {
+                    trumpetFirstCount=true;
+                    vehState=reverseCar;
+                }
+            }
+        }
+
+        if (vehState == reverseCar)
+        {
+            Compute00().bocheCompute(now_gps_ins, aim_gps_ins);
+            vehState = reversing;
+            qiedianCount=true;
+        }
+        if (vehState == reversing)
+        {
+            double dis = GetDistance(now_gps_ins, Compute00().nearTpoint);
+            iv::Point2D pt = iv::decition::Coordinate_Transfer(now_gps_ins.gps_x,now_gps_ins.gps_y,aim_gps_ins);
+            iv::Point2D ptnear = iv::decition::Coordinate_Transfer(Compute00().nearTpoint.gps_x,Compute00().nearTpoint.gps_y,aim_gps_ins);
+
+             double fdistonear = fabs(pt.x - ptnear.x);
+             if(fdistonear<0.5)  //将吉利项目中的停车逻辑移植过来
+    //        if (dis<2.0)//0.5
+            {
+                vehState = reverseCircle;
+                qiedianCount = true;
+                cout<<"到达近切点+++++++++++++++++++++++++++++++++"<<endl;
+            }
+            else
+            {
+                controlAng = 0;
+                dSpeed = 2;
+                dSecSpeed = dSpeed / 3.6;
+                gps_decition->wheel_angle = 0;
+                gps_decition->speed = dSpeed;
+                obsDistance=-1;
+                phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
+                //	gps_decition->accelerator = 0;
+                return gps_decition;
+            }
+        }
+
+        if (vehState == reverseCircle)
+        {
+            double dis = GetDistance(now_gps_ins, Compute00().farTpoint);
+            double angdis =abs(now_gps_ins.ins_heading_angle - aim_gps_ins.ins_heading_angle);
+            if((((angdis<5)||(angdis>355)))&&(dis<3.0))
+                //       if((((angdis<4)||(angdis>356)))&&(dis<2.0))
+            {
+                vehState = reverseDirect;
+                qiedianCount = true;
+                cout<<"到达远切点+++++++++++++++++++++++++++++++++"<<endl;
+
+            }
+            else {
+                if (qiedianCount && trumpet()<0)
+                    //         if (qiedianCount && trumpet()<1500)
+                {
+
+                    //                gps_decition->brake = 10;
+                    //                gps_decition->torque = 0;
+                    dSpeed=0;
+                    minDecelerate=min(minDecelerate,-0.5f);
+                    phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
+                }
+                else
+                {
+                    qiedianCount = false;
+                    trumpetFirstCount = true;
+
+
+                    dSpeed = 2;
+                    dSecSpeed = dSpeed / 3.6;
+                    gps_decition->speed = dSpeed;
+                    obsDistance=-1;
+                    phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
+
+                }
+
+                controlAng = Compute00().bocheAngle*16.5;
+                gps_decition->wheel_angle = 0 - controlAng*1.05;
+
+                cout<<"farTpointDistance================"<<dis<<endl;
+
+                return gps_decition;
+            }
+        }
+
+        if (vehState == reverseDirect)
+        {
+            //	double dis = GetDistance(now_gps_ins, aim_gps_ins);
+            Point2D pt = Coordinate_Transfer(now_gps_ins.gps_x,now_gps_ins.gps_y, aim_gps_ins);
+
+            if ((pt.y)<0.5)
+            {
+
+                qiedianCount=true;
+                vehState=reverseArr;
+                cout<<"到达泊车点+++++++++++++++++++++++++++++++++"<<endl;
+                //            gps_decition->accelerator = -3;
+                //            gps_decition->brake = 10;
+                //            gps_decition->torque = 0;
+                gps_decition->wheel_angle=0;
+                dSpeed=0;
+                minDecelerate=min(minDecelerate,-0.5f);
+                phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
+                return gps_decition;
+            }
+            else {
+
+                //if (qiedianCount && trumpet()<0)
+                if (qiedianCount && trumpet()<1000)
+                {
+
+                    //                gps_decition->brake = 10;
+                    //                gps_decition->torque = 0;
+                    dSpeed=0;
+                    minDecelerate=min(minDecelerate,-0.5f);
+                    phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
+                }
+                else
+                {
+                    qiedianCount = false;
+                    trumpetFirstCount = true;
+                    dSpeed = 1;
+                    dSecSpeed = dSpeed / 3.6;
+                    gps_decition->speed = dSpeed;
+                    obsDistance=-1;
+                    phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
+                }
+                controlAng = 0;
+                gps_decition->wheel_angle = 0;
+                return gps_decition;
+            }
+        }
+    }
+
+    if(ServiceCarStatus.mnParkType==2)//guosai pingtai yanzheng 直接倒车
+    {
+        ServiceCarStatus.bocheEnable=1;
+        if(ServiceCarStatus.bocheMode &&(vehState != dReverTZD))
+        {
+            vehState = dReverTZD;
+        }
+
+        if(vehState == dReverTZD)
+        {
+             Point2D pt = Coordinate_Transfer(now_gps_ins.gps_x,now_gps_ins.gps_y, aim_gps_ins);
+             if(pt.y<0.5)
+             {
+                 if(abs(realspeed)>0.1)
+                 {
+                     vehState = reverseArr;
+                 }
+                 gps_decition->wheel_angle = 0;
+                 gps_decition->speed = dSpeed;
+                 obsDistance=-1;
+                 minDecelerate=min(minDecelerate,-0.5f);
+                 phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
+                 //	gps_decition->accelerator = 0;
+                 return gps_decition;
+             }
+             else
+             {
+                 gps_decition->wheel_angle = 0;
+                 gps_decition->speed = dSpeed;
+                 obsDistance=-1;
+                 dSpeed = 2;
+                 dSecSpeed = dSpeed / 3.6;
+                 gps_decition->speed = dSpeed;
+                 phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
+                 //	gps_decition->accelerator = 0;
+                 return gps_decition;
+             }
+        }
+    }
+
+    if ((vehState == reverseArr)&&((ServiceCarStatus.mnParkType==1)||(ServiceCarStatus.mnParkType==0)
+                                   ||(ServiceCarStatus.mnParkType==2))) //不论哪种泊车最后都会到这个状态
+    {
+        ServiceCarStatus.bocheMode=false;
+        if (qiedianCount && trumpet()<1500)
+        {
+            dSpeed=0;
+            minDecelerate=min(minDecelerate,-0.5f);
+            phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
+            ServiceCarStatus.bocheMode=false;
+        }
+        else
+        {
+            qiedianCount = false;
+            trumpetFirstCount = true;
+            ServiceCarStatus.bocheEnable=0;
+            vehState=normalRun;
+            ServiceCarStatus.mbRunPause=true;
+            ServiceCarStatus.mnBocheComplete=100;
+            cout<<"泊车daowei mbRunPause+++++++++++++++++++++++++++++++++"<<endl;
+        }
+        gps_decition->wheel_angle = 0 ;
+        return gps_decition;
+    }
+
+/****************************self park logic end************************************************/
+
+    obsDistance = -1;
+    esrIndex = -1;
+    lidarDistance = -1;
+
+    obsDistanceAvoid = -1;
+    esrIndexAvoid = -1;
+    roadPre = -1;
+    avoidXNewPre=0;
+    //dSpeed = ServiceCarStatus.mroadmode_vel.mfmode0;
+    int nmapsize = gpsMapLine.size();
+
+    gpsTraceOri.clear();
+    gpsTraceNow.clear();
+    gpsTraceAim.clear();
+    gpsTraceAvoid.clear();
+    gpsTraceBack.clear();
+    gpsTraceNowLeft.clear();
+    gpsTraceNowRight.clear();
+
+    dSpeed =80;
+
+    if (!isFirstRun)
+    {
+        PathPoint = Compute00().getNearestPointIndex(now_gps_ins, gpsMapLine, DecideGps00::lastGpsIndex, DecideGps00::minDis, DecideGps00::maxAngle);
+        if(PathPoint<0)
+        {
+            PathPoint = Compute00().getFirstNearestPointIndex(now_gps_ins, gpsMapLine, DecideGps00::lastGpsIndex, DecideGps00::minDis, DecideGps00::maxAngle);
+        }
+    }
+
+
+    if (PathPoint<0)
+    {
+        minDecelerate=-1.0;
+        phaseSpeedDecition(gps_decition, secSpeed, -1, 0,now_gps_ins);
+
+        givlog->debug("decition_brain","PathPoint is smaller than 0, it is : %d",PathPoint);
+        return gps_decition;
+    }
+
+    DecideGps00::lastGpsIndex = PathPoint;
+
+    if(!ServiceCarStatus.inRoadAvoid)
+    {
+        roadOri = gpsMapLine[PathPoint]->roadOri;
+        roadSum = gpsMapLine[PathPoint]->roadSum;
+
+    }
+    else
+    {
+        roadOri=gpsMapLine[PathPoint]->roadOri*3+1;
+        roadSum = gpsMapLine[PathPoint]->roadSum*3;
+    }
+
+    if(roadNowStart)
+    {
+        roadNow=roadOri;
+        roadNowStart=false;
+    }
+
+
+    if(ServiceCarStatus.avoidObs)
+    {
+        gpsMapLine[PathPoint]->runMode=1;
+    }
+    else
+    {
+        gpsMapLine[PathPoint]->runMode=0;
+    }
+
+#ifdef AVOID_NEW
+    if(obstacle_avoid_flag==1)
+    {
+
+    }
+    else
+    {
+        avoidXNew=0;
+        roadNow = roadOri;
+        if(vehState==backOri)
+        {
+            vehState=normalRun;
+        }
+    }
+    givlog->debug("decition_brain","avoidXNew: %f",avoidXNew);
+#else
+    if(obstacle_avoid_flag==1)
+    {
+        avoidX=computeAvoidX(roadNow,roadOri,gpsMapLine[PathPoint]);
+    }
+    else
+    {
+        avoidX=0;
+        roadNow = roadOri;
+        if(vehState==backOri)
+        {
+            vehState=normalRun;
+        }
+    }
+#endif
+
+    if ( gpsMapLine[PathPoint]->runMode == 0 && (vehState == avoidObs || vehState == stopObs || vehState == preAvoid
+                                                 ||  vehState == avoiding || vehState == backOri || vehState ==preBack || vehState ==waitAvoid ) )
+    {
+        vehState = normalRun;
+        roadNow = roadOri;
+    }
+
+    gpsTraceOri= getGpsTrace(now_gps_ins, gpsMapLine, lastGpsIndex,circleMode);
+    gpsTraceMapOri=gpsTraceOri;
+
+    if((vehState == avoiding)&&(obstacle_avoid_flag == 1))
+    {
+        iv::Point2D now_s_d=cartesian_to_frenet1D(gpsMapLine,now_gps_ins.gps_x, now_gps_ins.gps_y);
+        if((now_s_d.s+20>(gpsTraceAvoidXY[gpsTraceAvoidXY.size()-1].s))&&((gpsTraceAvoidXY[gpsTraceAvoidXY.size()-1].s)<((*gpsMapLine[gpsMapLine.size()-1]).frenet_s)))
+        {
+            double sPathFinal=gpsTraceAvoidXY[gpsTraceAvoidXY.size()-1].s;
+            if(sPathFinal+20<=(*gpsMapLine[gpsMapLine.size()-1]).frenet_s)
+            {
+                sPathFinal=sPathFinal+20;
+            }
+            else
+            {
+                sPathFinal=(*gpsMapLine[gpsMapLine.size()-1]).frenet_s;
+            }
+            for(double s=gpsTraceAvoidXY[gpsTraceAvoidXY.size()-1].s;s<=sPathFinal;s+=0.1)
+            {
+                iv::Point2D gpsTracePoint=frenet_to_cartesian1D(gpsMapLine,s,-avoidXNew,PathPoint);
+                gpsTraceAvoidXY.push_back(gpsTracePoint);
+            }
+        }
+    }
+
+    if((vehState == avoiding || vehState == backOri) && (gpsTraceAvoidXY.size()>0) && (obstacle_avoid_flag==1))
+    {
+        gpsTraceOri=getGpsTraceAvoid(now_gps_ins, gpsTraceAvoidXY, lastGpsIndex,circleMode);
+    }
+
+    if(gpsTraceOri.empty())
+    {
+        gps_decition->wheel_angle = 0.2;
+        gps_decition->speed = dSpeed;
+
+        gps_decition->accelerator = -0.5;
+        gps_decition->torque = 0.0;
+        //gps_decition->brake=10; //
+        if(ServiceCarStatus.msysparam.mvehtype=="shenlan")  //长安深蓝决策发的brake是负值的时候才刹车，利用的是扭矩
+        {
+          gps_decition->brake=-20;
+        }
+        else
+        {
+           gps_decition->brake=10;
+        }
+        givlog->warn("decition_brain","gpsTraceOri is empty");
+        return gps_decition;
+    }
+
+
+
+//    traceDevi=gpsTraceOri[0].x;
+
+    //起步进行frenet规划后，车辆行驶至地图路线上，起步操作完毕。
+    //如果车辆本身启动位置在地图路线上，可不用进行frenet规划。
+    //    if(GetDistance(now_gps_ins,*gpsMapLine[PathPoint]) < 1){
+    //        startingFlag = false;
+    //    }
+//    startingFlag = false;
+//    if(startingFlag)
+//    {
+//        //        gpsTraceAim = gpsTraceOri;
+//        if(abs(gpsTraceOri[0].x)>1)
+//        {
+//            cout<<"frenetPlanner->getPath:pre"<<endl;
+//            gpsTraceNow = frenetPlanner->getPath(now_gps_ins,gpsMapLine,PathPoint,0,lidarGridPtr);
+//            cout<<"frenetPlanner->getPath,gpsTraceNow.size: "<<gpsTraceNow.size()<<endl;
+//            if(gpsTraceNow.size()==0)
+//            {
+//                gps_decition->accelerator = 0;
+//                gps_decition->brake=10;
+//                gps_decition->wheel_angle = lastAngle;
+//                gps_decition->speed = 0;
+//                return gps_decition;
+//            }
+//        }
+//        else
+//        {
+//            startingFlag = false;
+//        }
+//    }
+
+
+//    if(useFrenet){
+//        //如果车辆在变道中，启用frenet规划局部路径
+//        if(vehState == avoiding || vehState == backOri)
+//        {
+//            // avoidX = (roadOri - roadNow)*roadWidth;
+//            avoidX=computeAvoidX(roadNow,roadOri,gpsMapLine[PathPoint]);
+//            gpsTraceNow = frenetPlanner->getPath(now_gps_ins,gpsMapLine,PathPoint,avoidX,lidarGridPtr);
+//            //gpsTraceAim = laneChangePlanner->getPath(now_gps_ins,gpsMapLine,PathPoint,avoidX,lidarGridPtr);
+//            gpsTraceAim = getGpsTraceOffset(gpsTraceOri, avoidX);
+//            if(gpsTraceNow.size()==0)
+//            {
+//                gps_decition->accelerator = 0;
+//                gps_decition->brake=10;
+//                gps_decition->wheel_angle = lastAngle;
+//                gps_decition->speed = 0;
+//                return gps_decition;
+//            }
+//        }
+//    }
+
+#ifdef AVOID_NEW
+    if(gpsTraceNow.size()==0)
+    {
+        if (avoidXNew==0.0)
+        {
+            if((vehState == backOri)||(vehState == avoiding))
+            {
+                gpsTraceNow.assign(gpsTraceOri.begin(), gpsTraceOri.end());
+                getGpsTraceNowLeftRight(gpsTraceNow);
+            }
+            else
+            {
+                gpsTraceNow.assign(gpsTraceOri.begin(), gpsTraceOri.end());
+                gpsTraceNowLeft.assign(traceOriLeft.begin(),traceOriLeft.end());
+                gpsTraceNowRight.assign(traceOriRight.begin(),traceOriRight.end());
+            }
+        }
+        else
+        {
+            if((vehState == avoiding)||(vehState == backOri))
+            {
+                gpsTraceNow.assign(gpsTraceOri.begin(), gpsTraceOri.end());
+                getGpsTraceNowLeftRight(gpsTraceNow);
+            }
+            else
+            {
+                gpsTraceNow = getGpsTraceOffset(gpsTraceOri, avoidXNew);//luojixuyao xiugai
+                getGpsTraceNowLeftRight(gpsTraceNow);
+            }
+        }
+    }
+#else
+    if(gpsTraceNow.size()==0){
+        if (roadNow==roadOri)
+        {
+            if(vehState == backOri)
+            {
+                gpsTraceNow.assign(gpsTraceOri.begin(), gpsTraceOri.end());
+                getGpsTraceNowLeftRight(gpsTraceNow);
+            }
+            else{
+                gpsTraceNow.assign(gpsTraceOri.begin(), gpsTraceOri.end());
+                gpsTraceNowLeft.assign(traceOriLeft.begin(),traceOriLeft.end());
+                gpsTraceNowRight.assign(traceOriRight.begin(),traceOriRight.end());
+            }
+        }else
+        {
+            if((vehState == avoiding)||(vehState == backOri))
+            {
+                gpsTraceNow.assign(gpsTraceOri.begin(), gpsTraceOri.end());
+                //                gpsTraceNowLeft=getGpsTraceOffset(traceOriLeft, avoidX);
+                //                gpsTraceNowRight=getGpsTraceOffset(traceOriRight, avoidX);
+                getGpsTraceNowLeftRight(gpsTraceNow);
+            }else{
+                gpsTraceNow = getGpsTraceOffset(gpsTraceOri, avoidX);
+                //                gpsTraceNowLeft=getGpsTraceOffset(traceOriLeft, avoidX);
+                //                gpsTraceNowRight=getGpsTraceOffset(traceOriRight, avoidX);
+                getGpsTraceNowLeftRight(gpsTraceNow);
+            }
+        }
+    }
+
+#endif
+
+
+
+    if(vehState==normalRun)
+    {
+        if(gpsTraceNow.size()>200)
+        {
+            if(gpsTraceNow[200].x<-3)
+            {
+                gps_decition->leftlamp = true;
+                gps_decition->rightlamp = false;
+            }
+            else if(gpsTraceNow[200].x>3)
+            {
+                gps_decition->leftlamp = false;
+                gps_decition->rightlamp = true;
+            }
+            else
+            {
+                gps_decition->leftlamp = false;
+                gps_decition->rightlamp = false;
+            }
+        }
+    }
+
+    //  dSpeed = getSpeed(gpsTraceNow);
+
+
+    planTrace.clear();//Add By YuChuli, 2020.11.26
+    for(int i=0;i<gpsTraceNow.size();i++){
+        TracePoint pt(gpsTraceNow[i].x,gpsTraceNow[i].y);
+        planTrace.push_back(pt);
+    }
+
+    controlAng= getAngle(gpsTraceNow,now_gps_ins,gps_decition);
+
+    /****************************terminal point stop logic begin************************************************/
+    //转角
+    if((ServiceCarStatus.table_look_up_on==true)&&(hapo_Adapter->IsINterpolationOK()))
+    {
+        if(!circleMode && PathPoint>(gpsMapLine.size()-20))
+        {
+            controlAng=0;
+        }
+    }
+    else
+    {
+        if(!circleMode && PathPoint>(gpsMapLine.size()-250))
+        {
+            if(realspeed<0.5)
+                controlAng=0;
+        }
+    }
+    //加速度
+    double acc_end = 0.1;
+    static double distoend=1000;
+
+    int useaccend = 1;
+    double mstopbrake=0.6;
+
+    if(((ServiceCarStatus.table_look_up_on==true)&&(hapo_Adapter->IsINterpolationOK()))||(useaccend == 1))
+    {
+                if(PathPoint+1000>gpsMapLine.size()){
+                    distoend=computeDistToEnd(gpsMapLine,PathPoint);
+                }else{
+                    distoend=1000;
+                }
+                if(!circleMode && distoend<100){
+                        if(distoend<3.0)
+                        {
+                            std::cout<<"distoend:  "<<distoend<<std::endl;
+                        }
+                        double nowspeed = realspeed/3.6;
+                        double brakedis = 100;
+                        double stopbrake = mstopbrake;
+                        if(nowspeed>10)
+                        {
+                            brakedis = (nowspeed*nowspeed)/(2*2.0);
+                        }
+                        else
+                        {
+                            brakedis = (nowspeed*nowspeed)/(2*stopbrake)+3;
+                        }
+                        if((distoend<3)||(distoend<brakedis))
+                        {
+                            if(distoend == 0.0)distoend = 0.09;
+                            acc_end = (-1.0)*nowspeed*nowspeed/(2.0*distoend);
+                            if((acc_end<(-3.0))&&(nowspeed<10))acc_end = -3.0;
+                        }
+
+                        if((distoend <= 0.1)||(PathPoint>=gpsMapLine.size()-2))acc_end = (stopbrake + 0.1)*(-1.0);
+
+
+                        if(acc_end<0)minDecelerate = acc_end;
+
+                        givlog->debug("decition_brain","acc_end: %f",acc_end);
+
+                }
+    }
+    else
+    {
+        //                if(!circleMode && PathPoint>gpsMapLine.size()-brake_distance){
+        //                        minDecelerate=-0.5;
+        //                        std::cout<<"map finish brake"<<std::endl;
+        //                }
+        if(!circleMode){
+            double forecast_final=secSpeed*secSpeed+5;
+            int forecast_final_point=((int)forecast_final)*10+1500;
+            static int BrakePoint=-1;
+            static bool final_brake=false;
+            static double distance_to_end=1000;
+            static bool enterTofinal=false;  //20230417,shenlan
+            static double enterTofinal_speed=200;
+            if(PathPoint+forecast_final_point>gpsMapLine.size())
+            {
+                distance_to_end=computeDistToEnd(gpsMapLine,PathPoint);
+                if(ServiceCarStatus.txt_log_on==true)
+                {
+                    givlog->debug("decition_brain","distoend: %f",distance_to_end);
+                }
+                if((forecast_final>=distance_to_end)&&(realspeed>3))
+                {
+                    final_brake=true;
+                    if(BrakePoint==-1)
+                        BrakePoint=PathPoint-10;
+                }
+            }
+            else
+            {
+                distance_to_end=1000;
+            }
+            if(PathPoint<BrakePoint)
+            {
+                final_brake=false;
+                final_brake_lock=false;
+                brake_mode=false;
+                BrakePoint=-1;
+                enterTofinal=false;  //20230417,shenlan
+                enterTofinal_speed=200;
+            }
+            if(final_brake==true)
+            {
+                if((realspeed>3)&&(final_brake_lock==false))
+                {
+                    minDecelerate=-0.7;
+                }
+                else
+                {
+                    if(enterTofinal==false)  //20230417,shenlan)
+                    {
+                        enterTofinal_speed=realspeed;
+                        enterTofinal=true;
+                    }
+                    if(ServiceCarStatus.msysparam.mvehtype=="shenlan")
+                    {
+                       dSpeed=min(enterTofinal_speed, 3.0);
+                    }
+                    else
+                    {
+                       dSpeed=min(dSpeed, 3.0);
+                    }
+                    final_brake_lock=true;
+                    brake_mode=true;
+                    if(ServiceCarStatus.msysparam.mvehtype=="shenlan")
+                    {
+                        if(distance_to_end<2.0)
+                        {
+                            minDecelerate=-0.7;
+                        }
+
+                    }
+                    else
+                    {
+                        if(distance_to_end<0.8)
+                        {
+                            minDecelerate=-0.7;
+                        }
+                    }
+                }
+            }
+        }
+    }
+    if(brake_mode==true){
+        dSpeed=min(dSpeed, 3.0);
+    }
+    /****************************terminal point stop logic end************************************************/
+    //group map end park pro--------start
+    if(front_car_id>0){
+        static  bool final_lock=false;
+        if((distoend<90)&&(front_car.front_car_dis<obsDistance+10)){
+            if((obsDistance>3)&&(obsDistance<20)){
+                if((realspeed>3)&&(final_lock==false)){
+                    minDecelerate=-0.7;
+                }else{
+                    dSpeed = min(3.0,dSpeed);
+                    final_lock=true;
+                }
+                obsDistance=200;
+            }else if((obsDistance>1)&&(obsDistance<3)){
+                minDecelerate=-0.5;
+                obsDistance=200;
+            }else if(obsDistance<1){
+                minDecelerate=-1.0;
+            }
+            if(realspeed<1){
+                final_lock=false;
+            }
+        }
+    }
+    //group map end park pro-----------end
+
+    if(front_car_id>0)
+    {
+        static bool brake_state=false;
+        if(((front_car.vehState!=0)||(front_car.avoidX!=0))&&(front_car.front_car_dis<15))
+        {
+            brake_state=true;
+        }
+        if(brake_state==true)
+        {
+            dSpeed=min(dSpeed,ServiceCarStatus.mroadmode_vel.mfmode18);
+        }
+        if(((front_car.vehState==0)&&(front_car.avoidX==0))||(front_car.front_car_dis>25))
+        {
+            brake_state=false;
+        }
+
+    }
+
+
+
+    //2020-0106
+//    if(ServiceCarStatus.msysparam.mvehtype !="zhongche")
+//    {
+
+//        if(vehState==avoiding){
+//            ServiceCarStatus.msysparam.vehWidth=2.4;
+//            controlAng=max(-150.0,controlAng);//35  zj-150
+//            controlAng=min(150.0,controlAng);//35   zj-150
+//        }
+//        if(vehState==backOri){
+//            controlAng=max(-150.0,controlAng);//35   zj-150
+//            controlAng=min(150.0,controlAng);//35     zj-150
+//        }
+//    }
+
+//    givlog->debug("brain_decition","vehState: %d,controlAng: %f",vehState,controlAng);
+
+    if(ServiceCarStatus.speed_control==true)
+    {
+        dSpeed = min(doubledata[PathPoint][4],dSpeed);
+    }
+    else
+    {
+        if (gpsMapLine[PathPoint]->roadMode ==0)
+        {
+            dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode0,dSpeed);
+            gps_decition->leftlamp = false;
+            gps_decition->rightlamp = false;
+        }
+        else if (gpsMapLine[PathPoint]->roadMode == 5)
+        {
+            dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode5,dSpeed);
+            gps_decition->leftlamp = false;
+            gps_decition->rightlamp = false;
+        }
+        else if (gpsMapLine[PathPoint]->roadMode == 11)
+        {
+            dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode11,dSpeed);
+        }
+        else if (gpsMapLine[PathPoint]->roadMode == 14)
+        {
+            dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode14,dSpeed);
+            gps_decition->leftlamp = true;
+            gps_decition->rightlamp = false;
+        }
+        else if (gpsMapLine[PathPoint]->roadMode == 15)
+        {
+            dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode15,dSpeed);
+            gps_decition->leftlamp = false;
+            gps_decition->rightlamp = true;
+        }
+        else if (gpsMapLine[PathPoint]->roadMode == 16)
+        {
+            dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode16,dSpeed);
+            gps_decition->leftlamp = true;
+            gps_decition->rightlamp = false;
+        }
+        else if (gpsMapLine[PathPoint]->roadMode == 17)
+        {
+            dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode17,dSpeed);
+            gps_decition->leftlamp = false;
+            gps_decition->rightlamp = true;
+        }
+        else if (gpsMapLine[PathPoint]->roadMode == 18)
+        {
+            dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode18,dSpeed);
+        }
+        else if (gpsMapLine[PathPoint]->roadMode == 1)
+        {
+            dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode1,dSpeed);
+        }
+        else if (gpsMapLine[PathPoint]->roadMode == 2)
+        {
+            dSpeed = min(15.0,dSpeed);
+        }
+        else if (gpsMapLine[PathPoint]->roadMode == 7)
+        {
+            dSpeed = min(15.0,dSpeed);
+            xiuzhengCs=1.5;
+        }
+        else if (gpsMapLine[PathPoint]->roadMode == 4) //1220
+        {
+            dSpeed = min(4.0,dSpeed);
+        }
+        else{
+            dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode0,dSpeed);
+            gps_decition->leftlamp = false;
+            gps_decition->rightlamp = false;
+        }
+    }
+
+    if (gpsMapLine[PathPoint]->speed_mode == 2)
+    {
+        dSpeed = min(25.0,dSpeed);
+
+    }
+
+    if((gpsMapLine[PathPoint]->speed)>0.001)
+    {
+        dSpeed = min((gpsMapLine[PathPoint]->speed*3.6),dSpeed);
+        //        if((gpsMapLine[PathPoint]->speed)>4.5)
+        //        {
+        //            dSpeed =gpsMapLine[PathPoint]->speed*3.6;
+        //        }
+    }
+
+    //givlog->debug("decition_brain","brake_mode: %d",brake_mode);
+
+#ifdef AVOID_NEW  //20220223toggle
+    //    if((vehState==avoiding)||(vehState==preAvoid))
+    //    {
+    //        dSpeed = min(8.0,dSpeed);
+    //    }else if((vehState==backOri)||(avoidXNew!=0)){
+    //        dSpeed = min(12.0,dSpeed);
+    //    }
+#else
+    if((vehState==avoiding)||(vehState==backOri)||(roadNow != roadOri)||(vehState==preAvoid))
+    {
+        dSpeed = min(8.0,dSpeed);
+    }
+#endif
+
+
+
+
+    dSpeed = limitSpeed(controlAng, dSpeed);
+    dSecSpeed = dSpeed / 3.6;
+    //    givlog->debug("brain_decition_speed","dspeed: %f",
+    //                  dSpeed);
+
+    if(vehState==changingRoad)
+    {
+        if(gpsTraceNow[0].x>1.0)
+        {
+            gps_decition->leftlamp = false;
+            gps_decition->rightlamp = true;
+        }
+        else if(gpsTraceNow[0].x<-1.0)
+        {
+            gps_decition->leftlamp = true;
+            gps_decition->rightlamp = false;
+        }
+        else
+        {
+            vehState=normalRun;
+        }
+    }
+
+    static int obs_filter=0,obs_filter_flag=0,obs_filter_dis_memory=0;
+    static double obs_speed_for_avoid=-1,obs_filter_speed_memory=0;   //obs_speed_for_avoid: obstacle actual speed in km/h
+    if(!ServiceCarStatus.daocheMode)
+    {
+        //computeObsOnRoad(lidarGridPtr, gpsTraceNow, roadNow,gpsMapLine,lidar_per);
+        computeObsOnRoadXY(now_gps_ins,lidarGridPtr, gpsTraceNow,gpsTraceNowLeft,gpsTraceNowRight,roadNow,gpsMapLine,lidar_per);
+        if(obs_filter_flag==0)
+        {
+            if(obsDistance>0&&obsDistance<60)
+            {
+                obs_filter++;
+                if(obs_filter<20)                          //80
+                {
+                    obsDistance=-1;
+                    obsSpeed=0;
+                }
+                else
+                {
+                    obs_filter_flag=1;
+                    obs_filter_dis_memory=obsDistance;
+                    obs_filter_speed_memory=obsSpeed;
+                    obs_filter=0;
+                }
+            }
+            else
+            {
+                obs_filter=0;
+            }
+        }
+        else
+        {
+            if(obsDistance<0||obsDistance>60)
+            {
+                obs_filter++;
+                if(obs_filter<20)                           //80
+                {
+                    obsDistance=obs_filter_dis_memory;
+                    obsSpeed=obs_filter_speed_memory;
+                }
+                else
+                {
+                    obs_filter_flag=0;
+                    obs_filter=0;
+                }
+            }
+            else
+            {
+                obs_filter=0;
+                obs_filter_dis_memory=obsDistance;
+                obs_filter_speed_memory=obsSpeed;
+            }
+        }
+
+
+        obs_speed_for_avoid=(secSpeed+obsSpeed)*3.6;
+
+        givlog->debug("decition_brain","obs_filter: %d,obs_filter_flag: %d,obs_filter_dis_memory: %f,obsDistance: %f,obsSpeed: %f,obs_speed_for_avoid: %f",
+                      obs_filter,obs_filter_flag,obs_filter_dis_memory,obsDistance,obsSpeed,obs_speed_for_avoid);
+
+        obs_speed_for_avoid=0;//shenlan guosai zhangaiwu sudu zhijiegei 0
+    }
+    else
+    {
+        gpsTraceRear.clear();
+        for(unsigned int i=0;i<gpsTraceNow.size();i++)
+        {
+            Point2D pt;
+            pt.x=0-gpsTraceNow[i].x;
+            pt.y=0-gpsTraceNow[i].y;
+            gpsTraceRear.push_back(pt);
+        }
+        computeObsOnRoad(lidarGridPtr, gpsTraceNow, roadNow,gpsMapLine,lidar_per);
+        //  obsDistance=-1; //1227
+    }
+
+    ServiceCarStatus.mObsDistance=obsDistance;
+    ServiceCarStatus.mObsSpeed=obs_speed_for_avoid;
+
+    static bool avoid_speed_flag=false;
+    /*if(obsDistance>0&& (obsDistance<ServiceCarStatus.aocfDis+10)&&(abs(realspeed)>0.5)&&
+            (vehState==normalRun) &&(ObsTimeStart==-1)//&&(obs_speed_for_avoid<0.6)
+            && (gpsMapLine[PathPoint]->runMode==1)&&(gpsMapLine[PathPoint]->mode2!=3000))
+    {
+        minDecelerate=-0.4;
+        avoid_speed_flag=true;
+    }*/
+
+    road_permit_avoid=false;
+
+    if(ServiceCarStatus.msysparam.mvehtype=="shenlan") //比赛总路线可能不会太长
+    {
+        if(PathPoint+200<gpsMapLine.size()){
+            int road_permit_sum=0;
+            for(int k=PathPoint;k<PathPoint+200;k++){
+                if((gpsMapLine[k]->mbnoavoid==false))//&&(gpsMapLine[k]->roadSum>1))
+                    road_permit_sum++;
+            }
+            if(road_permit_sum>=200)
+                road_permit_avoid=true;
+        }
+    }
+    else
+    {
+        if(PathPoint+200<gpsMapLine.size()){   //400gaiwei 200
+            int road_permit_sum=0;
+            for(int k=PathPoint;k<PathPoint+200;k++){
+                if((gpsMapLine[k]->mbnoavoid==false))//&&(gpsMapLine[k]->roadSum>1))
+                    road_permit_sum++;
+            }
+            if(road_permit_sum>=200)
+                road_permit_avoid=true;
+        }
+    }
+
+    //shiyanbizhang 0929
+    if(obsDistance>0&& (obsDistance<ServiceCarStatus.aocfDis)&&(obs_speed_for_avoid>-8.0)&&(obs_speed_for_avoid<3.0)&&(realspeed>0.01)//&& (avoid_speed_flag==true)        //
+            &&(vehState==normalRun||vehState==backOri || vehState==avoiding) //&&(ObsTimeStart==-1)
+            && (gpsMapLine[PathPoint]->runMode==1)&& (road_permit_avoid==true)&&(gpsMapLine[PathPoint]->mode2!=3000)&&(gpsMapLine[PathPoint+300]->mode2!=3000)&&(gpsMapLine[PathPoint+100]->mode2!=3000)&&(gpsMapLine[PathPoint+300]->mode2!=23)){
+        //        ObsTimeStart=GetTickCount();
+        ObsTimeEnd+=1.0;
+        //dSpeed = min(6.0,dSpeed);
+        cout<<"\n====================preAvoid time count start==================\n"<<endl;
+    }
+    //    if(ObsTimeStart!=-1){
+    //        ObsTimeEnd=GetTickCount();
+    //    }
+    //    if(ObsTimeEnd!=-1){
+    //        ObsTimeSpan=ObsTimeEnd-ObsTimeStart;
+    //    }
+
+
+
+    //    if(ObsTimeSpan>ObsTimeWidth &&(obsDistance>1.5||obsDistance<0) && (gpsMapLine[PathPoint]->runMode==1)){ //1026
+    //        vehState=avoidObs;
+    //        ObsTimeStart=-1;
+    //        ObsTimeEnd=-1;
+    //        ObsTimeSpan=-1;
+    //        avoid_speed_flag=false;
+    //        cout<<"\n====================preAvoid time count finish==================\n"<<endl;
+    //    }
+
+
+
+
+    //    if((ObsTimeStart!=-1)&&(obs_speed_for_avoid>-8.0)&&(obs_speed_for_avoid<3.0)){
+    //            ObsTimeEnd+=1.0;
+    //    }
+
+    if(ObsTimeEnd>ObsTimeWidth &&(obsDistance>1.5||obsDistance<0) && (gpsMapLine[PathPoint]->runMode==1)){ //1026
+        vehState=avoidObs;
+        ObsTimeStart=-1;
+        ObsTimeEnd=-1;
+        ObsTimeSpan=-1;
+        avoid_speed_flag=false;
+        cout<<"\n====================preAvoid time count finish==================\n"<<endl;
+    }
+
+    if((obsDistance<0 ||obsDistance>ServiceCarStatus.aocfDis)||(road_permit_avoid==false)||(front_car_decide_avoid==false)){
+        ObsTimeStart=-1;
+        ObsTimeEnd=-1;
+        ObsTimeSpan=-1;
+        avoid_speed_flag=false;
+    }
+
+    //避障模式
+    if (vehState == avoidObs   || vehState==waitAvoid ) {
+
+        //      if (obsDistance <20 && obsDistance >0)
+        if (obsDistance <ServiceCarStatus.aocfDis && obsDistance >0)
+        {
+            dSpeed = min(6.0,dSpeed);
+            avoidTimes++;
+            //          if (avoidTimes>=6)
+            if (avoidTimes>=ServiceCarStatus.aocfTimes)
+            {
+                vehState = preAvoid;
+                avoidTimes = 0;
+            }
+
+        }
+        //        else if (obsDistance < 35 && obsDistance >0 && dSpeed>10)
+        //        {
+        //            dSpeed = 10;
+        //            avoidTimes = 0;
+        //        }
+        else if (obsDistance < 40 && obsDistance >0 && dSpeed>15)
+        {
+            dSpeed =  min(15.0,dSpeed);
+            avoidTimes = 0;
+        }
+        else
+        {
+            avoidTimes = 0;
+        }
+
+    }
+
+
+    if((vehState == preAvoid)||(avoidXNew!=0.0))//jiashang avoiding fangzhi avoid=0.0buneng lianbi
+    {
+        dSpeed = min(6.0,dSpeed);
+//        int avoidLeft_value=0;
+//        int avoidRight_value=0;
+//        int* avoidLeft_p=&avoidLeft_value;
+//        int* avoidRight_p=&avoidRight_value;
+        double avoidLeft_value=0;
+        double avoidRight_value=0;
+        double* avoidLeft_p=&avoidLeft_value;
+        double* avoidRight_p=&avoidRight_value;
+        computeAvoidBoundary(roadOri,roadSum,gpsMapLine[PathPoint]->mfLaneWidth,ServiceCarStatus.msysparam.vehWidth,avoidLeft_p,avoidRight_p);
+        avoidXNewPre=computeAvoidXDistance(lidarGridPtr,now_gps_ins, gpsMapLine,avoidLeft_value,avoidRight_value,avoidXNewLast);
+
+        givlog->debug("decition_brain","======avoidXNewPre==========:%f",avoidXNewPre);
+
+        //const int nAvoidPreCount = 100;
+        if(avoidXNewPreVector.size()<5){
+                   avoidXNewPreVector.push_back(avoidXNewPre);
+               }else{
+                   if(avoidXNewPreVector[0]!=avoidXNewLast){
+                       for(unsigned int i=1;i<avoidXNewPreVector.size();i++){
+                           if(avoidXNewPreVector[i]!=avoidXNewPreVector[0]){
+                               avoidXNewPreFilter=avoidXNewLast;
+                               break;
+                           }
+                           if(i==avoidXNewPreVector.size()-1)
+                               avoidXNewPreFilter=avoidXNewPreVector[0];
+                       }
+                   }
+                   avoidXNewPreVector.clear();
+               }
+               if(avoidXNewPreFilter!=avoidXNewLast){
+                   avoidXNew=avoidXNewPre;
+                   if(avoidXNew<0)
+                       turnLampFlag=-1;
+                   else if(avoidXNew>0)
+                       turnLampFlag=1;
+                   else
+                       turnLampFlag=0;
+
+                   gpsTraceNow.clear();
+                   gpsTraceAvoidXY.clear();
+                   givlog->debug("decition_brain","avoidXNew: %f,avoidXNewLast: %f",
+                                 avoidXNew,avoidXNewLast);
+                   //gpsTraceAvoidXY = getGpsTraceOffsetAvoid(gpsTraceOri, avoidXNew,now_gps_ins);
+                   gpsTraceAvoidXY = splinePlanner->chooseRoadBySpline(now_gps_ins,gpsMapLine,PathPoint,obsSpline,avoidXNew,avoidXNewLast);
+                   vehState = avoiding;
+                   startAvoidGpsIns = now_gps_ins;
+                   obstacle_avoid_flag=1;
+                   hasCheckedAvoidLidar = false;
+                   avoidXNewLast=avoidXNew;
+                   avoidXNewPreFilter=avoidXNew;
+               }
+           }
+
+//        if(avoidXNewPreVector.size()<nAvoidPreCount){
+//            avoidXNewPreVector.push_back(avoidXNewPre);
+//        }
+//        else
+//        {
+//            avoidXNewPreVector.erase(avoidXNewPreVector.begin());
+//            avoidXNewPreVector.push_back(avoidXNewPre);
+//        }
+
+//        if(vehState == preAvoid)
+//        {
+//            avoidXNew = avoidXNewPre;
+//        }
+//        else
+//        {
+//            if(fabs(avoidXNewLast)>0.1)
+//            {
+
+//                bool bOriginSafe = true;
+
+//                if(avoidXNewPreVector.size()<nAvoidPreCount)
+//                {
+//                    bOriginSafe = false;
+//                }
+//                else
+//                {
+//                    unsigned int j;
+//                    unsigned int nSize = avoidXNewPreVector.size();
+//                    for(j=0;j<nSize;j++)
+//                    {
+//                        if(fabs(avoidXNewPreVector[j])>0.1)
+//                        {
+//                            bOriginSafe = false;
+//                            break;
+//                        }
+//                    }
+//                }
+//                if(bOriginSafe)
+//                {
+//                    avoidXNew = 0;
+// //                   avoidXNewPreVector.clear();
+//                }
+//                else
+//                {
+//                    unsigned int j;
+//                    unsigned int nSize = avoidXNewPreVector.size();
+//                    if(avoidXNewPreVector.size()==nAvoidPreCount)
+//                    {
+//                        double filter = 0;
+//                        for(j=0;j<nSize;j++)
+//                        {
+//                            if(fabs(avoidXNewPreVector[j])>0.1)
+//                            {
+//                                if(fabs(filter)<0.1)filter = avoidXNewPreVector[j];
+//                                else
+//                                {
+//                                    if(fabs(filter - avoidXNewPreVector[j])>0.1)
+//                                    {
+//                                        filter = 0;
+//                                        break;
+//                                    }
+//                                }
+//                            }
+//                        }
+//                        if(fabs(filter)<0.1)
+//                        {
+//                            avoidXNew = avoidXNewLast;
+//                        }
+//                        else
+//                        {
+//                            if(fabs(filter - avoidXNewLast)>=2.0)
+//                            {
+//                                avoidXNew = filter;
+//                            }
+//                            else
+//                            {
+//                                avoidXNew = avoidXNewLast;
+//                            }
+//                        }
+//                    }
+//                    else
+//                    {
+//                        avoidXNew = avoidXNewLast;
+//                    }
+//                }
+//            }
+//        }
+
+
+//        if(avoidXNew<0)
+//            turnLampFlag=-1;
+//        else if(avoidXNew>0)
+//            turnLampFlag=1;
+//        else
+//            turnLampFlag=0;
+
+//        if(fabs(avoidXNew - avoidXNewLast)>0.1)
+//        {
+//            gpsTraceNow.clear();
+//            gpsTraceAvoidXY.clear();
+//            givlog->debug("decition_brain","avoidXNew: %f,avoidXNewLast: %f",
+//                          avoidXNew,avoidXNewLast);
+//            //gpsTraceAvoidXY = getGpsTraceOffsetAvoid(gpsTraceOri, avoidXNew,now_gps_ins);
+//            gpsTraceAvoidXY = splinePlanner->chooseRoadBySpline(now_gps_ins,gpsMapLine,PathPoint,obsSpline,avoidXNew,avoidXNewLast);
+//            vehState = avoiding;
+//            startAvoidGpsIns = now_gps_ins;
+//            obstacle_avoid_flag=1;
+//            hasCheckedAvoidLidar = false;
+//            avoidXNewLast=avoidXNew;
+//        }
+
+//        if(avoidXNewPreVector.size()<5){
+//            avoidXNewPreVector.push_back(avoidXNewPre);
+//        }else{
+//            if(avoidXNewPreVector[0]!=avoidXNewLast){
+//                for(unsigned int i=1;i<avoidXNewPreVector.size();i++){
+//                    if(avoidXNewPreVector[i]!=avoidXNewPreVector[0]){
+//                        avoidXNewPreFilter=avoidXNewLast;
+//                        break;
+//                    }
+//                    if(i==avoidXNewPreVector.size()-1)
+//                        avoidXNewPreFilter=avoidXNewPreVector[0];
+//                }
+//            }
+//            avoidXNewPreVector.clear();
+//        }
+//        if(avoidXNewPreFilter!=avoidXNewLast){
+//            avoidXNew=avoidXNewPre;
+//            if(avoidXNew<0)
+//                turnLampFlag=-1;
+//            else if(avoidXNew>0)
+//                turnLampFlag=1;
+//            else
+//                turnLampFlag=0;
+
+//            gpsTraceNow.clear();
+//            gpsTraceAvoidXY.clear();
+//            givlog->debug("decition_brain","avoidXNew: %f,avoidXNewLast: %f",
+//                          avoidXNew,avoidXNewLast);
+//            //gpsTraceAvoidXY = getGpsTraceOffsetAvoid(gpsTraceOri, avoidXNew,now_gps_ins);
+//            gpsTraceAvoidXY = splinePlanner->chooseRoadBySpline(now_gps_ins,gpsMapLine,PathPoint,obsSpline,avoidXNew,avoidXNewLast);
+//            vehState = avoiding;
+//            startAvoidGpsIns = now_gps_ins;
+//            obstacle_avoid_flag=1;
+//            hasCheckedAvoidLidar = false;
+//            avoidXNewLast=avoidXNew;
+//            avoidXNewPreFilter=avoidXNew;
+//        }
+//    }
+//20230407,
+//    if((vehState == preAvoid)||((avoidXNew!=0)))
+//  //    if((vehState == preAvoid)||(avoidXNew!=0))
+//      {
+//          dSpeed = min(8.0,dSpeed);//6gaiwei 10
+//          static int count_avoidx_0=0;
+//          double avoidLeft_value=0;
+//          double avoidRight_value=0;
+//          double* avoidLeft_p=&avoidLeft_value;
+//          double* avoidRight_p=&avoidRight_value;
+//          computeAvoidBoundary(roadOri,roadSum,gpsMapLine[PathPoint]->mfLaneWidth,ServiceCarStatus.msysparam.vehWidth,avoidLeft_p,avoidRight_p);
+//         // avoidXNewPre=computeAvoidXDistance(lidarGridPtr,now_gps_ins, gpsMapLine,lidar_per,avoidLeft_value,avoidRight_value,avoidXNewLast);
+//          avoidXNewPre=computeAvoidXDistance(lidarGridPtr,now_gps_ins, gpsMapLine,avoidLeft_value,avoidRight_value,avoidXNewLast);
+//          if(avoidXNewPreVector.size()<20){
+//              avoidXNewPreVector.push_back(avoidXNewPre);
+//          }else{
+//              if(avoidXNewPreVector[0]!=avoidXNewLast){
+//                  for(unsigned int i=1;i<avoidXNewPreVector.size();i++){
+//                      if(avoidXNewPreVector[i]!=avoidXNewPreVector[0]){
+//                          avoidXNewPreFilter=avoidXNewLast;
+//                          break;
+//                      }
+//                      if(i==avoidXNewPreVector.size()-1)
+//                          avoidXNewPreFilter=avoidXNewPreVector[0];
+//                  }
+//              }
+//              avoidXNewPreVector.clear();
+//          }
+//          if(avoidXNewPreFilter!=avoidXNewLast){
+//              avoidXNew=avoidXNewPre;
+//             //avoidXNew = avoidXNewPreFilter;
+//              if(avoidXNew<0)
+//                  turnLampFlag=-1;
+//              else if(avoidXNew>0)
+//                  turnLampFlag=1;
+//              else
+//                  turnLampFlag=0;
+
+//  //           gpsTraceNow.clear();
+////             gpsTraceAvoidXY.clear();
+//             givlog->debug("decition_brain","avoidXNew: %d,avoidXNewLast: %d",
+//                             avoidXNew,avoidXNewLast);
+//             //gpsTraceAvoidXY = getGpsTraceOffsetAvoid(gpsTraceOri, avoidXNew,now_gps_ins);
+//  //           gpsTraceAvoidXY = splinePlanner->chooseRoadBySpline(now_gps_ins,gpsMapLine,PathPoint,obsSpline,avoidXNew,avoidXNewLast);
+//             if(vehState == preAvoid)
+//             {
+//                 gpsTraceAvoidXY.clear();
+//                 gpsTraceAvoidXY = splinePlanner->chooseRoadBySpline(now_gps_ins,gpsMapLine,PathPoint,obsSpline,avoidXNew,avoidXNewLast);
+//             }
+//             else if(avoidXNew==0)
+//             {
+//                 count_avoidx_0++;
+//                 if(count_avoidx_0>45)// you 60 gaicheng 30
+//                 {
+//                     gpsTraceAvoidXY.clear();
+//                    gpsTraceAvoidXY = splinePlanner->chooseRoadBySpline(now_gps_ins,gpsMapLine,PathPoint,obsSpline,avoidXNew,avoidXNewLast);
+
+//                 }
+//              }
+//             if(avoidXNew!=0)
+//             {
+//                 count_avoidx_0=0;
+//             }
+//             vehState = avoiding;
+//             startAvoidGpsIns = now_gps_ins;
+//             obstacle_avoid_flag=1;
+//             hasCheckedAvoidLidar = false;
+//             avoidXNewLast=avoidXNew;
+//             avoidXNewPreFilter=avoidXNew;
+//          }
+//      }
+//  if (vehState == preAvoid)
+//  {
+//      //dSpeed = min(6.0,dSpeed);
+//      iv::Point2D now_s_d=cartesian_to_frenet1D(gpsMapLine,now_gps_ins.gps_x, now_gps_ins.gps_y);
+//              if ((obsDistance>ServiceCarStatus.aocfDis)&&(fabs(now_s_d.d)<0.05))
+//              {
+//                  // vehState = avoidObs; 0929
+//                  vehState=normalRun;
+//              }
+//  }
+//
+
+    //old_bz--------------------------------------------------------------------------------------------------
+    if (vehState == avoiding)
+    {
+        //        double avoidDistance=GetDistance(startAvoidGpsIns,now_gps_ins);   //Toggle 20220223
+        //        //若车辆到达变道后的路径，改变车辆状态，关闭frenet规划
+        //        if (useFrenet && abs(gpsTraceAim[0].x)<1.0) {
+        //            vehState = normalRun;
+        //            //            useFrenet = false;
+        //            //            useOldAvoid = true;
+        //        }
+        //        else if (useOldAvoid && avoidDistance>35) {    //zj 2021.06.21   gpsTraceNow[60].x)<0.02
+        //            // vehState = avoidObs; 0929
+        //            vehState = normalRun;
+        //            turnLampFlag=0;
+        //        }
+        //        else if (turnLampFlag>0)
+        //        {
+        //            gps_decition->leftlamp = false;
+        //            gps_decition->rightlamp = true;
+        //        }
+        //        else if(turnLampFlag<0)
+        //        {
+        //            gps_decition->leftlamp = true;
+        //            gps_decition->rightlamp = false;
+        //        }
+
+        dSpeed = min(6.0,dSpeed);
+        if (turnLampFlag>0)
+        {
+            gps_decition->leftlamp = false;
+            gps_decition->rightlamp = true;
+        }
+        else if(turnLampFlag<0)
+        {
+            gps_decition->leftlamp = true;
+            gps_decition->rightlamp = false;
+        }
+    }
+
+
+    if (vehState==preBack)
+    {
+        vehState = backOri;
+    }
+
+    if (vehState==backOri)
+    {
+        double backDistance=GetDistance(startBackGpsIns,now_gps_ins);
+        //若车辆到达变道后的路径，改变车辆状态，关闭frenet规划
+        if (useFrenet && abs(gpsTraceAim[0].x)<1.0) {
+            vehState = normalRun;
+            turnLampFlag=0;
+            //            useFrenet = false;
+            //            useOldAvoid = true;
+        }
+        else if (useOldAvoid && backDistance>35 ) {//abs(gpsTraceNow[60].x)<0.05
+            // vehState = avoidObs; 0929
+            vehState = normalRun;
+            obstacle_avoid_flag=0;
+        }
+        else if (turnLampFlag>0)
+        {
+            gps_decition->leftlamp = false;
+            gps_decition->rightlamp = true;
+        }
+        else if (turnLampFlag<0)
+        {
+            gps_decition->leftlamp = true;
+            gps_decition->rightlamp = false;
+        }
+    }
+
+
+    std::cout<<"\n原始RoadOri:%d\n"<<roadOri<<endl;
+    //   计算回归原始路线
+#ifdef AVOID_NEW
+    if((avoidXNew == 0.0)&&(vehState == avoiding))
+    {
+        iv::Point2D now_s_d=cartesian_to_frenet1D(gpsMapLine,now_gps_ins.gps_x, now_gps_ins.gps_y);
+        if(fabs(now_s_d.d)<0.05)
+        {
+            vehState=normalRun;
+            obstacle_avoid_flag=0;
+           // avoidXNewPreVector.clear();//20230526,huifu
+        }
+    }
+
+
+#else
+    if ((roadNow!=roadOri))
+    {
+        if(useFrenet){
+            if ((GetDistance(now_gps_ins, startAvoidGpsInsVector[roadOri])>max(avoidMinDistanceVector[roadOri],30.0)))
+            {
+                double offsetL = -(roadSum - 1)*roadWidth;
+                double offsetR = (roadOri - 0)*roadWidth;
+                roadPre = frenetPlanner->chooseRoadByFrenet(now_gps_ins,gpsMapLine,PathPoint,offsetL,offsetR,lidarGridPtr,roadNow);
+            }
+        }
+        else if(useOldAvoid){
+            roadPre = chooseBackRoad(lidarGridPtr, now_gps_ins, gpsMapLine,lidar_per);
+            //  avoidX = (roadOri - roadNow)*roadWidth; //1012
+            avoidX=computeAvoidX(roadNow,roadOri,gpsMapLine[PathPoint]);
+        }
+    }
+    if ((roadNow != roadOri && roadPre!=-1))
+    {
+
+        roadNow = roadPre;
+        //     avoidX = (roadOri - roadNow)*roadWidth;
+        avoidX=computeAvoidX(roadNow,roadOri,gpsMapLine[PathPoint]);
+
+        if(avoidX<0)
+            turnLampFlag<0;
+        else if(avoidX>0)
+            turnLampFlag>0;
+        else
+            turnLampFlag=0;
+
+        if(useOldAvoid){
+            //gpsTraceNow = getGpsTraceOffset(gpsTraceOri, avoidX);
+            gpsTraceAvoidXY.clear();
+            gpsTraceAvoidXY = getGpsTraceOffsetBackOri(gpsTraceOri, avoidX,now_gps_ins,gpsTraceNow);
+            startBackGpsIns = now_gps_ins;
+        }
+        else if(useFrenet){
+            gpsTraceNow = frenetPlanner->getPath(now_gps_ins,gpsMapLine,PathPoint,avoidX,lidarGridPtr);
+        }
+
+        vehState = backOri;
+        hasCheckedBackLidar = false;
+        //  checkBackObsTimes = 0;
+
+        cout<<"\n回归RoadNum:%d\n"<< roadNow<<endl;
+
+    }
+#endif
+
+    if ( vehState==changingRoad || vehState==chaocheBack)
+    {
+        double lastAng = 0.0 - lastAngle;
+
+        if(ServiceCarStatus.msysparam.mvehtype !="zhongche"){
+
+            if (controlAng>40)
+            {
+                controlAng =40;
+            }
+            else if (controlAng<-40)
+            {
+                controlAng = - 40;
+            }
+        }
+    }
+
+
+    //速度结合角度的限制
+    controlAng = limitAngle(realspeed, controlAng);
+
+    //1220
+    if(PathPoint+80<gpsMapLine.size()-1){
+        if(gpsMapLine[PathPoint+80]->roadMode==4  && !ServiceCarStatus.daocheMode){
+            changingDangWei=true;
+        }
+    }
+
+    if(changingDangWei){
+        if(abs(realspeed)>0.1){
+            dSpeed=0;
+        }else{
+            dSpeed=0;
+            gps_decition->dangWei=2;
+            ServiceCarStatus.daocheMode=true;
+        }
+    }
+
+    if(ServiceCarStatus.daocheMode)
+    {
+        controlAng=0-controlAng;
+    }
+
+
+    dSecSpeed = dSpeed / 3.6;
+    gps_decition->speed = dSpeed;
+
+    if (gpsMapLine[PathPoint]->runMode == 2)
+    {
+        obsDistance = -1;
+
+    }
+
+
+    //----------------------------------------shenlan 采集车车路协同,add---------------------------------------------
+    if(ServiceCarStatus.mRSUupdateTimer.elapsed()>10*1000)
+     {
+        ServiceCarStatus.rsu_traffic_type=200;//lvdeng
+        ServiceCarStatus.rsu_warning_type=200;
+ //       ServiceCarStatus.rsu_gps_lat = 0.0;  //路况信息或预警信息有效的时候才会使用到这写变量，所以这些变量的信息可不清除
+ //       ServiceCarStatus.rsu_gps_lng = 0.0;
+ //       ServiceCarStatus.rsu_trafficelimit_spd=200;
+     }
+     if(ServiceCarStatus.mRSUTrafficUpdateTimer.elapsed()>10*1000)
+     {
+        ServiceCarStatus.rsu_traffic_type=200;//lvdeng
+        ServiceCarStatus.rsu_trafficelimit_spd=200;
+
+//        ServiceCarStatus.iTrafficeLightLon=0;//20230310
+//        ServiceCarStatus.iTrafficeLightLat=0;//20230310
+
+ //       ServiceCarStatus.rsu_warning_type=200;
+ //       ServiceCarStatus.rsu_gps_lat = 0.0;  //路况信息或预警信息有效的时候才会使用到这写变量，所以这些变量的信息可不清除
+ //       ServiceCarStatus.rsu_gps_lng = 0.0;
+ //       ServiceCarStatus.rsu_trafficelimit_spd=200;
+     }
+     if(ServiceCarStatus.mRSUWarnUpdateTimer.elapsed()>10*1000)
+     {
+ //       ServiceCarStatus.rsu_traffic_type=200;//lvdeng
+        ServiceCarStatus.rsu_warning_type=200;
+        ServiceCarStatus.rsu_warnlimit_spd=200;
+ //       ServiceCarStatus.rsu_gps_lat = 0.0;  //路况信息或预警信息有效的时候才会使用到这写变量，所以这些变量的信息可不清除
+ //       ServiceCarStatus.rsu_gps_lng = 0.0;
+ //       ServiceCarStatus.rsu_trafficelimit_spd=200;
+     }
+    unsigned char traffic_type=ServiceCarStatus.rsu_traffic_type;//路况信息
+    unsigned char warning_type=ServiceCarStatus.rsu_warning_type;//预警信息，RSU获得
+    float distance_to_center=0;
+    GPS_INS traffic_center_gps;
+    traffic_center_gps.gps_lat=ServiceCarStatus.rsu_gps_lat;
+    traffic_center_gps.gps_lng=ServiceCarStatus.rsu_gps_lng;
+    float radiation_distance=ServiceCarStatus.rsu_radiation_distance;//事件辐射范围，从RSU获得
+    float trafficlimit_spd=ServiceCarStatus.rsu_trafficelimit_spd; //从RSU获得的限速值
+    float warnlimit_spd=ServiceCarStatus.rsu_warnlimit_spd; //从RSU获得的限速值
+    //以上变量信息都需要存储到log文件中
+    GaussProjCal(traffic_center_gps.gps_lng, traffic_center_gps.gps_lat,  &traffic_center_gps.gps_x, &traffic_center_gps.gps_y);
+   distance_to_center=GetDistance(now_gps_ins,traffic_center_gps);//因为是长直道路所以直接计算事件中心点和当前位置的距离即可
+
+   if((traffic_type==0x01)||(traffic_type==0x02)||(traffic_type==0x03)||(traffic_type==0x04)
+        ||(warning_type==0x01)||(warning_type==0x02))
+{
+   if((traffic_type==0x01)||(traffic_type==0x02)||(traffic_type==0x03))//塌方，施工，道路结冰，事件范围外10米缓慢减速，事件范围内停车
+    {
+       if((distance_to_center>=radiation_distance)&&(distance_to_center<=radiation_distance+10))
+       {
+          //dSpeed = min(1.0,realspeed-0.2);
+          dSpeed = max(1.0,realspeed-2);  //乘用车不同于底盘，速度是不稳定的，如果取最小一会就减成0了
+          ServiceCarStatus.vehicle_state_1x = 1;
+          ServiceCarStatus.target_spd_1x = 1.0;
+       }
+       else if(distance_to_center<radiation_distance)
+       {
+           dSpeed=0.0;
+           minDecelerate=-2.0;
+           ServiceCarStatus.vehicle_state_1x = 2;
+           ServiceCarStatus.target_spd_1x=0.0;
+       }
+       else
+       {}
+       if(ServiceCarStatus.txt_log_on==true)
+       {
+           givlog->debug("decition_brain","traffic_center_gps.gps_lat is : %f",traffic_center_gps.gps_lat);
+           givlog->debug("decition_brain","traffic_center_gps.gps_lng is : %f",traffic_center_gps.gps_lng);
+           givlog->debug("decition_brain","distance_to_center is : %f",distance_to_center);
+           givlog->debug("decition_brain","radiation_distance is : %f",radiation_distance);
+           givlog->debug("decition_brain","dSpeedd is : %f",dSpeed);
+           givlog->debug("decition_brain","minDecelerate is : %f",minDecelerate);
+           givlog->debug("decition_brain","traffic_type is : %d",traffic_type);
+       }
+    }
+   else if(traffic_type==0x04)//如果事件类型是限速，则在辐射范围内限速，辐射范围外正常行驶
+    {
+       if((distance_to_center>=radiation_distance)&&(distance_to_center<=radiation_distance+2))
+       {
+//          dSpeed=min((double)trafficlimit_spd,realspeed-0.5);//先让速度稍微减少一点
+          if(realspeed-2>trafficlimit_spd)
+           {
+                dSpeed=realspeed-2;//先让速度稍微减少一点
+           }
+           ServiceCarStatus.vehicle_state_1x = 1;
+           ServiceCarStatus.target_spd_1x = trafficlimit_spd;
+       }
+       else if(distance_to_center<radiation_distance)
+       {
+          //dSpeed=min((double)trafficlimit_spd,realspeed);
+           dSpeed=(double)trafficlimit_spd;
+           ServiceCarStatus.vehicle_state_1x = 1;
+           ServiceCarStatus.target_spd_1x = trafficlimit_spd;
+       }
+       else
+       {}
+       if(ServiceCarStatus.txt_log_on==true)
+       {
+           givlog->debug("decition_brain","distance_to_center is : %f",distance_to_center);
+           givlog->debug("decition_brain","radiation_distance is : %f",radiation_distance);
+           givlog->debug("decition_brain","dSpeed is : %f",dSpeed);
+           givlog->debug("decition_brain","traffic_type is : %d",traffic_type);
+       }
+    }
+    else
+    {}
+//碰撞预警，1减速，2 停车
+    if(warning_type==0x01)
+    {
+        dSpeed=warnlimit_spd;//此处要判断限速值是不是在有效值范围以内
+        ServiceCarStatus.vehicle_state_1x = 1;
+        ServiceCarStatus.target_spd_1x = warnlimit_spd;
+    }
+    else if(warning_type==0x02)
+    {
+        dSpeed=0.0;
+         ServiceCarStatus.vehicle_state_1x = 2;
+         ServiceCarStatus.target_spd_1x = 0;
+    }
+    else
+    {}
+}
+else
+{
+    ServiceCarStatus.vehicle_state_1x = 0;
+    ServiceCarStatus.target_spd_1x = dSpeed;
+}
+
+    std::cout<<"juecesudu0="<<dSpeed<<std::endl;
+
+    //ServiceCarStatus.milightCheckTimer.elapsed();
+    if(ServiceCarStatus.milightCheckTimer.elapsed()>5*1000)
+    {
+       ServiceCarStatus.iTrafficeLightColor=1;//lvdeng
+       ServiceCarStatus.iTrafficeLightTime=200;
+    }
+
+    //-------------------------------traffic light----------------------------------------------------------------------------------------
+
+    //1125 traficc
+    traffic_light_gps.gps_lat=ServiceCarStatus.iTrafficeLightLat;
+    traffic_light_gps.gps_lng=ServiceCarStatus.iTrafficeLightLon;
+    GaussProjCal(traffic_light_gps.gps_lng,traffic_light_gps.gps_lat, &traffic_light_gps.gps_x, &traffic_light_gps.gps_y);
+
+    if(traffic_light_gps.gps_lat!=0 && traffic_light_gps.gps_lng!=0){
+        traffic_light_pathpoint = Compute00().getNoAngleNearestPointIndex(traffic_light_gps, gpsMapLine);
+        //    traffic_light_pathpoint=130;
+        //float traffic_speed = ComputeTrafficLightSpeed(ServiceCarStatus.iTrafficeLightColor,gpsMapLine, traffic_light_pathpoint,PathPoint,secSpeed);
+        float traffic_speed=ComputeTrafficLightSpeed(ServiceCarStatus.iTrafficeLightColor,ServiceCarStatus.iTrafficeLightTime,gpsMapLine,
+                                                             traffic_light_pathpoint ,PathPoint,secSpeed,dSpeed);
+
+        dSpeed = min((double)traffic_speed,dSpeed);
+        if(traffic_speed==0)
+        {
+            minDecelerate=-2.0; //-2.0 ,深蓝车速比较小将减速度改小一些
+        }
+        if(ServiceCarStatus.txt_log_on==true)
+        {
+           givlog->debug("decition_brain","traffic_light_pathpoint is : %d",traffic_light_pathpoint);
+           givlog->debug("decition_brain","traffic_speed is : %f",traffic_speed);
+           givlog->debug("decition_brain","traffic_light_gps.gps_lat is : %f",traffic_light_gps.gps_lat);
+           givlog->debug("decition_brain","traffic_light_gps.gps_lng is : %f",traffic_light_gps.gps_lng);
+           givlog->debug("decition_brain","ServiceCarStatus.iTrafficeLightColor is : %d",ServiceCarStatus.iTrafficeLightColor);
+           givlog->debug("decition_brain","ServiceCarStatus.iTrafficeLightTime is : %d",ServiceCarStatus.iTrafficeLightTime);
+        }
+    }
+    //-------------------------------traffic light  end-----------------------------------------------------------------------------------------------
+
+
+    //-------------------------------v2x traffic light-----------------------------------------------------------------------------------------
+
+
+    double v2xTrafficSpeed= ComputeV2XTrafficLightSpeed( trafficLight,  gpsMapLine, v2xTrafficVector,
+                                                         PathPoint, secSpeed, dSpeed,  circleMode);
+
+
+    dSpeed = min(v2xTrafficSpeed,dSpeed);
+
+    //------------------------------v2x traffic light end--------------------------------------------------------------------------------------
+
+    //20230310 add begin,shanlan 由决策决定什么时候开启红绿等检测
+    float distance_to_stopline;
+    distance_to_stopline=GetDistance(now_gps_ins,traffic_light_gps);
+    givlog->debug("decition_brain","distance_to_stopline is : %f",distance_to_stopline);
+    if(distance_to_stopline<20.0)
+    {
+        ServiceCarStatus.mLightStartSensorBtn=true;
+
+    }
+    else
+    {
+        ServiceCarStatus.mLightStartSensorBtn=false;
+    }
+    //20230310 add end,由决策决定什么时候开启红绿等检测
+
+    transferGpsMode2(gpsMapLine);
+/**************************zhuche/tingche logic begin**************************************************************/
+    //准备驻车
+    if (readyZhucheMode)
+    {
+        cout<<"ready ZHECHE STATE+++++++++++++++++++++++++++++++++"<<endl;
+        cout<<"zhuche point : "<<zhuchePointNum<<endl;
+        double dis = GetDistance(*gpsMapLine[zhuchePointNum], now_gps_ins);
+        if (dis<35)
+        {
+            Point2D pt = Coordinate_Transfer((*gpsMapLine[zhuchePointNum]).gps_x, (*gpsMapLine[zhuchePointNum]).gps_y, now_gps_ins);
+            double zhucheDistance = pt.y;
+#if 0
+            if (dSpeed > 15)
+            {
+                dSpeed = 15;
+            }
+
+            if (zhucheDistance < 20 && dis < 25)
+            {
+                dSpeed = min(dSpeed, 5.0);
+            }
+#else
+            if (dSpeed > 12)
+            {
+                dSpeed = 12;
+            }
+
+            if (zhucheDistance < 9 && dis < 9)
+            {
+                dSpeed = min(dSpeed, 5.0);
+            }
+#endif
+            if (zhucheDistance < 3.0 && dis < 3.5)
+            {
+                dSpeed = min(dSpeed, 5.0);
+                zhucheMode = true;
+                readyZhucheMode = false;
+                cout<<"jinru ZHECHE STATE+++++++++++++++++++++++++++++++++"<<endl;
+            }
+        }
+    }
+    //驻车
+    if (zhucheMode)
+    {
+        int mzcTime = ServiceCarStatus.msysparam.mzhuchetime * 1000;
+
+        //        if(trumpet()<16000)
+        if (trumpet()<mzcTime)
+        {
+            //            if (trumpet()<12000){
+            cout<<"ZHECHE ING STATE+++++++++++++++++++++++++++++++++"<<endl;
+            minDecelerate=-1.0;
+            if(abs(realspeed)<0.2){
+                controlAng=0;  //tju
+            }
+        }
+        else
+        {
+            hasZhuched = true; //1125
+            zhucheMode = false;
+            trumpetFirstCount = true;
+            cout<<"ZHECHE STATE JIESHU +++++++++++++++++++++++++++++++++"<<endl;
+        }
+    }
+    //判断驻车标志位
+    if (hasZhuched && GetDistance(now_gps_ins,*gpsMapLine[zhuchePointNum])>20)
+    {
+        hasZhuched = false;
+    }
+
+    if (readyParkMode)
+    {
+        double parkDis = GetDistance(now_gps_ins, *gpsMapLine[finishPointNum]);
+        hasZhuched = true;
+
+        if (parkDis < 25)
+        {
+            Point2D pt = Coordinate_Transfer((*gpsMapLine[finishPointNum]).gps_x, (*gpsMapLine[finishPointNum]).gps_y, now_gps_ins);
+            double parkDistance = pt.y;
+            if (dSpeed > 15)
+            {
+                dSpeed = 15;
+            }
+
+            //dSpeed = 15;//////////////////////////////
+
+            if (parkDistance < 15 && parkDistance >= 4.5 && parkDis<20)
+            {
+                dSpeed = min(dSpeed, 8.0);
+            }
+            else if (parkDistance < 4.5 && parkDistance >= 3.5 && parkDis<5.0)
+            {
+                dSpeed = min(dSpeed, 5.0);
+            }
+            else if(parkDistance < 3.5 && parkDis<4.0)
+            {
+                dSpeed = min(dSpeed, 3.0);
+            }
+
+            //            float stopDis=2;
+            //            if(ServiceCarStatus.msysparam.mvehtype=="ge3"){
+            //                stopDis=1.6;
+            //            } else if(ServiceCarStatus.msysparam.mvehtype=="vv7"){
+            //                stopDis=1.8;
+            //            }
+
+            if (parkDistance < 1.6  && parkDis<2.0)
+            {
+                // dSpeed = 0;
+                parkMode = true;
+                readyParkMode = false;
+            }
+        }
+    }
+
+    if (parkMode)
+    {
+        minDecelerate=-1.0;
+
+        if(finishPointNum<0||finishPointNum>=gpsMapLine.size()){
+            parkMode=false;
+        }else if(GetDistance(now_gps_ins, *gpsMapLine[finishPointNum])>20){
+            parkMode=false;
+        }
+
+    }
+
+/**************************zhuche/tingche logic end**************************************************************/
+
+    for(int i = 0;i<ServiceCarStatus.car_stations.size();i++)
+    {
+        GPS_INS gpsIns;
+        GaussProjCal(ServiceCarStatus.car_stations[i].longtitude, ServiceCarStatus.car_stations[i].latitude,  &gpsIns.gps_x, &gpsIns.gps_y);
+
+        double dis = GetDistance(gpsIns, now_gps_ins);
+        if(dis<20)
+            ServiceCarStatus.currentstation = ServiceCarStatus.car_stations[i].ID;
+    }
+/**************************huche begin********************************************************************/
+    std::cout << "\n呼车指令状态:%d\n" <<  ServiceCarStatus.carState << std::endl;
+
+    if (ServiceCarStatus.carState == 0 && busMode)
+    {
+        minDecelerate=-1.0;
+    }
+
+    if (ServiceCarStatus.carState == 3 && busMode)
+    {
+        if(realspeed<0.1){
+            ServiceCarStatus.carState=0;
+            minDecelerate=-1.0;
+        }else{
+            nearStation=true;
+            dSpeed=0;
+        }
+    }
+
+    ///kkcai, 2020-01-03
+    //if (ServiceCarStatus.carState == 2 && busMode)
+    if (ServiceCarStatus.carState == 2)
+    {
+        aim_gps_ins.gps_lat=ServiceCarStatus.amilat;
+        aim_gps_ins.gps_lng=ServiceCarStatus.amilng;
+        //   gps2xy(ServiceCarStatus.amilat, ServiceCarStatus.amilng, &aim_gps_ins.gps_x, &aim_gps_ins.gps_y);
+        GaussProjCal( aim_gps_ins.gps_lng, aim_gps_ins.gps_lat,  &aim_gps_ins.gps_x, &aim_gps_ins.gps_y);
+        std::cout<<"lng"<<ServiceCarStatus.amilng<<std::endl;
+        qDebug("lat:%f", aim_gps_ins.gps_lat);
+        qDebug("lon:%f", aim_gps_ins.gps_lng);
+
+        std::cout<<"lat"<<ServiceCarStatus.amilat<<std::endl;
+        double dis = GetDistance(aim_gps_ins, now_gps_ins);
+        Point2D pt = Coordinate_Transfer((aim_gps_ins).gps_x, (aim_gps_ins).gps_y, now_gps_ins);
+
+        std::cout << "\n呼车dis距离:%f\n" << dis << std::endl;
+        std::cout << "\n呼车Y距离:%f\n" << pt.y << std::endl;
+
+        //         if (dis<20 && pt.y<8&& realspeed<0.1)
+        if (dis<20 && pt.y<5 && abs(pt.x)<3)
+        {
+            dSpeed = 0;
+            nearStation=true;
+            //is_arrivaled = true;
+            //ServiceCarStatus.status[0] = true;
+            ServiceCarStatus.istostation=1;
+            minDecelerate=-1.0;
+        }
+
+        else if (dis<20 && pt.y<15 && abs(pt.x)<3)
+        {
+            nearStation=true;
+            dSpeed = min(8.0, dSpeed);
+        }
+        else if (dis<30 && pt.y<20 && abs(pt.x)<3)
+        {
+            dSpeed = min(15.0, dSpeed);
+        }
+
+        else if (dis<50 && abs(pt.x)<3)
+        {
+            dSpeed = min(20.0, dSpeed);
+        }
+    }
+
+   /**************************huche end********************************************************************/
+    if(ServiceCarStatus.msysparam.mvehtype=="hapo")
+    {
+        if(obsDistance>0 && obsDistance<8)
+        {
+            dSpeed=0;
+        }
+    }
+//    else if(obsDistance>0 && obsDistance<15)
+    else if(obsDistance>0 && obsDistance<  ServiceCarStatus.socfDis)
+    {
+        dSpeed=0;
+    }
+
+    if(ServiceCarStatus.group_control){
+        dSpeed = ServiceCarStatus.group_comm_speed;
+    }
+
+    if(dSpeed==0){
+        if(realspeed<6)
+            minDecelerate=min(-0.5f,minDecelerate);
+        else
+            minDecelerate=min(-0.6f,minDecelerate); //1.0f    zj-0.6
+    }
+
+
+
+    if((ServiceCarStatus.table_look_up_on==true)&&(hapo_Adapter->IsINterpolationOK()))
+    {
+        if(acc_end<0)
+        {
+            if(minDecelerate > acc_end) minDecelerate = acc_end;
+        }
+    }
+
+    if(ServiceCarStatus.msysparam.mvehtype=="shenlan")
+    {
+        if(dSpeed>20.0)
+        {
+            dSpeed=20.0;   //shenlan bisai xiansu 10
+        }
+    }
+
+    std::cout<<"juecesudu0="<<dSpeed<<std::endl;
+
+    if(obsDistance == 0)obsDistance = -1;
+
+    phaseSpeedDecition(gps_decition, secSpeed, obsDistance, obsSpeed,now_gps_ins);
+    gps_decition->wheel_angle = 0.0 - controlAng;
+
+    Point2D now_s_d=iv::decition::cartesian_to_frenet1D(gpsMapLine,now_gps_ins.gps_x,now_gps_ins.gps_y);
+
+    givlog->debug("decition_brain","vehState: %d,PathPoint: %d,dSpeed: %f,obsDistance: %f,obsSpeed: %f,realspeed: %f,minDecelerate: %f,torque: %f,brake: %f,wheel_angle: %f,obs_speed_for_avoid: %f,mode2: %d,now_s: %f,now_d: %f",
+                  vehState,PathPoint,dSpeed,obsDistance,obsSpeed,realspeed,minDecelerate,gps_decition->torque,gps_decition->brake,gps_decition->wheel_angle,obs_speed_for_avoid,gpsMapLine[PathPoint]->mode2,now_s_d.s,now_s_d.d);
+
+    if(ServiceCarStatus.txt_log_on==true){
+        QDateTime dt = QDateTime::currentDateTime();
+        //将数据写入文件开始
+        ofstream outfile;
+        outfile.open("control_log.txt", ostream::app);
+        outfile<<dt.date().year()<<"-"<<dt.date().month()<<"-"<<dt.date().day()<<" "<<dt.time().hour()<<":"<<dt.time().minute()<<":"<<dt.time().second()<<"-"<<dt.time().msec()<<'\t'
+              <<vehState<<'\t'<<PathPoint<<'\t'<<setprecision(4)<<dSpeed<<'\t'<<obsDistance<<'\t'<<obsSpeed<<'\t'<<realspeed<<'\t'<<minDecelerate<<'\t'
+             <<gps_decition->torque<<'\t'<<gps_decition->brake<<'\t'<<gps_decition->wheel_angle<<'\t'<<now_gps_ins.gps_x<<'\t'<<now_gps_ins.gps_y<<'\t'<<now_s_d.s<<'\t'<<now_s_d.d<<endl;
+        outfile.close();
+        //将数据写入文件结束
+    }
+    //shuju cunchu gognnenng add,20210624,cxw
+    if(ServiceCarStatus.txt_log_on==true)
+    {
+        if(first_start_en)
+         {
+            first_start_en=false;
+            start_tracepoint = PathPoint;
+            if(circleMode)
+            {
+                if(start_tracepoint==0)
+                {
+
+                    end_tracepoint =gpsMapLine.size()-1;  //这种计算终点坐标的序号只适合与闭环地图
+                }
+                else
+                {
+                    end_tracepoint=start_tracepoint-1;
+                }
+            }
+            else
+            {
+               end_tracepoint =gpsMapLine.size()-1; //20210929,不是闭环地图的时候终点参考坐标就是地图数组的最大序号
+            }
+         }
+         double dis1 =  GetDistance(now_gps_ins, *gpsMapLine[start_tracepoint]);
+         double dis2 = 10;// GetDistance(now_gps_ins, *gpsMapLine[end_tracepoint]);
+         if(brake_mode==true)
+         {
+             dis2=2;
+         }
+
+         //double dis2 =  GetDistance(now_gps_ins, *gpsMapLine[end_tracepoint]);
+         if(circleMode)//闭环地图
+         {
+             if(dis1<3&&dis2<3&&circleMode) //距离不能太小，太小了可能会错过，所以适当将距离增大一些
+            // if(dis1<1&&dis2<1&&circleMode)
+             {
+                 file_cnt_add_en=true;  //201200102
+             }
+             else
+             {
+                 file_cnt_add_en=false;
+             }
+
+             if(dis1>10)
+             {
+                file_cnt_add=true;
+             }
+
+             if(file_cnt_add_en&&file_num<256&&file_cnt_add)
+             {
+                 file_num++;
+                 file_cnt_add=false;
+                 map_start_point=true;
+             }
+             if(file_num==256)
+             {
+                 file_num=1;
+             }
+             else
+             {
+             }
+             QDateTime dt=QDateTime::currentDateTime();
+             QString date =dt.toString("yyyy_MM_dd_hhmm");
+             QString strsen = "1xdatalog_";
+             date.append(strsen);
+             QString s = QString::number(file_num);
+             date.append(s);
+             date.append(".txt");
+             string filename;
+             filename=date.toStdString();
+             ofstream outfile;
+             outfile.open(filename, ostream::app);
+             if((file_cnt_add==false)&&(map_start_point==true))
+             {
+                 QDateTime dt=QDateTime::currentDateTime();
+                 outfile<<dt.date().year()<<"-"<<dt.date().month()<<"-"<<dt.date().day()<<" "<<dt.time().hour()<<":"
+                        <<dt.time().minute()<<":"<<dt.time().second()<<"-"<<dt.time().msec()
+                        <<endl;
+                 outfile<<"***********************the vehicle at map start point!*************************"<<endl;
+                 outfile<<"the number of lap  is "<<":    "<<file_num<<""<<endl;
+                 map_start_point=false;
+             }
+             QDateTime dt2=QDateTime::currentDateTime();
+             outfile <<dt2.time().hour()<<":"<<dt2.time().minute()<<":"<<dt2.time().second()<<":"<<dt2.time().msec()<<","
+                    <<"Decide_Dspeed"  << ","  <<setprecision(2)<<dSpeed<< ","
+                    <<"Decide_ACC"  << ","  <<setprecision(2)<<gps_decition->torque<< ","
+                    <<"Decide_Brake"<< ","  <<gps_decition->brake<< ","
+                    <<"Vehicle_RealSpd"<< ","  <<setprecision(2)<<now_gps_ins.speed<< ","
+                    <<"Decide_Angle"<< ","  << setprecision(2)<<gps_decition->wheel_angle<< ","
+                    <<"Vehicle_GPS_heading"<< ","<< setprecision(10)<<now_gps_ins.ins_heading_angle<< ","
+                    <<"Vehicle_GPS_X"<< ","<< setprecision(10)<<now_gps_ins.gps_lat<< ","
+                    <<"Vehicle_GPS_Y"<< ","<< setprecision(10)<<now_gps_ins.gps_lng<< ","
+                    <<"Trace_Point"<< ","<<PathPoint<< ","
+                    <<"OBS_Distance"<< ","<<obsDistance<< ","
+                    <<"OBS_Speed"<< ","<<obsSpeed<< ","
+                    <<endl;
+             outfile.close();
+         }
+         else //fei yuanxing luxian
+         {
+             if(dis1<3)
+             {
+                 file_cnt_add_en=true;  //201200102
+             }
+             else
+             {
+                 file_cnt_add_en=false;
+             }
+
+             if(dis1>3)
+             {
+                file_cnt_add=true;
+             }
+
+             if(file_cnt_add_en&&file_num<256&&file_cnt_add)
+             {
+                 file_num++;
+                 file_cnt_add=false;
+                 map_start_point=true;
+             }
+             if(file_num==256)//20210712
+             {
+                 file_num=1;
+             }
+             else
+             {
+             }
+             QDateTime dt=QDateTime::currentDateTime();
+             QString date =dt.toString("yyyy_MM_dd_hhmm");
+             QString strsen = "datalog_";
+             date.append(strsen);
+             QString s = QString::number(file_num);
+             date.append(s);
+             date.append(".txt");
+             string filename;
+             filename=date.toStdString();
+             ofstream outfile;
+             outfile.open(filename, ostream::app);
+             if((file_cnt_add==false)&&(map_start_point==true))
+             {
+                 QDateTime dt=QDateTime::currentDateTime();
+                 outfile<<dt.date().year()<<"-"<<dt.date().month()<<"-"<<dt.date().day()<<" "<<dt.time().hour()<<":"<<dt.time().minute()<<":"<<dt.time().second()<<"-"<<dt.time().msec()
+                       <<endl;
+                 outfile<<"********the vehicle near map start point!********"<<endl;
+                   outfile<<"the number of lap  is "<<":" <<file_num<<""<<endl;
+                 map_start_point=false;
+             }
+             if(dis2<3)
+             {
+                 outfile<<"********the vehicle near map end point!********" <<endl;
+                 QDateTime dt=QDateTime::currentDateTime();
+                 outfile<<dt.date().year()<<"-"<<dt.date().month()<<"-"<<dt.date().day()<<" "<<dt.time().hour()<<":"<<dt.time().minute()<<":"<<dt.time().second()<<"-"<<dt.time().msec()
+                       <<endl;
+             }
+             else
+             {
+                 float ttc = 0-(obsDistance/obsSpeed);
+                 double obsDistance_log=0;
+                 if(obsDistance>500)
+                     obsDistance_log=100;
+                 else
+                     obsDistance_log=obsDistance;
+                 QDateTime dt2=QDateTime::currentDateTime();
+                 double disToEndTrace =  GetDistance(now_gps_ins, *gpsMapLine[end_tracepoint]);
+                 double disToParkPoint =  GetDistance(now_gps_ins, aim_gps_ins);
+                 outfile <<dt2.time().hour()<<":"<<dt2.time().minute()<<":"<<dt2.time().second()<<":"<<dt2.time().msec()<<","
+                         <<"Decide_Dspeed"  << ","  <<setprecision(2)<<dSpeed<< ","
+                         <<"Decide_ACC"  << ","  <<setprecision(2)<<gps_decition->torque<< ","
+                         <<"Decide_Brake"<< ","  <<gps_decition->brake<< ","
+                         <<"Vehicle_RealSpd"<< ","  <<setprecision(2)<<now_gps_ins.speed<< ","
+        //                             <<"readyParkMode"<< ","<<readyParkMode<< ","
+        //                             <<"readyZhucheMode"<< ","<<readyZhucheMode<< ","
+                         <<"trace_disToEndTrace"<< ","<<disToEndTrace<< ","
+                         <<"disToParkPoint"<< ","<<disToParkPoint<< ","
+                         <<"OBS_Distance"<< ","<<obsDistance_log<< ","
+                         <<"OBS_Speed"<< ","<<obs_speed_for_avoid<< ","
+                         <<"obsSpeed_fusion"<<","<<obsSpeed<<","
+                         <<"SecSpeed"<<","<<secSpeed<<","
+                         <<"Vehicle_state"<< ","<<vehState<< ","
+                         <<"avoid_flag"<<","<<obstacle_avoid_flag<<","
+                         <<"avoidXNew"<<","<< setprecision(3)<<avoidXNew<<","
+                         <<"avoidXNewPre"<<","<< setprecision(3)<<avoidXNewPre<<","
+                         <<"Now_s_d_d"<< ","<< setprecision(3)<<now_s_d.d<< ","
+                         <<"Now_s_d_s"<< ","<< setprecision(3)<<now_s_d.s<< ","
+                         //<<"avoidXPre"<<","<<avoidXPre<<","
+                         <<"ObsTimeEnd"<<","<<ObsTimeEnd<<","
+                         <<"avoidXY_size"<<","<<gpsTraceAvoidXY.size()<<","
+                         <<"Min_Decelation"","<<minDecelerate<< ","
+                         <<"Decide_Angle"<< ","  << setprecision(2)<<gps_decition->wheel_angle<< ","
+                         <<"Vehicle_GPS_heading"<< ","<< setprecision(10)<<now_gps_ins.ins_heading_angle<< ","
+                         <<"Vehicle_GPS_lat"<< ","<< setprecision(10)<<now_gps_ins.gps_lat<< ","
+                         <<"Vehicle_GPS_lng"<< ","<< setprecision(10)<<now_gps_ins.gps_lng<< ","
+                         <<"Vehicle_GPS_X"<< ","<< setprecision(10)<<now_gps_ins.gps_x<< ","
+                         <<"Vehicle_GPS_Y"<< ","<< setprecision(10)<<now_gps_ins.gps_y<< ","
+
+                        <<"aim_GPS_lat"<< ","<< setprecision(10)<<aim_gps_ins.gps_lat<< ","
+                        <<"aim_GPS_lng"<< ","<< setprecision(10)<<aim_gps_ins.gps_lng<< ","
+                        <<"aim_GPS_X"<< ","<< setprecision(10)<<aim_gps_ins.gps_x<< ","
+                        <<"aim_GPS_Y"<< ","<< setprecision(10)<<aim_gps_ins.gps_y<< ","
+
+                         <<"MAP_GPS_heading"<< ","<< setprecision(10)<<gpsMapLine[PathPoint]->ins_heading_angle<< ","
+                         <<"MAP_GPS_X"<< ","<< setprecision(10)<<gpsMapLine[PathPoint]->gps_x<< ","
+                         <<"MAP_GPS_Y"<< ","<< setprecision(10)<<gpsMapLine[PathPoint]->gps_y<< ","
+                         <<"Trace_Point"<< ","<<PathPoint<< ","
+                         <<"changingDangWei"<< ","<<changingDangWei<< ","
+                         <<"gpsTraceOri"<< ","<<gpsTraceOri.size()<< ","
+                         <<"TTC"<< ","<<ttc<< ","
+                         <<"LightColor"  << ","  <<ServiceCarStatus.iTrafficeLightColor<< ","
+                         <<"LightTime"<< ","  <<ServiceCarStatus.iTrafficeLightTime<< ","
+                         <<"DisTostopline"<< ","  <<setprecision(2)<<distance_to_stopline<< ","
+        //                               <<"OBS_Distance"<< ","<<obsDistance_log<< ","
+        //                               <<"OBS_Speed"<< ","<<obs_speed_for_avoid<< ","
+        //                               <<"Vehicle_state"<< ","<<vehState<< ","
+        //                               <<"avoid_flag"<<","<<obstacle_avoid_flag<<","
+        //                               <<"avoidXNew"<<","<<avoidXNew<<","
+        //                               <<"avoidXNewPre"<<","<<avoidXNewPre<<","
+        ////                               <<"avoidXPre"<<","<<avoidXPre<<","
+        //                               <<"ObsTimeEnd"<<","<<ObsTimeEnd<<","
+                       <<"avoidXY_size"<<","<<gpsTraceAvoidXY.size()<<","
+        //                               <<"Min_Decelation"","<<minDecelerate<< ","
+                         <<"Decide_Angle"<< ","  << setprecision(2)<<gps_decition->wheel_angle<< ","
+                        // <<"Vehicle_GPS_heading"<< ","<< setprecision(10)<<now_gps_ins.ins_heading_angle<< ","
+                        // <<"Vehicle_GPS_X"<< ","<< setprecision(10)<<now_gps_ins.gps_lat<< ","
+                        // <<"Vehicle_GPS_Y"<< ","<< setprecision(10)<<now_gps_ins.gps_lng<< ","
+                        // <<"Trace_Point"<< ","<<PathPoint<< ","
+                        // <<"OBS_Speed"<< ","<<obsSpeed<< ","
+                        // <<"OBS_Distance"<< ","<<obsDistance<< ","
+                        // <<"TTC"<< ","<<ttc<< ","
+                         <<endl;
+                 outfile.close();
+             }
+          }
+     }
+
+    lastAngle=gps_decition->wheel_angle;
+    lastVehState=vehState;
+
+    //   qDebug("decide1 time is %d",xTime.elapsed());
+    return gps_decition;
+}
+
+
+
+void iv::decition::DecideGps00::initMethods(){
+
+    pid_Controller= new PIDController();
+
+    ge3_Adapter = new Ge3Adapter();
+    qingyuan_Adapter = new QingYuanAdapter();
+    vv7_Adapter = new VV7Adapter();
+    zhongche_Adapter = new ZhongcheAdapter();
+    bus_Adapter = new BusAdapter();
+    hapo_Adapter = new HapoAdapter();
+    sightseeing_Adapter = new SightseeingAdapter();
+    changanshenlan_Adapter = new ChanganShenlanAdapter();
+
+
+    mpid_Controller.reset(pid_Controller);
+    mge3_Adapter.reset(ge3_Adapter);
+    mqingyuan_Adapter.reset(qingyuan_Adapter);
+    mvv7_Adapter.reset(vv7_Adapter);
+    mzhongche_Adapter.reset(zhongche_Adapter);
+    mbus_Adapter.reset(bus_Adapter);
+    mhapo_Adapter.reset(hapo_Adapter);
+    msightseeing_Adapter.reset(sightseeing_Adapter);
+    mchanganshenlan_Adapter.reset(changanshenlan_Adapter);
+
+    frenetPlanner = new FrenetPlanner();
+    //    laneChangePlanner = new LaneChangePlanner();
+
+    mfrenetPlanner.reset(frenetPlanner);
+    //    mlaneChangePlanner.reset(laneChangePlanner);
+
+}
+
+
+void iv::decition::DecideGps00::phaseSpeedDecition(iv::decition::Decition  decition, double secSpeed, double obsDis, double obsSpeed, GPS_INS gpsIns ) {
+
+    pid_Controller->getControlDecition( gpsIns, gpsTraceNow,  dSpeed,  obsDis,  obsSpeed, false, true, &decition);
+
+    if((ServiceCarStatus.table_look_up_on==true)&&(hapo_Adapter->IsINterpolationOK()))
+    {
+        if((decition->accelerator > minDecelerate) && (minDecelerate < 0))
+        {
+            decition->accelerator = minDecelerate;
+        }
+    }
+
+    if(ServiceCarStatus.msysparam.mvehtype=="ge3"){
+        ge3_Adapter->getAdapterDeciton(gpsIns, gpsTraceNow, dSpeed, obsDis,obsSpeed,  decition->accelerator,ServiceCarStatus.mfCalAcc  ,changingDangWei, &decition);
+    }else if( ServiceCarStatus.msysparam.mvehtype=="qingyuan"){
+        qingyuan_Adapter->getAdapterDeciton(gpsIns, gpsTraceNow, dSpeed, obsDis,obsSpeed,  decition->accelerator,ServiceCarStatus.mfCalAcc  ,changingDangWei, &decition);
+    }else if( ServiceCarStatus.msysparam.mvehtype=="vv7"){
+        vv7_Adapter->getAdapterDeciton(gpsIns, gpsTraceNow, dSpeed, obsDis,obsSpeed,  decition->accelerator,ServiceCarStatus.mfCalAcc  ,changingDangWei, &decition);
+    }else if( ServiceCarStatus.msysparam.mvehtype=="zhongche"){
+        zhongche_Adapter->getAdapterDeciton(gpsIns, gpsTraceNow, dSpeed, obsDis,obsSpeed,  decition->accelerator,ServiceCarStatus.mfCalAcc  ,changingDangWei, &decition);
+    }else if(ServiceCarStatus.msysparam.mvehtype=="bus"){
+        bus_Adapter->getAdapterDeciton(gpsIns, gpsTraceNow, dSpeed, obsDis,obsSpeed,  decition->accelerator,ServiceCarStatus.mfCalAcc  ,changingDangWei, &decition);
+    }else if(ServiceCarStatus.msysparam.mvehtype=="hapo"){
+        hapo_Adapter->getAdapterDeciton(gpsIns, gpsTraceNow, dSpeed, obsDis,obsSpeed,  decition->accelerator,ServiceCarStatus.mfCalAcc  ,changingDangWei, &decition);
+    }else if(ServiceCarStatus.msysparam.mvehtype=="sightseeing"){
+        sightseeing_Adapter->getAdapterDeciton(gpsIns, gpsTraceNow, dSpeed, obsDis,obsSpeed,  decition->accelerator,ServiceCarStatus.mfCalAcc  ,changingDangWei, &decition);
+    }
+    else if(ServiceCarStatus.msysparam.mvehtype=="shenlan"){
+        changanshenlan_Adapter->getAdapterDeciton(gpsIns, gpsTraceNow, dSpeed, obsDis,obsSpeed,  decition->accelerator,ServiceCarStatus.mfCalAcc  ,changingDangWei, &decition);
+    }
+}
+
+
+
+std::vector<iv::Point2D> iv::decition::DecideGps00::getGpsTrace(iv::GPS_INS now_gps_ins, const std::vector<GPSData> gpsMapLine, int lastIndex, bool circleMode) {
+    vector<iv::Point2D> trace;
+
+    traceOriLeft.clear();
+    traceOriRight.clear();
+    int tracesize=800;
+    if(ServiceCarStatus.msysparam.mvehtype=="shenlan")
+    {
+        tracesize=400;//400;
+    }
+    else
+    {
+        tracesize=800;
+    }
+
+    if (gpsMapLine.size() > tracesize && PathPoint >= 0)
+    {
+        int aimIndex;
+        if(circleMode){
+            aimIndex=PathPoint+tracesize;
+        }else{
+            aimIndex=min((PathPoint+tracesize),(int)gpsMapLine.size());
+        }
+
+        for (int i = PathPoint; i < aimIndex; i++)
+        {
+            int index = i % gpsMapLine.size();
+            Point2D pt = Coordinate_Transfer((*gpsMapLine[index]).gps_x, (*gpsMapLine[index]).gps_y, now_gps_ins);
+            pt.x += offset_real * 0.032;
+            pt.v1 = (*gpsMapLine[index]).speed_mode;
+            pt.v2 = (*gpsMapLine[index]).mode2;
+            pt.roadMode=(*gpsMapLine[index]).roadMode;
+            if (gpsMapLine[index]->roadMode==3 && !hasZhuched)
+            {
+                readyZhucheMode = true;
+                DecideGps00::zhuchePointNum = index;
+            }
+
+            if (gpsMapLine[index]->roadMode==3 && !hasZhuched)
+            {
+                readyZhucheMode = true;
+                DecideGps00::zhuchePointNum = index;
+            }
+
+            //csvv7
+            if (gpsMapLine[index]->speed_mode==22 || gpsMapLine[index]->speed_mode==23)
+            {
+                readyParkMode = true;
+                DecideGps00::finishPointNum = index;
+            }
+            switch (pt.v1)
+            {
+            case 0:
+                pt.speed = 80;
+                break;
+            case 1:
+                pt.speed = 25;
+                break;
+            case 2:
+                pt.speed =25;
+                break;
+            case 3:
+                pt.speed = 20;
+                break;
+            case 4:
+                pt.speed =18;
+                break;
+            case 5:
+                pt.speed = 18;
+                break;
+            case 7:
+                pt.speed = 10;
+                break;
+            case 22:
+                pt.speed = 5;
+                break;
+            case 23:
+                pt.speed = 5;
+                break;
+            default:
+                break;
+            }
+            trace.push_back(pt);
+
+
+            double hdg=90 - (*gpsMapLine[index]).ins_heading_angle;
+            if(hdg < 0)  hdg = hdg + 360;
+            if(hdg >= 360) hdg = hdg - 360;
+
+            double xyhdg  = hdg/180.0*PI;
+            double ptLeft_x=(*gpsMapLine[index]).gps_x + ServiceCarStatus.msysparam.vehWidth / 2 * cos(xyhdg + PI / 2);
+            double ptLeft_y=(*gpsMapLine[index]).gps_y + ServiceCarStatus.msysparam.vehWidth / 2 * sin(xyhdg + PI / 2);
+            Point2D ptLeft = Coordinate_Transfer(ptLeft_x, ptLeft_y, now_gps_ins);
+
+            double ptRight_x=(*gpsMapLine[index]).gps_x + ServiceCarStatus.msysparam.vehWidth / 2 * cos(xyhdg - PI / 2);
+            double ptRight_y=(*gpsMapLine[index]).gps_y + ServiceCarStatus.msysparam.vehWidth / 2 * sin(xyhdg - PI / 2);
+            Point2D ptRight = Coordinate_Transfer(ptRight_x, ptRight_y, now_gps_ins);
+
+            traceOriLeft.push_back(ptLeft);
+            traceOriRight.push_back(ptRight);
+
+        }
+    }
+    return trace;
+}
+
+
+std::vector<iv::Point2D> iv::decition::DecideGps00::getGpsTraceAvoid(iv::GPS_INS now_gps_ins, const std::vector<Point2D> gpsTrace, int lastIndex, bool circleMode) {
+    vector<iv::Point2D> trace;
+
+    int index = -1;
+    float minDis = 10;
+
+    for (unsigned int i = 0; i < gpsTrace.size(); i++)
+    {
+        double tmpdis = sqrt((now_gps_ins.gps_x - gpsTrace[i].x) * (now_gps_ins.gps_x - gpsTrace[i].x) + (now_gps_ins.gps_y - gpsTrace[i].y) * (now_gps_ins.gps_y - gpsTrace[i].y));
+
+        if (tmpdis < minDis)
+        {
+            index = i;
+            minDis = tmpdis;
+        }
+    }
+
+    trace.clear();
+    if (index >= 0) {
+        for (unsigned int i = index; i < gpsTrace.size(); i++)
+        {
+            Point2D pt = Coordinate_Transfer(gpsTrace[i].x, gpsTrace[i].y, now_gps_ins);
+            trace.push_back(pt);
+        }
+    }
+    return trace;
+}
+
+
+
+std::vector<iv::Point2D>  iv::decition::DecideGps00::getGpsTraceOffset(std::vector<Point2D> gpsTrace, double offset) {
+
+    if (offset==0)
+    {
+        return gpsTrace;
+    }
+
+    std::vector<iv::Point2D> trace;
+    for (int j = 0; j < gpsTrace.size(); j++)
+    {
+        double sumx1 = 0, sumy1 = 0, count1 = 0;
+        double sumx2 = 0, sumy2 = 0, count2 = 0;
+        for (int k = max(0, j - 4); k <= j; k++)
+        {
+            count1 = count1 + 1;
+            sumx1 += gpsTrace[k].x;
+            sumy1 += gpsTrace[k].y;
+        }
+        for (int k = j; k <= min(gpsTrace.size() - 1, (size_t)(j + 4)); k++)
+        {
+            count2 = count2 + 1;
+            sumx2 += gpsTrace[k].x;
+            sumy2 += gpsTrace[k].y;
+        }
+        sumx1 /= count1; sumy1 /= count1;
+        sumx2 /= count2; sumy2 /= count2;
+
+        double anglevalue = atan2(sumy2 - sumy1, sumx2 - sumx1);
+
+        double carFrontx = gpsTrace[j].x;// -Form1.CarRear * Math.Cos(anglevalue);
+        double carFronty = gpsTrace[j].y;// -Form1.CarRear * Math.Sin(anglevalue);
+
+        double avoidLenth = abs(offset);
+        if (offset<0)
+        {
+            Point2D ptLeft(carFrontx + avoidLenth * cos(anglevalue + PI / 2),
+                           carFronty + avoidLenth  * sin(anglevalue + PI / 2));
+            ptLeft.speed = gpsTrace[j].speed;
+            ptLeft.v1 = gpsTrace[j].v1;
+            ptLeft.v2 = gpsTrace[j].v2;
+            trace.push_back(ptLeft);
+        }
+        else
+        {
+            Point2D ptRight(carFrontx + avoidLenth  * cos(anglevalue - PI / 2),
+                            carFronty + avoidLenth  * sin(anglevalue - PI / 2));
+            ptRight.speed = gpsTrace[j].speed;
+            ptRight.v1 = gpsTrace[j].v1;
+            ptRight.v2 = gpsTrace[j].v2;
+
+
+            trace.push_back(ptRight);
+        }
+    }
+
+    return trace;
+}
+
+void  iv::decition::DecideGps00::getGpsTraceNowLeftRight(std::vector<Point2D> gpsTrace) {
+    for (int j = 0; j < gpsTrace.size(); j++)
+    {
+        double sumx1 = 0, sumy1 = 0, count1 = 0;
+        double sumx2 = 0, sumy2 = 0, count2 = 0;
+        for (int k = max(0, j - 4); k <= j; k++)
+        {
+            count1 = count1 + 1;
+            sumx1 += gpsTrace[k].x;
+            sumy1 += gpsTrace[k].y;
+        }
+        for (unsigned int k = j; k <= min(gpsTrace.size() - 1, (size_t)(j + 4)); k++)
+        {
+            count2 = count2 + 1;
+            sumx2 += gpsTrace[k].x;
+            sumy2 += gpsTrace[k].y;
+        }
+        sumx1 /= count1; sumy1 /= count1;
+        sumx2 /= count2; sumy2 /= count2;
+
+        double anglevalue = atan2(sumy2 - sumy1, sumx2 - sumx1);
+
+        double carFrontx = gpsTrace[j].x;// -Form1.CarRear * Math.Cos(anglevalue);
+        double carFronty = gpsTrace[j].y;// -Form1.CarRear * Math.Sin(anglevalue);
+
+        Point2D ptLeft(carFrontx + ServiceCarStatus.msysparam.vehWidth / 2 * cos(anglevalue + PI / 2),
+                       carFronty + ServiceCarStatus.msysparam.vehWidth / 2 * sin(anglevalue + PI / 2));
+
+        Point2D ptRight(carFrontx + ServiceCarStatus.msysparam.vehWidth / 2 * cos(anglevalue - PI / 2),
+                        carFronty + ServiceCarStatus.msysparam.vehWidth / 2 * sin(anglevalue - PI / 2));
+
+        gpsTraceNowLeft.push_back(ptLeft);
+        gpsTraceNowRight.push_back(ptRight);
+    }
+
+}
+
+
+
+
+std::vector<iv::Point2D>  iv::decition::DecideGps00::getGpsTraceOffsetAvoid(std::vector<Point2D> gpsTrace, double offset,GPS_INS nowgps) {
+
+    if (offset==0)
+    {
+        return gpsTrace;
+    }
+
+    std::vector<iv::Point2D> trace;
+    std::vector<iv::Point2D> traceXY;
+    for (int j = 0; j < gpsTrace.size(); j++)
+    {
+        double sumx1 = 0, sumy1 = 0, count1 = 0;
+        double sumx2 = 0, sumy2 = 0, count2 = 0;
+        for (int k = max(0, j - 4); k <= j; k++)
+        {
+            count1 = count1 + 1;
+            sumx1 += gpsTrace[k].x;
+            sumy1 += gpsTrace[k].y;
+        }
+        for (int k = j; k <= min(gpsTrace.size() - 1, (size_t)(j + 4)); k++)
+        {
+            count2 = count2 + 1;
+            sumx2 += gpsTrace[k].x;
+            sumy2 += gpsTrace[k].y;
+        }
+        sumx1 /= count1; sumy1 /= count1;
+        sumx2 /= count2; sumy2 /= count2;
+
+        double anglevalue = atan2(sumy2 - sumy1, sumx2 - sumx1);
+
+        double carFrontx = gpsTrace[j].x;// -Form1.CarRear * Math.Cos(anglevalue);
+        double carFronty = gpsTrace[j].y;// -Form1.CarRear * Math.Sin(anglevalue);
+
+        double avoidLenth = abs(offset);
+        if (offset<0)
+        {
+            Point2D ptLeft(carFrontx + avoidLenth * cos(anglevalue + PI / 2),
+                           carFronty + avoidLenth  * sin(anglevalue + PI / 2));
+            ptLeft.speed = gpsTrace[j].speed;
+            ptLeft.v1 = gpsTrace[j].v1;
+            ptLeft.v2 = gpsTrace[j].v2;
+            trace.push_back(ptLeft);
+        }
+        else
+        {
+            Point2D ptRight(carFrontx + avoidLenth  * cos(anglevalue - PI / 2),
+                            carFronty + avoidLenth  * sin(anglevalue - PI / 2));
+            ptRight.speed = gpsTrace[j].speed;
+            ptRight.v1 = gpsTrace[j].v1;
+            ptRight.v2 = gpsTrace[j].v2;
+
+
+            trace.push_back(ptRight);
+        }
+    }
+
+
+    bool use_new_method = true;
+    if  (use_new_method)
+    {
+        const int val = 300;
+        if(trace.size()>val)
+        {
+            double V = trace[300].y;
+            for (int j = 0; j < val; j++)
+            {
+                double t = (double)j / val;
+                double s = t*t*(3.-2.*t);
+                double ox = s;
+                double oy = t *( V-3.0)+3.0;
+
+                trace[j].x=ox*trace[j].x;
+                trace[j].y=oy;
+            }
+        }
+    }
+
+    traceXY.clear();
+    for(int j=0;j<30;j++)
+    {
+        GPS_INS gpsxy = Coordinate_UnTransfer(gpsTrace[j].x,gpsTrace[j].y,nowgps);
+        Point2D gpsinxy(gpsxy.gps_x,gpsxy.gps_y);
+        traceXY.push_back(gpsinxy);
+    }
+    for(int j=0;j<trace.size();j++)
+    {
+        GPS_INS gpsxy = Coordinate_UnTransfer(trace[j].x,trace[j].y,nowgps);
+        Point2D gpsinxy(gpsxy.gps_x,gpsxy.gps_y);
+        traceXY.push_back(gpsinxy);
+    }
+    return traceXY;
+}
+
+std::vector<iv::Point2D>  iv::decition::DecideGps00::getGpsTraceOffsetBackOri(std::vector<Point2D> gpsTrace, double offset,GPS_INS nowgps,std::vector<Point2D> gpsTraceNowNow) {
+
+
+    std::vector<iv::Point2D> trace;
+    std::vector<iv::Point2D> traceXY;
+
+    if (offset==0)
+    {
+        trace.assign(gpsTrace.begin(), gpsTrace.end());
+    }
+    else
+    {
+        for (int j = 0; j < gpsTrace.size(); j++)
+        {
+            double sumx1 = 0, sumy1 = 0, count1 = 0;
+            double sumx2 = 0, sumy2 = 0, count2 = 0;
+            for (int k = max(0, j - 4); k <= j; k++)
+            {
+                count1 = count1 + 1;
+                sumx1 += gpsTrace[k].x;
+                sumy1 += gpsTrace[k].y;
+            }
+            for (int k = j; k <= min(gpsTrace.size() - 1, (size_t)(j + 4)); k++)
+            {
+                count2 = count2 + 1;
+                sumx2 += gpsTrace[k].x;
+                sumy2 += gpsTrace[k].y;
+            }
+            sumx1 /= count1; sumy1 /= count1;
+            sumx2 /= count2; sumy2 /= count2;
+
+            double anglevalue = atan2(sumy2 - sumy1, sumx2 - sumx1);
+
+            double carFrontx = gpsTrace[j].x;// -Form1.CarRear * Math.Cos(anglevalue);
+            double carFronty = gpsTrace[j].y;// -Form1.CarRear * Math.Sin(anglevalue);
+
+            double avoidLenth = abs(offset);
+            if (offset<0)
+            {
+                Point2D ptLeft(carFrontx + avoidLenth * cos(anglevalue + PI / 2),
+                               carFronty + avoidLenth  * sin(anglevalue + PI / 2));
+                ptLeft.speed = gpsTrace[j].speed;
+                ptLeft.v1 = gpsTrace[j].v1;
+                ptLeft.v2 = gpsTrace[j].v2;
+                trace.push_back(ptLeft);
+            }
+            else
+            {
+                Point2D ptRight(carFrontx + avoidLenth  * cos(anglevalue - PI / 2),
+                                carFronty + avoidLenth  * sin(anglevalue - PI / 2));
+                ptRight.speed = gpsTrace[j].speed;
+                ptRight.v1 = gpsTrace[j].v1;
+                ptRight.v2 = gpsTrace[j].v2;
+
+
+                trace.push_back(ptRight);
+            }
+        }
+    }
+
+
+    bool use_new_method = true;
+    if  (use_new_method)
+    {
+        const int val = 300;
+        if(trace.size()>val)
+        {
+            double V = trace[300].y;
+            for (int j = 0; j < val; j++)
+            {
+                double t = (double)j / val;
+                double s = t*t*(3.-2.*t);
+                double ox = s;
+                double oy = t *( V-3.0)+3.0;
+
+                trace[j].x=ox*trace[j].x;
+                trace[j].y=oy;
+            }
+        }
+    }
+
+    traceXY.clear();
+    for(int j=0;j<30;j++)
+    {
+        GPS_INS gpsxy = Coordinate_UnTransfer(gpsTraceNowNow[j].x,gpsTraceNowNow[j].y,nowgps);
+        Point2D gpsinxy(gpsxy.gps_x,gpsxy.gps_y);
+        traceXY.push_back(gpsinxy);
+    }
+    for(int j=0;j<trace.size();j++)
+    {
+        GPS_INS gpsxy = Coordinate_UnTransfer(trace[j].x,trace[j].y,nowgps);
+        Point2D gpsinxy(gpsxy.gps_x,gpsxy.gps_y);
+        traceXY.push_back(gpsinxy);
+    }
+    return traceXY;
+}
+
+
+
+double iv::decition::DecideGps00::getAngle(std::vector<Point2D> gpsTrace,GPS_INS gpsIns,iv::decition::Decition decition) {
+    double angle=0;
+    if ( abs(iv::decition::DecideGps00().minDis) < 20 && abs(iv::decition::DecideGps00().maxAngle) < 100)
+    {
+        //   angle = iv::decition::Compute00().getDecideAngle(gpsTrace, realspeed);
+        pid_Controller->getControlDecition(gpsIns, gpsTrace,  -1,  -1,  -1, true, false, &decition);
+        angle= decition->wheel_angle;
+    }
+    return angle;
+}
+
+double iv::decition::DecideGps00::getSpeed(std::vector<Point2D> gpsTrace) {
+    double speed=0;
+    int speedPoint = iv::decition::Compute00().getSpeedPointIndex(gpsTrace, max(10.0, realspeed));
+    speed = gpsTrace[speedPoint].speed;
+    for (int i = 0; i < speedPoint; i++) {
+        speed = min(speed, gpsTrace[i].speed);
+    }
+    return speed;
+}
+
+
+//void iv::decition::DecideGps00::getEsrObs(std::vector<iv::ObstacleBasic> obsRadars) {
+//
+//	if (!obsRadars.empty())
+//	{
+//		esrIndex = iv::decition::Compute00().getEsrIndex(gpsTrace, obsRadars);
+//
+//		if (esrIndex != -1)
+//		{
+//			 esrDistance = obsRadars[esrIndex].nomal_y;
+//
+//
+//
+//			obsSpeed = obsRadars[esrIndex].speed_y;
+//
+//		}
+//		else {
+//			esrDistance = -1;
+//		}
+//
+//	}
+//	else
+//	{
+//		esrIndex = -1;
+//		esrDistance = -1;
+//	}
+//	if (esrDistance < 0) {
+//		ODS("\n------------------>ESR障碍物距离:%f\n", esrDistance);
+//	}
+//	else {
+//		ODS("\n------------------>ESR障碍物距离:%f   ( %.05f , %.05f )\n", esrDistance, obsRadars[esrIndex].nomal_x, obsRadars[esrIndex].nomal_y);
+//	}
+//
+//	ODS("\n------------------>ESR障碍物速度:%f\n", obsSpeed);
+//}
+
+
+
+void iv::decition::DecideGps00::getEsrObsDistance(std::vector<Point2D> gpsTrace, int roadNum) {
+
+
+    int esrPathpoint;
+
+    esrIndex = iv::decition::Compute00().getEsrIndex(gpsTrace, roadNum, &esrPathpoint);
+
+    if (esrIndex != -1)
+    {
+        //优化
+        //        double distance = 0.0;
+        //        for(int i=0; i<esrPathpoint; ++i){
+        //            distance+=DirectDistance(gpsTrace[i].x,gpsTrace[i].y,gpsTrace[i+1].x,gpsTrace[i+1].y);
+        //        }
+        //        esrDistance = distance - Esr_Y_Offset;
+        //        if(esrDistance<=0){
+        //            esrDistance=1;
+        //        }
+
+
+        esrDistance = ServiceCarStatus.obs_radar[esrIndex].nomal_y;
+        obsSpeed = ServiceCarStatus.obs_radar[esrIndex].speed_y;
+
+    }
+    else {
+        esrDistance = -1;
+    }
+}
+
+
+
+
+void iv::decition::DecideGps00::getEsrObsDistanceAvoid() {
+
+    esrIndexAvoid = iv::decition::Compute00().getEsrIndexAvoid(gpsTraceAvoid);
+
+    if (esrIndexAvoid != -1)
+    {
+        esrDistanceAvoid = ServiceCarStatus.obs_radar[esrIndexAvoid].nomal_y;
+        obsSpeedAvoid = ServiceCarStatus.obs_radar[esrIndexAvoid].speed_y;
+
+    }
+    else {
+        esrDistanceAvoid = -1;
+    }
+
+    if (esrDistanceAvoid < 0) {
+        std::cout << "\nESR障碍物距离Avoid:%f\n" << esrDistanceAvoid << std::endl;
+    }
+    else {
+        std::cout << "\nESR障碍物距离Avoid:%f   %d:( %.05f , %.05f ,%.05f )\n" << esrDistanceAvoid << esrIndexAvoid << ServiceCarStatus.obs_radar[esrIndexAvoid].nomal_x << ServiceCarStatus.obs_radar[esrIndexAvoid].nomal_y << std::endl;
+    }
+
+    std::cout << "\nESR障碍物速度Avoid:%f\n" << obsSpeedAvoid <<std::endl;
+}
+
+
+
+
+double iv::decition::DecideGps00::limitAngle(double speed, double angle) {
+    double preAngle = angle;
+
+    if (speed > 15)
+    {
+        if (preAngle > 350)
+        {
+            preAngle = 350;
+        }
+        if (preAngle < -350)
+        {
+            preAngle = -350;
+        }
+    }
+    if (speed > 22)
+    {
+        if (preAngle > 200)
+        {
+            preAngle = 200;
+        }
+        if (preAngle < -200)
+        {
+            preAngle = -200;
+        }
+    }
+    if (speed > 25)
+    {
+        if (preAngle > 150)
+        {
+            preAngle = 150;
+        }
+        if (preAngle < -150)
+        {
+            preAngle = -150;
+        }
+    }
+    if (speed > 30)
+    {
+        if (preAngle > 70)
+        {
+            preAngle = 70;
+        }
+        if (preAngle < -70)
+        {
+            preAngle = -70;
+        }
+    }
+    if (speed > 45)                     //20
+    {
+        if (preAngle > 15)
+        {
+            preAngle = 15;
+        }
+        if (preAngle < -15)
+        {
+            preAngle = -15;
+        }
+    }
+    return preAngle;
+}
+
+
+
+double iv::decition::DecideGps00::limitSpeed(double angle, double speed)
+{
+    if (abs(angle) > 500 && speed > 8)
+        speed = 8;
+    else if (abs(angle) > 350 && speed > 14)
+        speed = 14;
+    else if (abs(angle) > 200 && speed > 21)
+        speed = 21;
+    else if (abs(angle) > 150 && speed > 24)
+        speed = 24;
+    else if (abs(angle) > 60 && speed > 29)
+        speed = 29;
+    else if (abs(angle) > 20 && speed > 34)
+        speed = 34;
+    return max(0.0, speed);
+}
+
+
+bool iv::decition::DecideGps00::checkAvoidEnable(int roadNum)
+{
+    if ((obsDisVector[roadNum] > 0 && obsDisVector[roadNum] < obsDisVector[roadNow] + ServiceCarStatus.msysparam.vehLenth)
+                                   || (obsDisVector[roadNum] > 0 && obsDisVector[roadNum] < 15))
+    {
+        return false;
+    }
+
+    if (roadNum - roadNow > 1)
+    {
+        for (int i = roadNow + 1; i < roadNum; i++)
+        {
+            if (obsDisVector[i] < 8 + (i-roadNow) * 2 && obsDisVector[i]>0)
+            {
+                return false;
+            }
+        }
+
+    }
+
+    else if (roadNow - roadNum > 1)
+    {
+        for (int i = roadNow-1; i > roadNum; i--)
+        {
+            if (obsDisVector[i] < 8 + (roadNow-i) * 2 && obsDisVector[i]>0)
+            {
+                return false;
+            }
+        }
+    }
+    return true;
+}
+
+
+
+bool iv::decition::DecideGps00::checkReturnEnable(double avoidX, iv::LidarGridPtr lidarGridPtr, int roadNum) {
+    //lsn
+    if (obsDisVector[roadNum]>0 && obsDisVector[roadNum]<20)
+    {
+        return false;
+    }
+    if ((obsDisVector[roadNum]>0 && obsDisVector[roadNum]<50 && obsDisVector[roadNum]< obsDisVector[roadNow]) ||
+            (obsDisVector[roadNum]>0 && obsDisVector[roadNum]<50 && obsDisVector[roadNow]<0))
+    {
+        return false;
+    }
+
+
+    if (roadNum - roadNow>1)
+    {
+        for (int i = roadNow + 1; i < roadNum; i++)
+        {
+            if (obsDisVector[i] < 8 + (i-roadNow) * 2 && obsDisVector[i]>0) {
+                return false;
+            }
+        }
+
+    }
+
+    else if (roadNow - roadNum>1)
+    {
+        for (int i = roadNow - 1; i >roadNum; i--)
+        {
+            if (obsDisVector[i] < 8 + (roadNow-i) * 2 && obsDisVector[i]>0) {
+                return false;
+            }
+        }
+    }
+    return true;
+}
+
+
+void iv::decition::DecideGps00::getObsAvoid(iv::LidarGridPtr lidarGridPtr) {
+
+
+
+    if (lidarGridPtr == NULL)
+    {
+        iv::decition::DecideGps00::lidarDistanceAvoid = iv::decition::DecideGps00::lastLidarDisAvoid;
+    }
+    else {
+        obsPointAvoid = Compute00().getLidarObsPointAvoid(gpsTraceAvoid, lidarGridPtr);
+        iv::decition::DecideGps00::lastLidarDisAvoid = iv::decition::DecideGps00::lidarDistanceAvoid;
+    }
+    std::cout << "\nLidarAvoid距离:%f\n" << iv::decition::DecideGps00::lidarDistanceAvoid << std::endl;
+    getEsrObsDistanceAvoid();
+
+    //lidarDistanceAvoid = -1;   //20200103 apollo_fu
+
+    if (esrDistanceAvoid>0 && iv::decition::DecideGps00::lidarDistanceAvoid > 0)
+    {
+        if (iv::decition::DecideGps00::lidarDistanceAvoid >= esrDistanceAvoid)
+        {
+            obsDistanceAvoid = esrDistanceAvoid;
+            obsSpeedAvoid = ServiceCarStatus.obs_radar[esrIndexAvoid].speed_y;
+        }
+        else if (!ServiceCarStatus.obs_radar.empty())
+        {
+            obsDistanceAvoid = iv::decition::DecideGps00::lidarDistanceAvoid;
+            obsSpeedAvoid = Compute00().getObsSpeed(obsPointAvoid, secSpeed);
+            std::cout << "\n计算obsSpeed:%f\n" << obsSpeedAvoid << std::endl;
+        }
+        else
+        {
+            obsDistanceAvoid = iv::decition::DecideGps00::lidarDistanceAvoid;
+            obsSpeedAvoid = obsSpeedAvoid = 0 - secSpeed;;
+            std::cout << "\n毫米波无数据，计算obsSpeed:%f\n" << obsSpeedAvoid << std::endl;
+        }
+    }
+    else if (esrDistanceAvoid>0)
+    {
+        obsDistanceAvoid = esrDistanceAvoid;
+        obsSpeedAvoid = ServiceCarStatus.obs_radar[esrIndexAvoid].speed_y;
+    }
+    else if (iv::decition::DecideGps00::lidarDistanceAvoid > 0)
+    {
+        obsDistanceAvoid = iv::decition::DecideGps00::lidarDistanceAvoid;
+        obsSpeedAvoid = Compute00().getObsSpeed(obsPointAvoid, secSpeed);
+        std::cout << "\n计算obsSpeed:%f\n" << obsSpeedAvoid << std::endl;
+    }
+    else {
+        obsDistanceAvoid = esrDistanceAvoid;
+        obsSpeedAvoid = 0 - secSpeed;
+    }
+
+    std::cout << "\n最终得出的obsDistanceAvoid:%f\n" << obsDistanceAvoid << std::endl;
+
+
+
+
+
+    if (iv::decition::DecideGps00::obsDistanceAvoid <0 && obsLostTimeAvoid<4)
+    {
+        iv::decition::DecideGps00::obsDistanceAvoid = iv::decition::DecideGps00::lastDistanceAvoid;
+        obsLostTimeAvoid++;
+    }
+    else
+    {
+        obsLostTimeAvoid = 0;
+        iv::decition::DecideGps00::lastDistanceAvoid = -1;
+    }
+
+
+
+
+    if (obsDistanceAvoid>0)
+    {
+        iv::decition::DecideGps00::lastDistanceAvoid = obsDistanceAvoid;
+    }
+
+    std::cout << "\nODSAvoid距离:%f\n" << iv::decition::DecideGps00::obsDistanceAvoid << std::endl;
+}
+
+void iv::decition::DecideGps00::init() {
+    for (int i = 0; i < roadSum; i++)
+    {
+        lastEsrIdVector.push_back(-1);
+        lastEsrCountVector.push_back(0);
+        GPS_INS gps_ins;
+        gps_ins.gps_x = 0;
+        gps_ins.gps_y = 0;
+        startAvoidGpsInsVector.push_back(gps_ins);
+        avoidMinDistanceVector.push_back(0);
+    }
+}
+
+
+
+#include <QTime>
+
+void iv::decition::DecideGps00::computeObsOnRoad(iv::LidarGridPtr lidarGridPtr, std::vector<Point2D> gpsTrace, int roadNum,
+                                                 const std::vector<GPSData> gpsMapLine, std::vector<iv::Perception::PerceptionOutput> lidar_per) {
+
+    //    QTime xTime;
+    //   xTime.start();
+    //    qDebug("time 0 is %d ",xTime.elapsed());
+    double obs,obsSd;
+
+    if (lidarGridPtr == NULL)
+    {
+        lidarDistance = lastLidarDis;
+        //	lidarDistance = lastlidarDistance;
+
+    }
+    else {
+        obsPoint = Compute00().getLidarObsPoint(gpsTrace, lidarGridPtr);
+        float lidarXiuZheng=0;
+        if(!ServiceCarStatus.useMobileEye){
+            lidarXiuZheng=0-ServiceCarStatus.msysparam.frontGpsXiuzheng;
+        }
+        lidarDistance = obsPoint.y + lidarXiuZheng;   //激光距离推到车头  gjw20191110 lidar
+        //    lidarDistance=-1;
+        if (lidarDistance<0)
+        {
+            lidarDistance = -1;
+        }
+        lastLidarDis = lidarDistance;
+    }
+
+    if(lidarDistance<0){
+        lidarDistance=500;
+    }
+
+    std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "激光雷达距离:" << lidarDistance << std::endl;
+
+    ServiceCarStatus.mLidarObs = lidarDistance;
+    obs = lidarDistance;
+    obsSd= obsPoint.obs_speed_y;
+
+    if(ServiceCarStatus.useLidarPerPredict){
+        double preDis= predictObsOnRoad(lidar_per, gpsTrace, 1.0);
+        if (preDis<obs){
+            obs = preDis;
+            if(abs(obs-preDis>0.5)){
+                obsSd = 0-realspeed;
+            }
+        }
+    }
+
+
+    if(roadNum==roadNow){
+        obsDistance=obs;
+        obsSpeed=obsSd;
+    }
+
+
+    if(obsDistance<500&&obsDistance>0){
+        std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "obsDistance<500:" << obsDistance << std::endl;
+    }
+    std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "最终得出的obsDistance:" << obsDistance << std::endl;
+
+
+    ServiceCarStatus.mObs = obsDistance;
+    if(ServiceCarStatus.mObs>100){
+        ServiceCarStatus.mObs =-1;
+    }
+
+    if (obsDistance>0)
+    {
+        lastDistance = obsDistance;
+    }
+
+    //lsn
+    if(obs<0){
+        obsDisVector[roadNum]=500;
+    }else{
+        obsDisVector[roadNum]=obs;
+    }
+
+    //   qDebug("time 3 is %d ",xTime.elapsed());
+
+}
+
+//void iv::decition::DecideGps00::computeObsOnRoadXY(iv::LidarGridPtr lidarGridPtr, std::vector<Point2D> gpsTrace,std::vector<Point2D> gpsTraceLeft,std::vector<Point2D> gpsTraceRight,int roadNum,
+//                                                 const std::vector<GPSData> gpsMapLine, std::vector<iv::Perception::PerceptionOutput> lidar_per) {
+void iv::decition::DecideGps00::computeObsOnRoadXY( iv::GPS_INS now_gps_ins,iv::LidarGridPtr lidarGridPtr, std::vector<Point2D> gpsTrace,std::vector<Point2D> gpsTraceLeft,std::vector<Point2D> gpsTraceRight,int roadNum,
+                                                    const std::vector<GPSData> gpsMapLine, std::vector<iv::Perception::PerceptionOutput> lidar_per) {
+
+    //    QTime xTime;
+    //   xTime.start();
+    //    qDebug("time 0 is %d ",xTime.elapsed());
+    double obs,obsSd;
+    double obsCarCoordinateX,obsCarCoordinateY;
+    GPS_INS obsGeodetic;
+    Point2D obsFrenet,obsFrenetMid;
+    double obsCarCoordinateDistance=-1;
+
+    if (lidarGridPtr == NULL)
+    {
+        lidarDistance = lastLidarDis;
+        //	lidarDistance = lastlidarDistance;
+
+    }
+    else {
+        //obsPoint = Compute00().getLidarObsPoint(gpsTrace, lidarGridPtr);
+
+        obsPoint = Compute00().getLidarObsPointXY(gpsTrace,gpsTraceLeft,gpsTraceRight,lidarGridPtr);
+        float lidarXiuZheng=0;
+        if(!ServiceCarStatus.useMobileEye){
+            lidarXiuZheng=0-ServiceCarStatus.msysparam.frontGpsXiuzheng;
+        }
+
+        obsCarCoordinateX=obsPoint.x;
+        obsCarCoordinateY=obsPoint.y;
+        obsGeodetic = Coordinate_UnTransfer(obsCarCoordinateX, obsCarCoordinateY, now_gps_ins);   //车体转大地
+        obsFrenetMid=cartesian_to_frenet1D(gpsMapLine,obsGeodetic.gps_x,obsGeodetic.gps_y);          //大地转frenet
+        iv::Point2D now_s_d=cartesian_to_frenet1D(gpsMapLine,now_gps_ins.gps_x,now_gps_ins.gps_y);
+        givlog->debug("decition_brain","road_num: %d,obsFrenetMid_s: %f,now_s: %f",roadNum,obsFrenetMid.s,now_s_d.s);
+        obsFrenet.s=obsFrenetMid.s-now_s_d.s;
+        lidarDistance = obsFrenet.s;//obsPoint.y + lidarXiuZheng;   //激光距离推到车头  gjw20191110 lidar
+        //    lidarDistance=-1;
+//        if(obsPoint.s < 0)
+//        {
+//           lidarDistance = -1;
+//        }
+        if (lidarDistance<0)
+        {
+            lidarDistance = -1;
+        }
+        lastLidarDis = lidarDistance;
+    }
+
+
+
+
+
+
+    if(lidarDistance<0){
+        lidarDistance=500;
+    }
+
+
+
+    std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "激光雷达距离:" << lidarDistance << std::endl;
+
+    ServiceCarStatus.mLidarObs = lidarDistance;
+    obs = lidarDistance;
+
+    obsSd= obsPoint.obs_speed_y;
+
+    if(ServiceCarStatus.useLidarPerPredict){
+        double preDis= predictObsOnRoad(lidar_per, gpsTrace, 1.0);
+        if (preDis<obs){
+            obs = preDis;
+            if(abs(obs-preDis>0.5)){
+                obsSd = 0-realspeed;
+            }
+        }
+    }
+
+
+    if(roadNum==roadNow){
+        obsDistance=obs;
+        obsSpeed=obsSd;
+    }
+
+    if(obsPoint.obs_type==2){
+        obsSpeed=-secSpeed;
+    }
+
+    if(obsDistance<500&&obsDistance>0){
+        std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "obsDistance<500:" << obsDistance << std::endl;
+    }
+    std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "最终得出的obsDistance:" << obsDistance << std::endl;
+
+
+    ServiceCarStatus.mObs = obsDistance;
+    if(ServiceCarStatus.mObs>100){
+        ServiceCarStatus.mObs =-1;
+    }
+
+    if (obsDistance>0)
+    {
+        lastDistance = obsDistance;
+    }
+
+    //lsn
+    if(obs<0){
+        obsDisVector[roadNum]=500;
+    }else{
+        obsDisVector[roadNum]=obs;
+    }
+
+    //   qDebug("time 3 is %d ",xTime.elapsed());
+
+}
+
+
+//1220
+void iv::decition::DecideGps00::computeRearObsOnRoad(iv::LidarGridPtr lidarGridPtr, std::vector<Point2D> gpsTrace, int roadNum,
+                                                     const std::vector<GPSData> gpsMapLine) {
+
+    double obs,obsSd;
+
+
+    if (lidarGridPtr == NULL)
+    {
+        lidarDistance = lastLidarDis;
+        //	lidarDistance = lastlidarDistance;
+
+    }
+    else {
+        obsPoint = Compute00().getLidarRearObsPoint(gpsTrace, lidarGridPtr);
+        float lidarXiuZheng=0;
+        if(!ServiceCarStatus.useMobileEye){
+            lidarXiuZheng = ServiceCarStatus.msysparam.rearGpsXiuzheng;
+        }
+        if(abs(obsPoint.y)>lidarXiuZheng)
+            lidarDistance = abs(obsPoint.y)-lidarXiuZheng;   //激光距离推到车头 1220
+
+        //    lidarDistance=-1;
+        if (lidarDistance<0)
+        {
+            lidarDistance = -1;
+        }
+        lastLidarDis = lidarDistance;
+    }
+
+
+
+    if(lidarDistance<0){
+        lidarDistance=500;
+    }
+
+
+    std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "激光雷达距离:" << lidarDistance << std::endl;
+
+    if(lidarDistance==500){
+        lidarDistance=-1;
+    }
+
+
+
+
+    ServiceCarStatus.mLidarObs = lidarDistance;
+
+    obs=lidarDistance;
+    //	obsSpeed = 0 - secSpeed;
+    obsSd = 0 -secSpeed;
+
+
+
+
+    if(roadNum==roadNow){
+        obsDistance=obs;
+        obsSpeed=obsSd;
+    }
+
+
+    if (obsDistance <0 && obsLostTime<4)
+    {
+        obsDistance = lastDistance;
+        obsLostTime++;
+    }
+    else
+    {
+        obsLostTime = 0;
+        lastDistance = -1;
+    }
+
+
+    std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "最终得出的obsDistance:" << obsDistance << std::endl;
+
+    ServiceCarStatus.mObs = obsDistance;
+    if(ServiceCarStatus.mObs>100){
+        ServiceCarStatus.mObs =-1;
+    }
+
+    if (obsDistance>0)
+    {
+        lastDistance = obsDistance;
+    }
+
+    //lsn
+    if(obs<0){
+        obsDisVector[roadNum]=500;
+    }else{
+        obsDisVector[roadNum]=obs;
+    }
+
+}
+
+
+
+double iv::decition::DecideGps00::predictObsOnRoad(std::vector<iv::Perception::PerceptionOutput> lidar_per,std::vector<Point2D> gpsTrace,double realSpeed){
+    double preObsDis=500;
+
+    if(!lidar_per.empty()){
+        preObsDis=PredictObsDistance(  realSpeed,  gpsTrace, lidar_per);
+        lastPreObsDistance=preObsDis;
+
+    }else{
+        preObsDis=lastPreObsDistance;
+    }
+
+    ServiceCarStatus.mfttc = preObsDis;
+    return preObsDis;
+    //    if(preObsDis<obsDistance){
+    //        obsDistance=preObsDis;
+    //        lastDistance=obsDistance;
+    //    }
+}
+
+int iv::decition::DecideGps00::chooseAvoidRoad(iv::LidarGridPtr lidarGridPtr, GPS_INS now_gps_ins, const std::vector<GPSData> gpsMapLine,std::vector<iv::Perception::PerceptionOutput> lidar_per) {
+    roadPre = -1;
+
+    for (int i =  0; i < roadSum; i++)
+    {
+        gpsTraceAvoid.clear();
+        avoidX=computeAvoidX(i,roadOri,gpsMapLine[PathPoint]);
+        gpsTraceAvoid = getGpsTraceOffset(gpsTraceOri, avoidX);
+        computeObsOnRoad(lidarGridPtr, gpsTraceAvoid, i,gpsMapLine,lidar_per);
+    }
+
+    if (lidarGridPtr!=NULL)
+    {
+        hasCheckedAvoidLidar = true;
+    }
+
+    for(int i=0; i<roadSum;i++){
+        std::cout<<"\odsjuli====================:\n"<<i<<"chedaojuli:"<<obsDisVector[i]<<endl;
+
+    }
+
+    checkAvoidObsTimes++;
+    if (checkAvoidObsTimes<4 || hasCheckedAvoidLidar==false)
+    {
+        return - 1;
+    }
+
+
+    for (int i = 1; i < roadSum; i++)
+    {
+        if (roadNow + i < roadSum) {
+            //   avoidX = (roadOri-roadNow-i)*roadWidth;
+            avoidX=computeAvoidX(roadNow+i,roadOri,gpsMapLine[PathPoint]);
+            if (checkAvoidEnable(roadNow+i))
+            {
+                /*if (roadNow==roadOri)
+                {
+                    avoidRunDistance = obsDisVector[roadNow] + ServiceCarStatus.msysparam.vehLenth;
+                    startAvoid_gps_ins = now_gps_ins;
+                }	*/
+                avoidMinDistanceVector[roadNow]= obsDisVector[roadNow] + 2*ServiceCarStatus.msysparam.vehLenth;
+                startAvoidGpsInsVector[roadNow] = now_gps_ins;
+                roadPre = roadNow + i;
+                return roadPre;
+            }
+        }
+
+        if (roadNow - i >= 0)
+        {
+            //    avoidX = (roadOri - roadNow+i)*roadWidth;
+            avoidX=computeAvoidX(roadNow-i,roadOri,gpsMapLine[PathPoint]);
+            if (checkAvoidEnable(roadNow - i))
+            {
+                /*if (roadNow == roadOri)
+                {
+                    avoidRunDistance = obsDisVector[roadNow] + ServiceCarStatus.msysparam.vehLenth;
+                    startAvoid_gps_ins = now_gps_ins;
+                }*/
+                avoidMinDistanceVector[roadNow] = obsDisVector[roadNow] + 2*ServiceCarStatus.msysparam.vehLenth;
+                startAvoidGpsInsVector[roadNow] = now_gps_ins;
+                roadPre = roadNow - i;
+                return roadPre;
+            }
+        }
+
+    }
+    return roadPre;
+}
+
+int iv::decition::DecideGps00::chooseBackRoad(iv::LidarGridPtr lidarGridPtr, GPS_INS now_gps_ins, std::vector<GPSData> gpsMapLine,std::vector<iv::Perception::PerceptionOutput> lidar_per)
+{
+    roadPre = -1;
+
+    for (int i =  0; i <roadSum; i++)
+    {
+        gpsTraceBack.clear();
+        avoidX=computeAvoidX(i,roadOri,gpsMapLine[PathPoint]);
+        gpsTraceBack = getGpsTraceOffset(gpsTraceOri, avoidX);
+        computeObsOnRoad(lidarGridPtr, gpsTraceBack, i,gpsMapLine,lidar_per);
+    }
+
+    if (lidarGridPtr != NULL)
+    {
+        hasCheckedBackLidar = true;
+    }
+
+    checkBackObsTimes++;
+    if (checkBackObsTimes<4 || hasCheckedBackLidar == false)
+    {
+        return -1;
+    }
+
+    if ((GetDistance(now_gps_ins, startAvoidGpsInsVector[roadOri])>max(avoidMinDistanceVector[roadOri],40.0)) &&
+            (checkReturnEnable(avoidX, lidarGridPtr, roadOri)))
+    {
+        roadPre = roadOri;
+        return roadPre;
+    }
+
+    if (roadNow-roadOri>1)
+    {
+        for (int i = roadOri + 1;i < roadNow;i++) {
+            if (checkReturnEnable(avoidX, lidarGridPtr, i)&&
+                    (GetDistance(now_gps_ins, startAvoidGpsInsVector[i])>  avoidMinDistanceVector[i])&&
+                    (GetDistance(now_gps_ins, startAvoidGpsInsVector[i])> 30.0))
+            {
+                roadPre = i;
+                return roadPre;
+            }
+        }
+    }
+
+    else if (roadNow <roadOri-1)
+    {
+        for (int i = roadOri - 1;i > roadNow;i--) {
+            if (checkReturnEnable(avoidX, lidarGridPtr, i)&&
+                    (GetDistance(now_gps_ins, startAvoidGpsInsVector[i])>avoidMinDistanceVector[i])&&
+                    (GetDistance(now_gps_ins, startAvoidGpsInsVector[i])>  30.0))
+            {
+                roadPre = i;
+                return roadPre;
+            }
+        }
+    }
+
+    return roadPre;
+}
+
+
+double iv::decition::DecideGps00::trumpet() {
+    if (trumpetFirstCount)
+    {
+        trumpetFirstCount = false;
+        trumpetLastTime= GetTickCount();
+        trumpetTimeSpan = 0.0;
+    }
+    else
+    {
+        trumpetStartTime= GetTickCount();
+        trumpetTimeSpan += trumpetStartTime - trumpetLastTime;
+        trumpetLastTime = trumpetStartTime;
+    }
+
+    return trumpetTimeSpan;
+}
+
+
+
+
+double iv::decition::DecideGps00::transferP() {
+    if (transferFirstCount)
+    {
+        transferFirstCount = false;
+        transferLastTime= GetTickCount();
+        transferTimeSpan = 0.0;
+    }
+    else
+    {
+        transferStartTime= GetTickCount();
+        transferTimeSpan += transferStartTime - transferLastTime;
+        transferLastTime = transferStartTime;
+    }
+
+    return transferTimeSpan;
+}
+
+
+
+void  iv::decition::DecideGps00::handBrakePark(iv::decition::Decition  decition, long duringTime, GPS_INS now_gps_ins)
+{
+    if (abs(now_gps_ins.speed)>0.1)
+    {
+        decition->accelerator = 0;
+        decition->brake = 20;
+        decition->wheel_angle = 0;
+
+    }
+    else
+    {
+        decition->accelerator = 0;
+        decition->brake = 20;
+        decition->wheel_angle = 0;
+        handPark = true;
+        handParkTime = duringTime;
+    }
+}
+
+
+
+
+void iv::decition::DecideGps00::getMapBeside(std::vector<iv::GPSData> navigation_data, iv::GPS_INS now_gps_ins)
+{
+    gmapsL.clear();
+    gmapsR.clear();
+    for (int i = 0; i < 31; i++)
+    {
+        std::vector<iv::GPSData> gpsMapLineBeside;
+        //	gpsMapLineBeside = iv::decition::ComputeUnit().getBesideGpsMapLine(*ServiceCarStatus.location, navigation_data, -0.5*i);
+        gpsMapLineBeside = iv::decition::Compute00().getBesideGpsMapLine(now_gps_ins, navigation_data, -0.5*i);
+        gmapsL.push_back(gpsMapLineBeside);
+    }
+
+    for (int i = 0; i < 31; i++)
+    {
+        std::vector<iv::GPSData> gpsMapLineBeside;
+        //		gpsMapLineBeside = iv::decition::ComputeUnit().getBesideGpsMapLine(*ServiceCarStatus.location, navigation_data, 0.5*i);
+        gpsMapLineBeside = iv::decition::Compute00().getBesideGpsMapLine(now_gps_ins, navigation_data, 0.5*i);
+        gmapsR.push_back(gpsMapLineBeside);
+    }
+}
+
+
+bool iv::decition::DecideGps00::checkChaoCheBack(iv::LidarGridPtr lidarGridPtr)
+{
+    if (lidarGridPtr == NULL)
+    {
+        return false;
+        //	lidarDistance = lastlidarDistance;
+    }
+    else
+    {
+        obsPoint = Compute00().getLidarObsPoint(gpsTraceOri, lidarGridPtr);
+        double lidarDistance = obsPoint.y - 2.5;   //激光距离推到车头
+        //     ODS("\n超车雷达距离:%f\n", lidarDistance);
+        if (lidarDistance >-20 && lidarDistance<35)
+        {
+            checkChaoCheBackCounts = 0;
+            return false;
+        }
+        else
+        {
+            checkChaoCheBackCounts++;
+        }
+        if (checkChaoCheBackCounts>2)
+        {
+            checkChaoCheBackCounts = 0;
+            return true;
+        }
+    }
+    return false;
+}
+
+void iv::decition::DecideGps00::updateGroupDate(GPS_INS now_gps_ins,float realspeed,float theta,float s)
+{
+    Point2D pt = Coordinate_Transfer( now_gps_ins.gps_x, now_gps_ins.gps_y, group_ori_gps);  //大地转车体
+
+    ServiceCarStatus.group_x_local=pt.x;
+    //  ServiceCarStatus.group_y_local=pt.y;
+    ServiceCarStatus.group_y_local=s;
+    if(realspeed < 0.36)
+    {
+        ServiceCarStatus.group_velx_local = 0;
+        ServiceCarStatus.group_vely_local = 0;
+    }
+    else
+    {
+        ServiceCarStatus.group_velx_local = realspeed * sin(theta) / 3.6;
+        ServiceCarStatus.group_vely_local = realspeed * cos(theta) / 3.6;
+    }
+
+    ServiceCarStatus.group_pathpoint = PathPoint;
+}
+
+
+
+float  iv::decition::DecideGps00::ComputeTrafficLightSpeed(int traffic_light_color, const std::vector<GPSData> gpsMapLine,
+                                                           int traffic_light_pathpoint, int pathpoint,float secSpeed)
+{
+    float traffic_speed = 200;
+    float traffic_dis = 0;
+
+    if(abs(secSpeed)<0.1)
+    {
+        secSpeed=0;
+    }
+
+    if(pathpoint <= traffic_light_pathpoint)
+    {
+        for(int i=pathpoint;i<traffic_light_pathpoint;i++)
+        {
+            traffic_dis +=GetDistance(*gpsMapLine[i],*gpsMapLine[i+1]);
+        }
+    }
+    else
+    {
+        for(int i = pathpoint; i < gpsMapLine.size()-1; i++)
+        {
+            traffic_dis += GetDistance(*gpsMapLine[i],*gpsMapLine[i+1]);
+        }
+        for(int i = 0; i < traffic_light_pathpoint; i++)
+        {
+            traffic_dis += GetDistance(*gpsMapLine[i],*gpsMapLine[i+1]);
+        }
+    }
+
+    if(traffic_light_color==0 && traffic_dis<10)
+    {
+        traffic_speed=0;
+    }
+
+    return  traffic_speed;
+}
+
+float  iv::decition::DecideGps00:: ComputeTrafficLightSpeed(int traffic_light_color, int traffic_light_time,  const std::vector<GPSData> gpsMapLine,int traffic_light_pathpoint,
+                                                           int pathpoint,float secSpeed,float dSpeed){
+    float traffic_speed=200;
+    float traffic_dis=0;
+    float passTime;
+    float passSpeed;
+    bool passEnable=false;
+
+    if(abs(secSpeed)<0.1){
+        secSpeed=0;
+    }
+
+
+
+    if(pathpoint <= traffic_light_pathpoint){
+        for(int i=pathpoint;i<traffic_light_pathpoint;i++){
+            traffic_dis +=GetDistance(*gpsMapLine[i],*gpsMapLine[i+1]);
+        }
+    }else{
+        for(int i=pathpoint;i<gpsMapLine.size()-1;i++){
+            traffic_dis +=GetDistance(*gpsMapLine[i],*gpsMapLine[i+1]);
+        }
+        for(int i=0;i<traffic_light_pathpoint;i++){
+            traffic_dis+=GetDistance(*gpsMapLine[i],*gpsMapLine[i+1]);
+        }
+    }
+
+    //    if(traffic_light_color != 0)
+    //    {
+    //        int a = 3;
+    //    }
+
+
+//    if(traffic_light_color==0 && traffic_dis<10){
+//        traffic_speed=0;
+//    }
+
+    if(traffic_light_color==2 && traffic_dis<10){  //cxw,1xhonglvdeng gaibian
+        traffic_speed=0;
+//        return  traffic_speed;
+//        if(traffic_dis>3.0)
+//        {
+//            traffic_speed=sqrt(traffic_dis*2.0*9.8*0.5);
+//        }
+//        else
+//        {
+//           //minDecelerate=-0.7;
+//           traffic_speed=0;
+//        }
+        return  traffic_speed;
+    }
+    //    else   //20200108
+    //    {
+    //        traffic_speed=10;
+    //    }
+//    return  traffic_speed;
+
+    passSpeed = min((float)(dSpeed/3.6),secSpeed);
+    passTime = traffic_dis/(dSpeed/3.6);
+
+
+
+//1+x V2R    //1:绿灯 2：红灯 3：黄灯
+    //最开始0x0: 红色 0x1: 黄色 0x2: 绿色
+
+    switch(traffic_light_color){
+    case 2://case 0: //for 1+x修改
+        if(passTime>traffic_light_time+1 && traffic_dis>10){
+            passEnable=true;
+        }else{
+            passEnable=false;
+        }
+        break;
+   case 3:// case 1:
+        if(passTime<traffic_light_time-1 && traffic_dis<10){
+            passEnable=true;
+        }else{
+            passEnable = false;
+        }
+        break;
+    case 1://case 2:
+        if(passTime<traffic_light_time){
+            passEnable= true;
+        }else{
+            passEnable=false;
+        }
+        break;
+
+
+    default:
+        passEnable=true; //20230413
+        break;
+    }
+
+    if(!passEnable){
+        if(traffic_dis<5){
+            traffic_speed=0;
+        }else if(traffic_dis<10){
+            traffic_speed=5;
+        }else if(traffic_dis<20){
+            traffic_speed=15;
+        }else if(traffic_dis<30){
+            traffic_speed=25;
+        }else if(traffic_dis<50){
+            traffic_speed=30;
+        }
+    }
+    return traffic_speed;
+
+}
+void iv::decition::DecideGps00::computeObsOnRoadByFrenet(iv::LidarGridPtr lidarGridPtr, const std::vector<Point2D>& gpsTrace, double & obs,
+                                                         const std::vector<iv::GPSData>& gpsMap,int pathpoint,GPS_INS nowGps)
+{
+    //    Point2D obsCombinePoint = Point2D(-1,-1);
+    iv::decition::FrenetPoint car_now_frenet_point = iv::decition::FrenetPlanner::getFrenetfromXY(0,0,gpsTrace,gpsMap,pathpoint,nowGps);
+    double obsSd;
+    if (lidarGridPtr == NULL)
+    {
+        lidarDistance = lastLidarDis;
+        //	lidarDistance = lastlidarDistance;
+    }
+    else
+    {
+        obsPoint = Compute00().getLidarObsPoint(gpsTrace, lidarGridPtr);
+        //        lidarDistance = obsPoint.y-2.5;   //激光距离推到车头
+        iv::decition::FrenetPoint lidarFPointTmp = iv::decition::FrenetPlanner::getFrenetfromXY(this->obsPoint.x,this->obsPoint.y,gpsTrace,gpsMap,pathpoint,nowGps);
+        lidarDistance = lidarFPointTmp.s - car_now_frenet_point.s - 2.5;//激光距离推到车头 此处的参数要不要改改！！！！ 2022 apollo_fu
+
+        //    lidarDistance=-1;
+        if (lidarDistance < 0)
+        {
+            lidarDistance = -1;
+        }
+        lastLidarDis = lidarDistance;
+    }
+
+    FrenetPoint esr_obs_frenet_point;
+    getEsrObsDistanceByFrenet(gpsTrace, car_now_frenet_point,esr_obs_frenet_point,gpsMap,pathpoint,nowGps);
+
+    if(lidarDistance < 0){
+        lidarDistance = 500;
+    }
+    if(esrDistance < 0){
+        esrDistance = 500;
+    }
+
+    std::cout << ServiceCarStatus.mRunTime.elapsed() << "ms:" << "激光雷达距离:" << lidarDistance << std::endl;
+    std::cout << ServiceCarStatus.mRunTime.elapsed() << "ms:" << "毫米波距离:" << esrDistance << std::endl;
+    myesrDistance = esrDistance;
+
+    if(lidarDistance == 500)
+    {
+        lidarDistance = -1;
+    }
+    if(esrDistance == 500)
+    {
+        esrDistance = -1;
+    }
+
+    ServiceCarStatus.mRadarObs = esrDistance;
+    ServiceCarStatus.mLidarObs = lidarDistance;
+
+    if (esrDistance>0 && lidarDistance > 0)
+    {
+        if (lidarDistance >= esrDistance)
+        {
+            obs = esrDistance;
+            //            obsSd=ServiceCarStatus.obs_radar[esrIndex].speed_y;
+            obsSd = obsSpeed;
+            //障碍物信息使用毫米波采集的。其位置坐标加上偏移量，转换成相对于惯导的位置，即转换成车辆坐标系下的位置坐标。
+            //            obsCombinePoint = Point2D(ServiceCarStatus.obs_radar[esrIndex].nomal_x+Esr_Offset,ServiceCarStatus.obs_radar[esrIndex].nomal_y+Esr_Y_Offset);
+        }
+        else if (!ServiceCarStatus.obs_radar.empty())
+        {
+            obs = lidarDistance;
+            //            obsCombinePoint = obsPoint;
+            //            obsSd = Compute00().getObsSpeedByFrenet(obsPoint, secSpeed,gpsTrace);
+            obsSd = Compute00().getObsSpeedByFrenet(obsPoint, secSpeed,gpsTrace,gpsMap,pathpoint,nowGps);
+            std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "计算obsSpeed:" << obsSpeed << std::endl;
+        }
+        else
+        {
+            obs=lidarDistance;
+            //            obsCombinePoint = obsPoint;
+            obsSd = 0 -secSpeed * cos(car_now_frenet_point.tangent_Ang-PI/2);
+            std::cout << ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "毫米波无数据，计算obsSpeed:" << obsSpeed << std::endl;
+        }
+    }
+    else if (esrDistance>0)
+    {
+        obs = esrDistance;
+        //        obsSd = ServiceCarStatus.obs_radar[esrIndex].speed_y;
+        obsSd = obsSpeed;
+        //障碍物信息使用毫米波采集的。其位置坐标加上偏移量，转换成相对于惯导的位置，即转换成车辆坐标系下的位置坐标。
+        //        obsCombinePoint = Point2D(ServiceCarStatus.obs_radar[esrIndex].nomal_x+Esr_Offset,ServiceCarStatus.obs_radar[esrIndex].nomal_y+Esr_Y_Offset);
+    }
+    else if (lidarDistance > 0)
+    {
+        obs = lidarDistance;
+        //        obsCombinePoint = obsPoint;
+        obsSd = Compute00().getObsSpeedByFrenet(obsPoint, secSpeed,gpsTrace,gpsMap,pathpoint,nowGps);
+        std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "计算obsSpeed:" << obsSpeed << std::endl;
+    }
+    else
+    {
+        obs = esrDistance;
+        //        obsCombinePoint = Point2D(ServiceCarStatus.obs_radar[esrIndex].nomal_x+Esr_Offset,ServiceCarStatus.obs_radar[esrIndex].nomal_y+Esr_Y_Offset);
+        obsSd = 0 - secSpeed*cos(car_now_frenet_point.tangent_Ang-PI/2);
+    }
+
+    obsDistance = obs;
+    obsSpeed = obsSd;
+
+    std::cout << ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "最终得出的obsDistance:" << obsDistance << std::endl;
+
+    ServiceCarStatus.mObs = obsDistance;
+    if(ServiceCarStatus.mObs>100)
+    {
+        ServiceCarStatus.mObs =-1;
+    }
+
+    if (obsDistance>0)
+    {
+        lastDistance = obsDistance;
+    }
+
+    if(obs<0)
+    {
+        obsDistance=500;
+    }
+    else
+    {
+        obsDistance=obs;
+    }
+}
+
+void iv::decition::DecideGps00::getRearEsrObsDistance(std::vector<Point2D> gpsTrace, int roadNum)
+{
+    esrIndex = iv::decition::Compute00().getRearEsrIndex(gpsTrace, roadNum);
+
+    if (esrIndex != -1)
+    {
+        esrDistance = ServiceCarStatus.obs_rear_radar[esrIndex].nomal_y;
+        obsSpeed = ServiceCarStatus.obs_rear_radar[esrIndex].speed_y;
+    }
+    else
+    {
+        esrDistance = -1;
+    }
+}
+
+void iv::decition::DecideGps00::getEsrObsDistanceByFrenet(const std::vector<Point2D>& gpsTrace, const FrenetPoint car_now_frenet_point,
+                                                          FrenetPoint &esrObs_F_Point, const std::vector<iv::GPSData>& gpsMap,int pathpoint,GPS_INS nowGps)
+{
+    esrIndex = iv::decition::Compute00().getEsrIndexByFrenet(gpsTrace,esrObs_F_Point,gpsMap,pathpoint,nowGps);
+    if (esrIndex != -1)
+    {
+        //障碍物的距离，是障碍物和车辆沿道路上的距离，即s。并不直接是它的x坐标或y坐标或两点直线坐标。
+        //严格来说应是 esrDistance = 障碍物的s - 车辆的s - Esr_Y_Offset，但这是根据车辆位置实时搜索路径，故车辆的s极其小。
+        esrDistance = esrObs_F_Point.s - car_now_frenet_point.s - Esr_Y_Offset;  //-Esr_Y_Offset是把距离推到车头，换算frenet坐标时，把它加上了的。故需减去。
+
+        double speedx = ServiceCarStatus.obs_radar[esrIndex].speed_x;  //障碍物相对于车辆x轴的速度
+        double speedy = ServiceCarStatus.obs_radar[esrIndex].speed_y;  //障碍物相对于车辆y轴的速度
+        double speed_combine = sqrt(speedx * speedx + speedy * speedy);    //将x、y轴两个方向的速度求矢量和
+        //障碍物的速度方向与道路方向的夹角。用于将速度分解到s方向和d方向。
+        //所谓道路方向是指，道路上离障碍物的最近的点沿道路弧线的切线方向。
+        double Etheta = esrObs_F_Point.tangent_Ang - atan2(speedy,speedx);
+
+        obsSpeed = speed_combine * cos(Etheta);  //由speed_combine分解的s轴方向上的速度
+    }
+    else
+    {
+        esrDistance = -1;
+    }
+}
+
+void iv::decition::DecideGps00::getV2XTrafficPositionVector(const std::vector<GPSData> gpsMapLine)
+{
+    v2xTrafficVector.clear();
+    for (int var = 0; var < gpsMapLine.size(); var++)
+    {
+        if(gpsMapLine[var]->roadMode == 6 || gpsMapLine[var]->mode2 == 1000001)
+        {
+            v2xTrafficVector.push_back(var);
+        }
+    }
+}
+
+float iv::decition::DecideGps00::ComputeV2XTrafficLightSpeed(iv::TrafficLight trafficLight, const std::vector<GPSData> gpsMapLine,std::vector<int> v2xTrafficVector,
+                                                             int pathpoint,float secSpeed,float dSpeed, bool circleMode)
+{
+    float trafficSpeed = 200;
+    int nearTraffixPoint = -1;
+    float nearTrafficDis = 0;
+    int traffic_color = 0;
+    int traffic_time = 0;
+    bool passThrough = false;
+    float dSecSpeed = dSpeed / 3.6;
+
+    if(v2xTrafficVector.empty())
+    {
+        return trafficSpeed;
+    }
+    if(!circleMode)
+    {
+        if(pathpoint > v2xTrafficVector.back())
+        {
+            return trafficSpeed;
+        }
+        else
+        {
+            for(int i = 0; i < v2xTrafficVector.size();i++)
+            {
+                if (pathpoint <= v2xTrafficVector[i])
+                {
+                    nearTraffixPoint = v2xTrafficVector[i];
+                    break;
+                }
+            }
+        }
+    }
+    else if(circleMode)
+    {
+        if(pathpoint > v2xTrafficVector.back())
+        {
+            nearTraffixPoint = v2xTrafficVector[0];
+        }
+        else
+        {
+            for(int i=0; i < v2xTrafficVector.size();i++)
+            {
+                if (pathpoint <= v2xTrafficVector[i])
+                {
+                    nearTraffixPoint = v2xTrafficVector[i];
+                    break;
+                }
+            }
+        }
+    }
+
+    if(nearTraffixPoint != -1)
+    {
+        for(int i = pathpoint;i < nearTraffixPoint;i++)
+        {
+            nearTrafficDis += GetDistance(*gpsMapLine[i],*gpsMapLine[i+1]);
+        }
+    }
+
+    if(nearTrafficDis>50)
+    {
+        return trafficSpeed;
+    }
+
+    int roadMode = gpsMapLine[pathpoint]->roadMode;
+    if(roadMode == 14 || roadMode == 16)
+    {
+        traffic_color = trafficLight.leftColor;
+        traffic_time = trafficLight.leftTime;
+    }
+    else if(roadMode == 15 ||roadMode == 17)
+    {
+        traffic_color = trafficLight.rightColor;
+        traffic_time = trafficLight.rightTime;
+    }
+    else
+    {
+        traffic_color = trafficLight.straightColor;
+        traffic_time = trafficLight.straightTime;
+    }
+
+//    passThrough = computeTrafficPass(nearTrafficDis,traffic_color,traffic_time,dSecSpeed);
+//    if(passThrough)
+//    {
+//        return trafficSpeed;
+//    }
+//    else
+//    {
+//        trafficSpeed = computeTrafficSpeedLimt(nearTrafficDis);
+//        if(nearTrafficDis < 6)
+//        {
+//            float decelerate = 0 - ( secSpeed * secSpeed * 0.5 / nearTrafficDis);
+//            minDecelerate = min(minDecelerate,decelerate);
+//        }
+//        return trafficSpeed;
+//    }
+    double fTrafficBrake = 1.0;  //移植吉利项目的v2x
+    passThrough=computeTrafficPass(nearTrafficDis,traffic_color,traffic_time,secSpeed,dSecSpeed);
+    if(passThrough){
+        return trafficSpeed;
+    }else{
+        trafficSpeed=computeTrafficSpeedLimt(nearTrafficDis);
+        if(nearTrafficDis<(dSecSpeed*dSecSpeed/(2*fTrafficBrake))){
+            if(fabs(nearTrafficDis)<1.0)
+            {
+                minDecelerate = -1.0 * fTrafficBrake;
+            }
+            else
+            {
+                if((0.5*secSpeed*secSpeed/fTrafficBrake) > (nearTrafficDis*0.9))
+                {
+                    float decelerate =0-( secSpeed*secSpeed*0.5/(nearTrafficDis-0.5));
+                    minDecelerate=min(minDecelerate,decelerate);
+                }
+            }
+        }
+        return trafficSpeed;
+    }
+
+    return trafficSpeed;
+}
+
+//bool iv::decition::DecideGps00::computeTrafficPass(float trafficDis,int trafficColor,float trafficTime,float dSecSpeed)
+bool iv::decition::DecideGps00::computeTrafficPass(float trafficDis,int trafficColor,float trafficTime,float realSecSpeed,float dSecSpeed)
+{
+    float passTime=0;
+    double fAcc = 0.6;
+    if(dSecSpeed == 0)return true;
+    if (trafficColor == 2 || trafficColor == 3)
+    {
+        return false;
+    }
+    else if(trafficColor==0)
+    {
+        return true;
+    }
+    else
+    {
+        passTime=trafficDis/dSecSpeed;
+        if(passTime+1< trafficTime)
+        {
+            double passTime2= trafficTime - 0.1;
+            double disthru = realSecSpeed * passTime2 + 0.5 * fAcc * pow(passTime2,2);
+            if((disthru -1.0)< trafficDis )
+            {
+                if((realSecSpeed<3.0)&&(trafficDis>10))
+                {
+                    return true;
+                }
+                return false;
+            }
+            return true;
+        }
+        else
+        {
+            return false;
+        }
+    }
+}
+
+float iv::decition::DecideGps00::computeTrafficSpeedLimt(float trafficDis)
+{
+    float limit=200;
+    double fBrake = 1.0;
+    if(trafficDis < 1.0)
+    {
+        limit =0;
+    }
+    else
+    {
+        limit = 3.6*sqrt(2.0* fabs(fBrake) * (trafficDis-1.0) );
+    }
+    return limit;
+    if(trafficDis<10)
+    {
+        limit = 0;
+    }
+    else if(trafficDis<15)
+    {
+        limit = 5;
+    }
+    else if(trafficDis<20)
+    {
+        limit=10;
+    }
+    else if(trafficDis<30)
+    {
+        limit=15;
+    }
+    return limit;
+}
+
+void iv::decition::DecideGps00::transferGpsMode2( const std::vector<GPSData> gpsMapLine)
+{
+    static int obstacle_disable = 0;
+    static int speed_slowdown_flag = 0;
+    static bool lock_flag = false;
+    double forecast_distance = 0;
+    int forecast_point_num = 0;
+    bool cross = false;
+
+    double secLowSpeed = ServiceCarStatus.mroadmode_vel.mfmode18 / 3.6;   //m/s
+    if(secSpeed > secLowSpeed)
+    {
+        forecast_distance = secSpeed * secSpeed - secLowSpeed * secLowSpeed + 5;
+        forecast_point_num = ((int)forecast_distance) * 10;
+        if((PathPoint + forecast_point_num + 2) > gpsMapLine.size())
+            forecast_point_num = 0;
+    }
+    if((PathPoint + forecast_point_num-8 > 0) && (PathPoint + forecast_point_num + 8 < gpsMapLine.size()))
+    {
+        for(int i = PathPoint + forecast_point_num - 8; i < PathPoint + forecast_point_num + 8; i++)
+        {
+            if(gpsMapLine[i]->mode2 == 5000)
+                cross = true;
+        }
+    }
+
+    //givlog->debug("decition_brain","PATHFORE: %d,Forecast: %d,cross: %d", PathPoint + forecast_point_num,forecast_point_num,cross);
+    if(gpsMapLine[PathPoint]->mode2 == 3000)
+    {
+        if(obsDistance>4)
+        {       //7   zj-5
+            obsDistance = 200;
+        }
+        else
+        {
+            lock_flag = false;
+            obsSpeed  = -realspeed / 3.6;
+        }
+
+        if((realspeed > 3) && (lock_flag == false))
+        {
+            minDecelerate = -0.5;
+        }
+        else
+        {
+            dSpeed = min(dSpeed,3.0);
+            lock_flag = true;
+        }
+    }
+    else if(gpsMapLine[PathPoint]->mode2 == 3001)
+    {
+        obstacle_disable = 1;
+    }
+    else if(gpsMapLine[PathPoint]->mode2 == 3002)
+    {
+        obstacle_disable = 0;
+    }
+    else if(gpsMapLine[PathPoint]->mode2 == 4000)
+    {
+        //ServiceCarStatus.msysparam.vehWidth=5.6;
+    }
+    else if(cross == true)
+    {
+        speed_slowdown_flag =1;
+        lock_flag = false;
+    }
+    else if(gpsMapLine[PathPoint]->mode2==5001)
+    {
+        speed_slowdown_flag=0;
+    }
+
+    if(obstacle_disable==1)
+    {
+        obsDistance=200;
+    }
+
+    if(speed_slowdown_flag == 1)
+    {
+        if((realspeed > ServiceCarStatus.mroadmode_vel.mfmode18) && (lock_flag == false))
+        {
+            minDecelerate = -0.3;
+        }
+        else
+        {
+            dSpeed = min(dSpeed,ServiceCarStatus.mroadmode_vel.mfmode18);
+            lock_flag = true;
+        }
+    }
+}
+
+float iv::decition::DecideGps00::computeAvoidX(int roadAim,int roadOri,GPSData gps)
+{
+    if(roadAim==roadOri)
+    {
+        return 0;
+    }
+    float x = 0;
+    float veh_to_roadSide = (gps->mfLaneWidth - ServiceCarStatus.msysparam.vehWidth) * 0.5;
+    float roadSide_to_roadSide = ServiceCarStatus.msysparam.vehWidth;
+    if(!ServiceCarStatus.inRoadAvoid)
+    {
+        x = (roadOri-roadAim) * gps->mfLaneWidth;
+    }
+    else
+    {
+        int num = roadOri - roadAim;
+        switch (abs(num) % 3)
+        {
+        case 0:
+            x = (num / 3) * gps->mfLaneWidth;
+            break;
+        case 1:
+            if(num > 0)
+            {
+                x = (num / 3) * gps->mfLaneWidth + veh_to_roadSide;
+            }
+            else
+            {
+                x = (num / 3) * gps->mfLaneWidth - veh_to_roadSide;
+            }
+            break;
+        case 2:
+            if(num > 0)
+            {
+                x = (num / 3) * gps->mfLaneWidth + veh_to_roadSide + roadSide_to_roadSide;
+            }
+            else
+            {
+                x = (num / 3) * gps->mfLaneWidth - veh_to_roadSide - roadSide_to_roadSide;
+            }
+
+            break;
+        default:
+            break;
+        }
+    }
+    return x;
+}
+
+double iv::decition::DecideGps00::computeDistToEnd(const std::vector<GPSData> gpsMapLine,int pathpoint)
+{
+    double dist_to_end = 0;
+    for(int i = pathpoint;i < gpsMapLine.size() - 1; i++)
+    {
+        if(gpsMapLine[i]->mode2 != 23)
+            dist_to_end += GetDistance(*gpsMapLine[i+1], *gpsMapLine[i]);
+    }
+    return dist_to_end;
+}
+
+void iv::decition::DecideGps00::computeAvoidBoundary(int roadOriginal,int roadSumMap,double roadWidthEvery,double vehicleWidth,int* avoidXLeft, int* avoidXRight )
+{
+    double RightBoundary = (((double)roadOriginal + 0.5) * roadWidthEvery - vehicleWidth / 2);
+    *avoidXRight = ((int)RightBoundary);
+    double LeftBoundary = (((double)(roadSumMap -roadOriginal - 1) + 0.5) * roadWidthEvery - vehicleWidth / 2);
+    *avoidXLeft = (-(int)LeftBoundary);
+}
+
+void iv::decition::DecideGps00::computeAvoidBoundary(int roadOriginal,int roadSumMap,double roadWidthEvery,double vehicleWidth,double* avoidXLeft, double* avoidXRight )
+{
+    double RightBoundary = (((double)roadOriginal + 0.5) * roadWidthEvery - vehicleWidth / 2);
+    *avoidXRight = ((int)RightBoundary);
+    double LeftBoundary = (((double)(roadSumMap -roadOriginal - 1) + 0.5) * roadWidthEvery - vehicleWidth / 2);
+    *avoidXLeft = (-(int)LeftBoundary);
+}
+
+int iv::decition::DecideGps00::computeAvoidXDistance(iv::LidarGridPtr lidarGridPtr, GPS_INS now_gps_ins, const std::vector<GPSData> gpsMapLine,
+                                                     int avoidLeft,int avoidRight,int offsetLast)
+{
+    int signed_record_avoidX = offsetLast, record_avoidX = 20;
+    double obs_cur_distance =500, record_obstacle_distance;
+    iv::Point2D obs_spline_record;
+    double step = 0.5; //lyp
+
+    obs_property.clear();
+    for (double i =  avoidLeft; i <= avoidRight; i=i+step)  //由原来的1米改成0.5米，将i由int 转换成double
+    {
+        obsvalue x_value;
+        obsvalue *x = &x_value;
+        x_value.avoid_distance = i;
+        gpsTraceAvoid.clear();
+        gpsTraceAvoid = getGpsTraceOffset(gpsTraceMapOri, i);
+        obs_spline_record=computeObsOnRoadSpline(lidarGridPtr, gpsTraceAvoid, i,gpsMapLine,now_gps_ins,x);
+        obs_property.push_back(x_value);
+        if(i==offsetLast)
+        {
+            obs_cur_distance=x_value.obs_distance;
+            record_obstacle_distance=obs_cur_distance;
+        }
+        givlog->debug("decition_brain","i: %d,obs:%f",x_value.avoid_distance,x_value.obs_distance);
+
+        obsSpline.push_back(obs_spline_record);
+    }
+
+    if (lidarGridPtr != NULL)
+    {
+        hasCheckedAvoidLidar = true;
+    }
+    if(offsetLast==0)
+    {
+        unsigned int vector_num_record=0;
+        for(int j = 0; j < obs_property.size(); j++)
+        {
+            if(obs_property[j].obs_distance > 100)
+            {
+                if(abs(obs_property[j].avoid_distance) < record_avoidX)
+                {
+                    record_avoidX = abs(obs_property[j].avoid_distance);
+                    signed_record_avoidX = obs_property[j].avoid_distance;
+                    vector_num_record = j;
+                }
+            }
+        }
+        if((signed_record_avoidX < 0) && (signed_record_avoidX-1 >= avoidLeft))
+        {
+            //int obs_test=abs(signed_record_avoidX)-1;
+            int obs_test = vector_num_record-1;
+            if(obs_property[obs_test].obs_distance > 100)
+            {
+                signed_record_avoidX -= 1;
+                givlog->debug("decition_brain","singrecord_Y: %d",signed_record_avoidX);
+            }
+        }
+        //added by lyp
+        // if((signed_record_avoidX < 0) && (signed_record_avoidX-1 >= avoidLeft))
+        // {
+        //     //int obs_test=abs(signed_record_avoidX)-1;
+        //     int obs_test = vector_num_record-1;
+        //     if(obs_property[obs_test].obs_distance > 100)
+        //     {
+        //         signed_record_avoidX -= 1;
+        //         givlog->debug("decition_brain","singrecord_Y: %d",signed_record_avoidX);
+        //     }
+        // }
+
+        if(signed_record_avoidX == 0)
+        {
+            for(int j = 0; j < obs_property.size(); j++)
+            {
+                if(obs_property[j].obs_distance > obs_cur_distance+15)
+                {
+                    if(obs_property[j].obs_distance > record_obstacle_distance)
+                    {
+                        record_avoidX = abs(obs_property[j].avoid_distance);
+                        signed_record_avoidX = obs_property[j].avoid_distance;
+                        record_obstacle_distance = obs_property[j].obs_distance;
+                    }
+                }
+            }
+        }
+    }
+    else
+    {
+        for(int j = 0; j < obs_property.size();j++)
+        {
+            if(obs_property[j].obs_distance > 100)
+            {
+                if((abs(obs_property[j].avoid_distance)<record_avoidX)&&(obs_property[j].avoid_distance!=offsetLast))
+                {
+                    record_avoidX=abs(obs_property[j].avoid_distance);
+                    signed_record_avoidX=obs_property[j].avoid_distance;
+                }
+            }
+        }
+        if(signed_record_avoidX==0)
+        {
+            return 0;
+        }
+        else if(obs_cur_distance>30)
+        {
+            signed_record_avoidX = offsetLast;
+        }
+        else if((obs_cur_distance<=30) && (signed_record_avoidX==offsetLast))
+        {
+            for(int j = 0;j<obs_property.size();j++)
+            {
+                if(obs_property[j].obs_distance>obs_cur_distance+15)
+                {
+                    if(obs_property[j].obs_distance>record_obstacle_distance)
+                    {
+                        record_avoidX = abs(obs_property[j].avoid_distance);
+                        signed_record_avoidX = obs_property[j].avoid_distance;
+                        record_obstacle_distance = obs_property[j].obs_distance;
+                    }
+                }
+            }
+        }
+    }
+
+    if((signed_record_avoidX!=offsetLast)&&(obs_cur_distance<50))
+    {
+        avoidMinDistance = obs_cur_distance + 2*ServiceCarStatus.msysparam.vehLenth;
+        startAvoidGps = now_gps_ins;
+        return signed_record_avoidX;
+    }
+    return offsetLast;
+}
+
+double iv::decition::DecideGps00::computeAvoidXDistance(iv::LidarGridPtr lidarGridPtr, GPS_INS now_gps_ins, const std::vector<GPSData> gpsMapLine,
+                                                     double avoidLeft,double avoidRight,double offsetLast)
+{
+    double signed_record_avoidX = offsetLast, record_avoidX = 20;
+    double obs_cur_distance =500, record_obstacle_distance;
+    iv::Point2D obs_spline_record;
+    double step = 0.5;
+
+    obs_property.clear();
+    for (double i =  avoidLeft; i <= avoidRight; i=i+step)  //由原来的1米改成0.5米，将i由int 转换成double
+    {
+        obsvalue x_value;
+        obsvalue *x = &x_value;
+        x_value.avoid_distance = i;
+        gpsTraceAvoid.clear();
+        gpsTraceAvoid = getGpsTraceOffset(gpsTraceMapOri, i);
+        obs_spline_record=computeObsOnRoadSpline(lidarGridPtr, gpsTraceAvoid, i,gpsMapLine,now_gps_ins,x);
+        obs_property.push_back(x_value);
+        if(i==offsetLast)
+        {
+            obs_cur_distance=x_value.obs_distance;
+            record_obstacle_distance=obs_cur_distance;
+        }
+        givlog->debug("decition_brain","i: %d,obs:%f",x_value.avoid_distance,x_value.obs_distance);
+
+        obsSpline.push_back(obs_spline_record);
+    }
+
+    if (lidarGridPtr != NULL)
+    {
+        hasCheckedAvoidLidar = true;
+    }
+    if(offsetLast==0)//上次的避障距离是0或者沿参考线行驶第一次避障
+    {
+        unsigned int vector_num_record=0;
+        for(int j = 0; j < obs_property.size(); j++)
+        {
+            if(obs_property[j].obs_distance > 100) //障碍物距离大于100米认为可以臂章
+            {
+                if(abs(obs_property[j].avoid_distance) < record_avoidX)
+                {
+                    record_avoidX = abs(obs_property[j].avoid_distance);    //找到距离车辆当前行驶的位置横向最近的臂章距离
+                    signed_record_avoidX = obs_property[j].avoid_distance;
+                    vector_num_record = j;
+                }
+            }
+        }
+        //优先向左避障
+        if((signed_record_avoidX < 0) && (signed_record_avoidX-2*step>= avoidLeft))  //-0.5  -1  -1.5都可以避障的时候选则-1.5,
+        {
+            //int obs_test=abs(signed_record_avoidX)-1;
+            int obs_test = vector_num_record-1;
+            int obs_testneighbor = obs_test-1;
+            if((obs_property[obs_test].obs_distance > 100)&&(obs_property[obs_testneighbor].obs_distance>100))
+            {
+                signed_record_avoidX =signed_record_avoidX-2*step;
+                givlog->debug("decition_brain","singrecord_Y: %d",signed_record_avoidX);
+            }
+        }
+        else if((signed_record_avoidX < 0) && (signed_record_avoidX-1*step>= avoidLeft))//-0.5  -1 可以避障的时候选则-1,
+        {
+            //int obs_test=abs(signed_record_avoidX)-1;
+            int obs_test = vector_num_record-1;
+            if(obs_property[obs_test].obs_distance > 100)
+            {
+                signed_record_avoidX -= 1;
+                givlog->debug("decition_brain","singrecord_Y: %d",signed_record_avoidX);
+            }
+        }
+        else if((signed_record_avoidX > 0) && (signed_record_avoidX+2*step<= avoidRight))  //0.5  1  1.5都可以避障的时候选则1.5,
+        {
+            int obs_test = vector_num_record+1;
+            int obs_testneighbor = obs_test+1;
+            if((obs_property[obs_test].obs_distance > 100)&&(obs_property[obs_testneighbor].obs_distance>100)
+                    )
+            {
+                signed_record_avoidX =signed_record_avoidX+2*step;
+                givlog->debug("decition_brain","singrecord_Y: %d",signed_record_avoidX);
+            }
+        }
+        else if((signed_record_avoidX>0) && (signed_record_avoidX+1*step<= avoidRight))//0.5  1 可以避障的时候选则1,
+        {
+            int obs_test = vector_num_record+1;
+            if(obs_property[obs_test].obs_distance > 100)
+            {
+                signed_record_avoidX -= 1;
+                givlog->debug("decition_brain","singrecord_Y: %d",signed_record_avoidX);
+            }
+        }
+        else if (signed_record_avoidX < 0)
+        {
+                signed_record_avoidX=signed_record_avoidX; //只有-0.5 0.5 的时候就选则-0.5  0.5,优先选择-0.5
+        }
+        else
+        {
+            ;
+        }
+
+        if(signed_record_avoidX == 0)
+        {
+            for(unsigned int j = 0; j < obs_property.size(); j++)
+            {
+                if(obs_property[j].obs_distance > obs_cur_distance+15)
+                {
+                    if(obs_property[j].obs_distance > record_obstacle_distance)
+                    {
+                        record_avoidX = abs(obs_property[j].avoid_distance);
+                        signed_record_avoidX = obs_property[j].avoid_distance;
+                        record_obstacle_distance = obs_property[j].obs_distance;
+                    }
+                }
+            }
+            givlog->debug("decition_brain","offsetLast==0,and signed_record_avoidX==0");//20230213
+        }
+    }
+    else
+    {
+        for(int j = 0; j < obs_property.size();j++)
+        {
+            if(obs_property[j].obs_distance > 100)
+            {
+                if((abs(obs_property[j].avoid_distance)<record_avoidX)&&(obs_property[j].avoid_distance!=offsetLast))
+                {
+                    record_avoidX=abs(obs_property[j].avoid_distance);
+                    signed_record_avoidX=obs_property[j].avoid_distance;//返回的是距离原始参考路线最近的路线
+                }
+            }
+        }
+        if(signed_record_avoidX==0) //优先返回原参考路线
+        {
+            return 0;
+        }
+        else if(obs_cur_distance>30)//如果当前障碍物距离大于30米，继续当前的避障距离
+        {
+            signed_record_avoidX = offsetLast;
+        }
+        else if((obs_cur_distance<=30) && (signed_record_avoidX==offsetLast))
+        {
+            for(unsigned int j = 0;j<obs_property.size();j++)
+            {
+                if(obs_property[j].obs_distance>obs_cur_distance+15)
+                {
+                    if(obs_property[j].obs_distance>record_obstacle_distance)
+                    {
+                        record_avoidX = abs(obs_property[j].avoid_distance);
+                        signed_record_avoidX = obs_property[j].avoid_distance;
+                        record_obstacle_distance = obs_property[j].obs_distance;
+                    }
+                }
+            }
+            givlog->debug("decition_brain","obs_cur_distance<=30and signed_record_avoidX==offsetLast");//20230213
+        }
+    }
+
+    if((signed_record_avoidX!=offsetLast)&&(obs_cur_distance<50))
+    {
+        avoidMinDistance = obs_cur_distance + 2*ServiceCarStatus.msysparam.vehLenth;
+        startAvoidGps = now_gps_ins;
+        return signed_record_avoidX;
+    }
+    return offsetLast;
+}
+
+//iv::Point2D iv::decition::DecideGps00::computeObsOnRoadSpline(iv::LidarGridPtr lidarGridPtr, std::vector<Point2D> gpsTrace, int roadNum,
+//                                                              const std::vector<GPSData> gpsMapLine,GPS_INS now_gps_ins,obsvalue* obsva)
+iv::Point2D iv::decition::DecideGps00::computeObsOnRoadSpline(iv::LidarGridPtr lidarGridPtr, std::vector<Point2D> gpsTrace, double roadNum,
+                                                              const std::vector<GPSData> gpsMapLine,GPS_INS now_gps_ins,obsvalue* obsva) //20230213
+{
+    double obs=-1,obsSd=-1;
+    double obsCarCoordinateX,obsCarCoordinateY;
+    GPS_INS obsGeodetic;
+    Point2D obsFrenet,obsFrenetMid;
+    double obsCarCoordinateDistance=-1;
+
+    if (lidarGridPtr == NULL)
+    {
+        lidarDistance = lastLidarDis;
+    }
+    else
+    {
+        obsPoint = Compute00().getLidarObsPoint(gpsTrace, lidarGridPtr);
+        obsCarCoordinateDistance=obsPoint.y;
+
+        if((obsCarCoordinateDistance > 0) && (obsCarCoordinateDistance < 100))
+        {
+            obsCarCoordinateX = obsPoint.x;
+            obsCarCoordinateY = obsPoint.y;
+            obsGeodetic  = Coordinate_UnTransfer(obsCarCoordinateX, obsCarCoordinateY, now_gps_ins);      //车体转大地
+            obsFrenetMid = cartesian_to_frenet1D(gpsMapLine,obsGeodetic.gps_x,obsGeodetic.gps_y);          //大地转frenet
+            iv::Point2D now_s_d = cartesian_to_frenet1D(gpsMapLine,now_gps_ins.gps_x,now_gps_ins.gps_y);
+            givlog->debug("decition_brain","road_num: %d,obsFrenetMid_s: %f,now_s: %f",roadNum,obsFrenetMid.s,now_s_d.s);
+            obsFrenet.s=obsFrenetMid.s-now_s_d.s;
+            obsFrenet.d=roadNum;
+
+            //test
+            //obsFrenet.s=obsPoint.y;
+            //obsFrenet.d=roadNum;
+        }
+        else
+        {
+            obsFrenet.s=500;
+            obsFrenet.d=roadNum;
+        }
+
+        if (obsFrenet.s<0)
+        {
+            obsFrenet.s=0;
+        }
+        lidarDistance = obsFrenet.s;
+
+        lastLidarDis = lidarDistance;
+    }
+
+    obs = lidarDistance;
+    obsSd = obsPoint.obs_speed_y;
+    obsva->obs_distance = obs;
+    obsva->obs_speed = obsSd;
+
+    return obsFrenet;
+}

src/decition/decition_brain_sf_changan_shenlan/decition/decition.pri

@@ -30,7 +30,8 @@ HEADERS += \
     $$PWD/adc_adapter/interpolation_2d.h \
     $$PWD/adc_tools/PolynomialRegression.h \
     $$PWD/adc_tools/polyfit.h \
-    $$PWD/adc_planner/spline_planner.h
+    $$PWD/adc_planner/spline_planner.h \
+    $$PWD/adc_adapter/hunter_adapter.h
 
 SOURCES += \
     $$PWD/adc_adapter/changan_shenlan_adapter.cpp \
@@ -60,4 +61,5 @@ SOURCES += \
     $$PWD/adc_adapter/sightseeing_adapter.cpp \
     $$PWD/adc_adapter/interpolation_2d.cc \
     $$PWD/adc_tools/polyfit.cpp \
-    $$PWD/adc_planner/spline_planner.cpp
+    $$PWD/adc_planner/spline_planner.cpp \
+    $$PWD/adc_adapter/hunter_adapter.cpp

src/decition/decition_brain_sf_changan_shenlan/decition_brain_sf_changan_shenlan.pro

@@ -2,7 +2,7 @@ QT -= gui
 
 QT += network dbus xml widgets
 
-CONFIG += c++11 console
+CONFIG += c++11  #console
 CONFIG -= app_bundle
 
 # The following define makes your compiler emit warnings if you use
@@ -34,8 +34,10 @@ SOURCES += $$PWD/../common/main.cpp \
     ../../include/msgtype/fusionobject.pb.cc \
     ../../include/msgtype/carstate.pb.cc \
     ../../include/msgtype/v2r.pb.cc \
-    ../../include/msgtype/groupmsg.pb.cc
-
+    ../../include/msgtype/cameraobjectarray.pb.cc\
+    ../../include/msgtype/cameraobject.pb.cc\
+    ../../include/msgtype/groupmsg.pb.cc \
+    ../../include/msgtype/brainrunstate.pb.cc
 
 include($$PWD/../common/common/common.pri)
 include($$PWD/decition/decition.pri)
@@ -98,9 +100,10 @@ HEADERS += \
     ../common/perception_sf/sensor_radar.h \
     ../common/common/sysparam_type.h \
     ../../include/msgtype/carstate.pb.h \
-
     ../../include/msgtype/v2r.pb.h \
-
-    ../../include/msgtype/groupmsg.pb.h
+    ../../include/msgtype/cameraobjectarray.pb.h\
+    ../../include/msgtype/cameraobject.pb.h\
+    ../../include/msgtype/groupmsg.pb.h \
+    ../../include/msgtype/brainrunstate.pb.h
 
 

src/decition/decition_test/decition_test/decition_test.pro

@@ -0,0 +1,34 @@
+#-------------------------------------------------
+#
+# Project created by QtCreator 2023-04-23T15:52:35
+#
+#-------------------------------------------------
+
+QT       += core gui
+
+greaterThan(QT_MAJOR_VERSION, 4): QT += widgets
+
+TARGET = decition_test
+TEMPLATE = app
+
+# The following define makes your compiler emit warnings if you use
+# any feature of Qt which has been marked as deprecated (the exact warnings
+# depend on your compiler). Please consult the documentation of the
+# deprecated API in order to know how to port your code away from it.
+DEFINES += QT_DEPRECATED_WARNINGS
+
+# You can also make your code fail to compile if you use deprecated APIs.
+# In order to do so, uncomment the following line.
+# You can also select to disable deprecated APIs only up to a certain version of Qt.
+#DEFINES += QT_DISABLE_DEPRECATED_BEFORE=0x060000    # disables all the APIs deprecated before Qt 6.0.0
+
+
+SOURCES += \
+        main.cpp \
+        mainwindow.cpp
+
+HEADERS += \
+        mainwindow.h
+
+FORMS += \
+        mainwindow.ui

src/decition/decition_test/decition_test/main.cpp

@@ -0,0 +1,11 @@
+#include "mainwindow.h"
+#include <QApplication>
+
+int main(int argc, char *argv[])
+{
+    QApplication a(argc, argv);
+    MainWindow w;
+    w.show();
+
+    return a.exec();
+}

src/decition/decition_test/decition_test/mainwindow.cpp

@@ -0,0 +1,14 @@
+#include "mainwindow.h"
+#include "ui_mainwindow.h"
+
+MainWindow::MainWindow(QWidget *parent) :
+    QMainWindow(parent),
+    ui(new Ui::MainWindow)
+{
+    ui->setupUi(this);
+}
+
+MainWindow::~MainWindow()
+{
+    delete ui;
+}

src/decition/decition_test/decition_test/mainwindow.h

@@ -0,0 +1,22 @@
+#ifndef MAINWINDOW_H
+#define MAINWINDOW_H
+
+#include <QMainWindow>
+
+namespace Ui {
+class MainWindow;
+}
+
+class MainWindow : public QMainWindow
+{
+    Q_OBJECT
+
+public:
+    explicit MainWindow(QWidget *parent = 0);
+    ~MainWindow();
+
+private:
+    Ui::MainWindow *ui;
+};
+
+#endif // MAINWINDOW_H

src/decition/decition_test/decition_test/mainwindow.ui

@@ -0,0 +1,24 @@
+<ui version="4.0">
+ <class>MainWindow</class>
+ <widget class="QMainWindow" name="MainWindow" >
+  <property name="geometry" >
+   <rect>
+    <x>0</x>
+    <y>0</y>
+    <width>400</width>
+    <height>300</height>
+   </rect>
+  </property>
+  <property name="windowTitle" >
+   <string>MainWindow</string>
+  </property>
+  <widget class="QMenuBar" name="menuBar" />
+  <widget class="QToolBar" name="mainToolBar" />
+  <widget class="QWidget" name="centralWidget" />
+  <widget class="QStatusBar" name="statusBar" />
+ </widget>
+ <layoutDefault spacing="6" margin="11" />
+ <pixmapfunction></pixmapfunction>
+ <resources/>
+ <connections/>
+</ui>

src/decition/laneline_decition_brain_sf_changan_shenlan/README.md

@@ -1,34 +1,47 @@
 ## 车道保持模块使用说明
 **三个关键模块**：摄像头驱动模块、车道线检测模块、车道保持决策模块
+
 **其他模块**：CAN驱动模块、惯导定位模块、控制模块
 
-各模块启动顺序：惯导定位模块（默认开启）—— 摄像头驱动模块 —— 车道线检测模块 —— CAN驱动模块 —— 控制模块 —— 道保持决策模块
+**各模块启动顺序**：惯导定位模块（默认开启）—— 摄像头驱动模块 —— 车道线检测模块 —— CAN驱动模块 —— 控制模块 —— 车道保持决策模块
+
 **所有模块启动，车辆方向盘有偏转表示程序正常接管车辆，可切换D档使车辆自动驾驶。**
 
 ***
-1. **摄像头驱动模块**：
-* 作用：获取前景摄像头采集的图像数据，并将数据发送到共享内存。
-* 启用步骤：
-    1. 确认前景摄像头型号以及连接到米文工控机的接口号。（目前型号：AR0147，接口号常为0）。
-    2. 浏览器登陆米文设置页面（127.0.0.1:3000），系统设置-GMSL设置，设置对应接口（VIDEO0）对应的摄像头型号。
-    3. 验证摄像头驱动模块能否正常工作：命令行启动摄像头驱动，有持续的image数据显示表示驱动运行正常。
-    4. 启动摄像头驱动模块。有两种启动方式：命令行启动or自动驾驶系统控制界面开关启动。
-2. **车道线检测模块**
-* 作用：读取共享内存中的图像数据，利用深度学习车道线检测模型识别图像中的车道线，并将检测的车道线数据发送到共享内存。
-* 启动步骤：
-    1. 车道线检测模块运行需要车道线检测模型文件和图像透视变换配置文件，确保文件存放路径正确。
-    2. 图像透视变换标定，目的是实现图像坐标系到世界坐标系的转换。标定步骤如下。
-        1. 运行show_coordinate_new.py文件
-        2. 选定车道线检测的ROI
-        3. 在两侧车道线上放置4个标记物，要求：在图像中，近车端标记物靠近车前盖但不被其遮盖，远车端标记物距离近车端标记物为10米
-        4. 在图像中点出4个标记物位置，并记录下左上、右上、左下、右下四个点像素坐标
-        5. 将步骤4中的像素坐标写入图像透视变换配置文件pers_ori.yaml中
-    3. 启动车道线检测模块。可在自动驾驶系统控制界面开关启动，观察弹出的视频中，车道线检测结果是否准确，观察透视变换后的图像中车道线检测结果是否准确。
-3. **车道保持决策模块**
-* 作用：读取共享内存中的车道线检测数据，根据检测的车道线数据生成车辆行驶轨迹，保持车辆在车道线内行驶。
-* 启动步骤：
-    1. 车辆行驶轨迹坐标修正：修改ADS_decision.xml中的camera_x_adjust(横向偏移)、camera_y_adjust（纵向偏移）和laneline_speed（车道保持行驶速度）。
-        1. camera_x_adjust：车辆行驶中心到车道中心偏移（通常为0即可）
-        2. camera_y_adjust：近车端标记物到惯导（车辆后轮中线）的纵向距离
-        3. laneline_speed：车辆进行车道线保持行驶时的速度
-    2. 启动车道保持决策模块，可在自动驾驶系统控制界面开关启动。
+**摄像头驱动模块**
+
+作用：获取前景摄像头采集的图像数据，并将数据发送到共享内存。
+
+启动步骤：
+
+1. 确认前景摄像头型号以及连接到米文工控机的接口号。（目前型号：AR0147，接口号常为0）。
+2. 浏览器登陆米文设置页面（127.0.0.1:3000），系统设置-GMSL设置，设置对应接口（VIDEO0）对应的摄像头型号。
+3. 验证摄像头驱动模块能否正常工作：命令行启动摄像头驱动，有持续的image数据显示表示驱动运行正常。
+4. 启动摄像头驱动模块。有两种启动方式：命令行启动or自动驾驶系统控制界面开关启动。
+
+**车道线检测模块**
+
+作用：读取共享内存中的图像数据，利用深度学习车道线检测模型识别图像中的车道线，并将检测的车道线数据发送到共享内存。
+
+启动步骤：
+
+1. 车道线检测模块运行需要车道线检测模型文件和图像透视变换配置文件，确保文件存放路径正确。
+2. 图像透视变换标定，目的是实现图像坐标系到世界坐标系的转换。标定步骤如下。
+   1. 运行show_coordinate_new.py文件。在文件所在路径下打开命令行，运行命令`python3 show_coordinate_new.py `。
+   2. 观察显示的视频画面，选定车道线检测的ROI。
+   3. 在两侧车道线上放置4个标记物，要求：在图像中，近车端标记物靠近车前盖但不被其遮盖，远车端标记物距离近车端标记物为10米。
+   4. 按下q键，在图像中点出4个标记物位置，并记录下左上、左下、右上、右下四个点像素坐标。
+   5. 将步骤4中的像素坐标写入图像透视变换配置文件**pers_ori.yaml**（app/yaml文件夹下）中。
+3. 启动车道线检测模块。可在自动驾驶系统控制界面开关启动，观察弹出的视频中，车道线检测结果是否准确，观察透视变换后的图像中车道线检测结果是否准确。
+
+**车道保持决策模块**
+
+作用：读取共享内存中的车道线检测数据，根据检测的车道线数据生成车辆行驶轨迹，保持车辆在车道线内行驶。
+
+启动步骤：
+
+1. 车辆行驶轨迹坐标修正：修改**ADS_decision.xml**（app文件夹下）中的camera_x_adjust（横向偏移）、camera_y_adjust（纵向偏移）和laneline_speed（车道保持行驶速度）。
+   - camera_x_adjust：车辆行驶中心到车道中心偏移（通常为0即可）
+   - camera_y_adjust：近车端标记物到惯导（车辆后轮中线）的纵向距离
+   - laneline_speed：车辆进行车道线保持行驶时的速度
+2. 启动车道保持决策模块，可在自动驾驶系统控制界面开关启动。

src/decition/laneline_decition_brain_sf_changan_shenlan/decition/brain.cpp

@@ -379,8 +379,15 @@ void iv::decition::BrainDecition::run() {
     std::string laneline_speed = xp.GetParam("laneline_speed","5.0");
     ServiceCarStatus.laneline_speed = atof(laneline_speed.data());
 
+    std::string socfDis = xp.GetParam("socfDis","15");
+    std::string aocfDis = xp.GetParam("aocfDis","25");
+    std::string aocfTimes = xp.GetParam("aocfTimes","3");
 
 
+    ServiceCarStatus.socfDis = atof(socfDis.data());
+    ServiceCarStatus.aocfDis = atof(aocfDis.data());
+    ServiceCarStatus.aocfTimes = atof(aocfTimes.data());
+
     givfault->SetFaultState(0,0,"OK.");
 
     while (eyes->isAllSensorRunning() &&(!boost::this_thread::interruption_requested()))
@@ -580,7 +587,7 @@ void iv::decition::BrainDecition::run() {
             }else{
 
                 std::cout << "----------------------------------------------getDecideFromLaneline-----------------"<<std::endl;
-                decition_gps = decitionfromlaneline->getDecideFromLaneline(*gps_data_, navigation_data);
+                decition_gps = decitionfromlaneline->getDecideFromLaneline(*gps_data_, navigation_data,templidar);
             }          
             mMutexMap.unlock();
 
@@ -913,13 +920,14 @@ void iv::decition::BrainDecition::UpdateLineToMap(iv::GPS_INS now_gps_ins)
         }
 
         if(left_line_points.size()!=0&&right_line_points.size()!=0){
+            roadWidth = fabs(left_line_points[0].x - right_line_points[0].x);
             k = k1;
             last_k = k;
-            line_length = std::min(left_line_points.size(),right_line_points.size());
+            line_length = left_line_points.size();
             for(int j=0;j<line_length;++j){
                 iv::Point2D reference_line_point;
-                reference_line_point.x = (left_line_points[j].x+right_line_points[j].x)/2;
-                reference_line_point.y = (left_line_points[j].y+right_line_points[j].y)/2;
+                reference_line_point.x = left_line_points[j].x+roadWidth/2.0;
+                reference_line_point.y = left_line_points[j].y;
                 reference_line_points.push_back(reference_line_point);
             }
             gpsTraceFromLane = iv::decition::DecideFromLaneline::getGpsTraceFromLane(reference_line_points);
@@ -930,7 +938,7 @@ void iv::decition::BrainDecition::UpdateLineToMap(iv::GPS_INS now_gps_ins)
             line_length = left_line_points.size();
             for(int j=0;j<line_length;++j){
                 iv::Point2D reference_line_point;
-                reference_line_point.x = left_line_points[j].x+1.8;
+                reference_line_point.x = left_line_points[j].x+roadWidth/2.0;
                 reference_line_point.y = left_line_points[j].y;
                 reference_line_points.push_back(reference_line_point);
             }
@@ -942,7 +950,7 @@ void iv::decition::BrainDecition::UpdateLineToMap(iv::GPS_INS now_gps_ins)
             line_length = right_line_points.size();
             for(int j=0;j<line_length;++j){
                 iv::Point2D reference_line_point;
-                reference_line_point.x = right_line_points[j].x-1.8;
+                reference_line_point.x = right_line_points[j].x-roadWidth/2.0;
                 reference_line_point.y = right_line_points[j].y;
                 reference_line_points.push_back(reference_line_point);
             }

src/decition/laneline_decition_brain_sf_changan_shenlan/decition/brain.h

@@ -78,6 +78,7 @@ namespace iv {
             std::vector<iv::Point2D> gpsTraceFromLane,lastGpsTraceFromLane,smoothGpsTraceFromLane;
             std::deque<std::vector<iv::Point2D>> lanePointDeque;
             double k, last_k;
+            double roadWidth = 3.6;
             int minLengthOfLaneline = 1000;
             void UpdateLineToMap(GPS_INS now_gps_ins);
 

src/decition/laneline_decition_brain_sf_changan_shenlan/decition/decide_from_laneline.cpp

@@ -44,6 +44,10 @@ double iv::decition::DecideFromLaneline::obsSpeed=0;
 bool iv::decition::DecideFromLaneline::isFirstRun = true;
 float iv::decition::DecideFromLaneline::minDecelerate=0;
 
+double iv::decition::DecideFromLaneline::lidarDistance = -1;
+double iv::decition::DecideFromLaneline::lastLidarDis = -1;
+double iv::decition::DecideFromLaneline::lastDistance = 500;
+
 extern double offset_real;
 extern double realspeed;
 extern double dSpeed;
@@ -51,8 +55,9 @@ extern double dSecSpeed;
 extern double secSpeed;
 
 extern std::vector<iv::Point2D>  gpsTraceAvoid,gpsTraceNow,gpsTraceBack,gpsTracePre,gpsTraceOri,gpsTraceRear,gpsTraceNowLeft,gpsTraceNowRight,gpsTraceAvoidXY,gpsTraceMapOri;
+extern std::vector<iv::Point2D> traceOriLeft,traceOriRight;//get by vehWidth and heading angle
 
-iv::decition::Decition iv::decition::DecideFromLaneline::getDecideFromLaneline(GPS_INS now_gps_ins, const std::vector<GPSData> gpsMapLine)
+iv::decition::Decition iv::decition::DecideFromLaneline::getDecideFromLaneline(GPS_INS now_gps_ins, const std::vector<GPSData> gpsMapLine, iv::LidarGridPtr lidarGridPtr)
 {
     Decition laneline_decition(new DecitionBasic);
 
@@ -126,6 +131,8 @@ iv::decition::Decition iv::decition::DecideFromLaneline::getDecideFromLaneline(G
     if(gpsTraceNow.size()==0)
     {
         gpsTraceNow.assign(gpsTraceOri.begin(), gpsTraceOri.end());
+        gpsTraceNowLeft.assign(traceOriLeft.begin(),traceOriLeft.end());
+        gpsTraceNowRight.assign(traceOriRight.begin(),traceOriRight.end());
     }
 
 
@@ -220,6 +227,63 @@ iv::decition::Decition iv::decition::DecideFromLaneline::getDecideFromLaneline(G
     transferGpsMode2(gpsMapLine);
     dSpeed = min(dSpeed, ServiceCarStatus.laneline_speed);
 
+    // obstacle avoid start
+    computeObsOnRoadXY(now_gps_ins,lidarGridPtr, gpsTraceNow,gpsTraceNowLeft,gpsTraceNowRight,gpsMapLine);
+    static int obs_filter=0,obs_filter_flag=0,obs_filter_dis_memory=0;
+    static double obs_filter_speed_memory=0;
+    if(obs_filter_flag==0)
+    {
+        if(obsDistance>0&&obsDistance<60)
+        {
+            obs_filter++;
+            if(obs_filter<20)                          //80
+            {
+                obsDistance=-1;
+                obsSpeed=0;
+            }
+            else
+            {
+                obs_filter_flag=1;
+                obs_filter_dis_memory=obsDistance;
+                obs_filter_speed_memory=obsSpeed;
+                obs_filter=0;
+            }
+        }
+        else
+        {
+            obs_filter=0;
+        }
+    }
+    else
+    {
+        if(obsDistance<0||obsDistance>60)
+        {
+            obs_filter++;
+            if(obs_filter<40)                           //80
+            {
+                obsDistance=obs_filter_dis_memory;
+                obsSpeed=obs_filter_speed_memory;
+            }
+            else
+            {
+                obs_filter_flag=0;
+                obs_filter=0;
+            }
+        }
+        else
+        {
+            obs_filter=0;
+            obs_filter_dis_memory=obsDistance;
+            obs_filter_speed_memory=obsSpeed;
+        }
+    }
+    ServiceCarStatus.mObsDistance=obsDistance;
+    if(obsDistance>0 && obsDistance<  ServiceCarStatus.socfDis)
+    {
+        dSpeed=0;
+    }
+    // obstacle avoid end
+
     laneline_decition->speed = dSpeed;
     laneline_decition->wheel_angle = 0.0 - controlAng;
 
@@ -371,6 +435,14 @@ double iv::decition::DecideFromLaneline::limitAngle(double speed, double angle)
             preAngle = -15;
         }
     }
+    if(preAngle > 400)
+        {
+            preAngle = 400;
+        }
+        if(preAngle < -400)
+        {
+            preAngle = -400;
+        }
     return preAngle;
 }
 
@@ -417,7 +489,8 @@ std::vector<iv::Point2D> iv::decition::DecideFromLaneline::getLanelinePoints(con
 
 std::vector<iv::Point2D> iv::decition::DecideFromLaneline::getGpsTrace(iv::GPS_INS now_gps_ins, const std::vector<GPSData> gpsMapLine, int lastIndex, bool circleMode) {
     vector<iv::Point2D> trace;
-
+    traceOriLeft.clear();
+    traceOriRight.clear();
     if (gpsMapLine.size() > 0 && PathPointNew >= 0) {
         int aimIndex;
         if(circleMode){
@@ -482,6 +555,8 @@ std::vector<iv::Point2D> iv::decition::DecideFromLaneline::getGpsTrace(iv::GPS_I
             double ptRight_y=(*gpsMapLine[index]).gps_y + ServiceCarStatus.msysparam.vehWidth / 2 * sin(xyhdg - PI / 2);
             Point2D ptRight = Coordinate_Transfer(ptRight_x, ptRight_y, now_gps_ins);
 
+            traceOriLeft.push_back(ptLeft);
+            traceOriRight.push_back(ptRight);
 
         }
     }
@@ -608,3 +683,60 @@ void iv::decition::DecideFromLaneline::phaseSpeedDecition(iv::decition::Decition
         changanshenlan_Adapter->getAdapterDeciton(gpsIns, gpsTraceNow, dSpeed, obsDis,obsSpeed,  decition->accelerator,ServiceCarStatus.mfCalAcc  ,changingDangWei, &decition);
     }
 }
+
+void iv::decition::DecideFromLaneline::computeObsOnRoadXY( iv::GPS_INS now_gps_ins,iv::LidarGridPtr lidarGridPtr, std::vector<Point2D> gpsTrace,std::vector<Point2D> gpsTraceLeft,std::vector<Point2D> gpsTraceRight,
+                                                    const std::vector<GPSData> gpsMapLine) {
+
+    double obs,obsSd;
+    double obsCarCoordinateX,obsCarCoordinateY;
+    GPS_INS obsGeodetic;
+    Point2D obsFrenet,obsFrenetMid;
+
+    if (lidarGridPtr == NULL)
+    {
+        lidarDistance = lastLidarDis;
+    }
+    else {
+        obsPoint = Compute00().getLidarObsPointXY(gpsTrace,gpsTraceLeft,gpsTraceRight,lidarGridPtr);
+        float lidarXiuZheng=0;
+        if(!ServiceCarStatus.useMobileEye){
+            lidarXiuZheng=0-ServiceCarStatus.msysparam.frontGpsXiuzheng;
+        }
+
+        obsCarCoordinateX=obsPoint.x;
+        obsCarCoordinateY=obsPoint.y;
+        obsGeodetic = Coordinate_UnTransfer(obsCarCoordinateX, obsCarCoordinateY, now_gps_ins);   //车体转大地
+        obsFrenetMid=cartesian_to_frenet1D(gpsMapLine,obsGeodetic.gps_x,obsGeodetic.gps_y);          //大地转frenet
+        iv::Point2D now_s_d=cartesian_to_frenet1D(gpsMapLine,now_gps_ins.gps_x,now_gps_ins.gps_y);
+        obsFrenet.s=obsFrenetMid.s-now_s_d.s;
+        lidarDistance = obsFrenet.s;//obsPoint.y + lidarXiuZheng;   //激光距离推到车头  gjw20191110 lidar
+
+        if (lidarDistance<0)
+        {
+            lidarDistance = -1;
+        }
+        lastLidarDis = lidarDistance;
+    }
+
+
+    if(lidarDistance<0){
+        lidarDistance=500;
+    }
+
+    std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "激光雷达距离:" << lidarDistance << std::endl;
+
+    ServiceCarStatus.mLidarObs = lidarDistance;
+    obs = lidarDistance;
+
+    obsSd= obsPoint.obs_speed_y;
+
+    ServiceCarStatus.mObs = obsDistance;
+    if(ServiceCarStatus.mObs>100){
+        ServiceCarStatus.mObs =-1;
+    }
+
+    if (obsDistance>0)
+    {
+        lastDistance = obsDistance;
+    }
+}

src/decition/laneline_decition_brain_sf_changan_shenlan/decition/decide_from_laneline.h

@@ -32,6 +32,11 @@ public:
     DecideFromLaneline();
     ~DecideFromLaneline();
 
+    static double lidarDistance;
+    static double lastLidarDis;
+    iv::Point2D obsPoint;
+    static double lastDistance;
+
     static double minDis;
     static double maxAngle;
     static int lastGpsIndexNew;
@@ -71,7 +76,7 @@ public:
 
 
 
-    Decition getDecideFromLaneline(GPS_INS gpsIns, const std::vector<GPSData> gpsMapLine);
+    Decition getDecideFromLaneline(GPS_INS gpsIns, const std::vector<GPSData> gpsMapLine, iv::LidarGridPtr lidarGridPtr);
 
     void initMethods();
     static std::vector<Point2D> getGpsTrace(GPS_INS rp, const std::vector<GPSData> gpsMap, int lastIndex, bool circleMode);
@@ -88,6 +93,8 @@ public:
     double limitSpeed(double angle, double speed);
 
     void  getGpsTraceNowLeftRight(std::vector<Point2D> gpsTrace);
+    void computeObsOnRoadXY(iv::GPS_INS now_gps_ins,iv::LidarGridPtr lidarGridPtr, std::vector<Point2D> gpsTrace,std::vector<Point2D> gpsTraceLeft,
+                            std::vector<Point2D> gpsTraceRight,const std::vector<GPSData> gpsMapLine);
     
 private:
     //  void changeRoad(int roadNum);

src/decition/shenlan _chassis_test/decition/chassic_form.ui

@@ -0,0 +1,21 @@
+<ui version="4.0" >
+ <author></author>
+ <comment></comment>
+ <exportmacro></exportmacro>
+ <class>Form</class>
+ <widget class="QWidget" name="Form" >
+  <property name="geometry" >
+   <rect>
+    <x>0</x>
+    <y>0</y>
+    <width>400</width>
+    <height>300</height>
+   </rect>
+  </property>
+  <property name="windowTitle" >
+   <string>Form</string>
+  </property>
+ </widget>
+ <pixmapfunction></pixmapfunction>
+ <connections/>
+</ui>

src/detection/detection_chassis/decodechassis.cpp

@@ -1,6 +1,7 @@
 
 #include "decodechassis.h"
 #include <iostream>
+#include <chrono>
 #include "ivlog.h"
 #include "ivfault.h"
 
@@ -12,6 +13,27 @@ extern "C"
 iv::Ivlog * mygivlog;
 iv::Ivfault * mygivfault;
 
+
+void PrintCANRaw(const iv::can::canraw * praw )
+{
+    char strout[512];
+    int ndatalen = praw->len();
+    int i;
+    snprintf(strout,512,"%ld id: %02X data: ",std::chrono::system_clock::now().time_since_epoch().count()/1000000, praw->id());
+    char * pdata = new char[8];
+    if(ndatalen>0)pdata = new char[ndatalen];
+    memcpy(pdata,praw->data().data(),ndatalen);
+    for(i=0;i<ndatalen;i++)
+    {
+        char strtem[10];
+        snprintf(strtem,10,"%02X ",pdata[i]);
+        strncat(strout,strtem,512);
+    }
+    std::cout<<strout<<std::endl;
+    delete  pdata;
+
+}
+
 /**
   * @brief ShareChassis
   * Share Chassis State
@@ -488,7 +510,9 @@ int ProcShenLanCANFDChassis(void *pa, iv::can::canmsg *pmsg)
                 vehspeed = static_cast<double>(value) * 0.05625;
 
                 xchassis.set_vel(static_cast<float>(vehspeed));
+
                 ShareChassis(pa,&xchassis);
+                xchassis.clear_angle_feedback();
                 std::cout<<"veh: "<<xchassis.vel()<<std::endl;
             }
             else
@@ -496,9 +520,169 @@ int ProcShenLanCANFDChassis(void *pa, iv::can::canmsg *pmsg)
                 std::cout<<" 1CC not 64 bytes. 1CC bytes: "<<praw->len()<<std::endl;
             }
         }
+        if(praw->id() == 0x18a)
+        {
+            unsigned char byte[64];
+            double ang;
+            if(praw->len() == 64)
+            {
+                memcpy(byte,praw->data().data(),64);
+                unsigned int value;
+                value = byte[0]&0xFF;
+                value = (value<<8) +  static_cast<unsigned int >( byte[1]);
+                if(value<32767)
+                    ang = static_cast<double>(value) * 0.1;
+                else
+                    ang=static_cast<double>(value) * 0.1-65536*0.1;
+
+                xchassis.set_angle_feedback(static_cast<float>(ang));
+     //           ShareChassis(pa,&xchassis);
+                std::cout<<"ang: "<<xchassis.angle_feedback()<<std::endl;
+            }
+            else
+            {
+                std::cout<<" 1CC not 64 bytes. 1CC bytes: "<<praw->len()<<std::endl;
+            }
+        }
 
     }
 
 
     return 0;
 }
+
+int ProcHunterChassis(void *pa, iv::can::canmsg *pmsg)
+{
+    int i;
+    static iv::chassis xchassis;
+    static bool bHave0x221 = false;
+    static bool bHave0x361 = false;
+    float fsoc = 0.0;
+    float fsoh = 0.0;
+    float fbatv = 0.0;
+    float fbati = 0.0;
+    float fbatt = 0.0;
+    float fmileage_left = 0.0;
+    float fmileage_right = 0.0;
+    double fwheelangle_feedback = 0.0;
+    for(i=0;i<pmsg->rawmsg_size();i++)
+    {
+
+        const iv::can::canraw * praw = &(pmsg->rawmsg(i));
+//        unsigned char data[8];
+//        memcpy(data,praw->data().data(),8);
+        if(praw->id() == 0x221)
+        {
+            unsigned char vdata[2];
+            memcpy(&vdata,praw->data().data(),2);
+            unsigned char vswap[2];
+            vswap[0] = vdata[1];
+            vswap[1] = vdata[0];
+            short svalue;
+            memcpy(&svalue,vswap,2);
+            double vehspeed =  svalue;
+            vehspeed = vehspeed * 0.001 *3.6;
+
+            memcpy(&vdata,praw->data().data() + 6,2);
+            vswap[0] = vdata[1];
+            vswap[1] = vdata[0];
+            memcpy(&svalue,vswap,2);
+            fwheelangle_feedback =  svalue;
+            fwheelangle_feedback = fwheelangle_feedback * 0.001;
+
+
+            xchassis.set_vel(static_cast<float>(vehspeed));
+            xchassis.set_angle_feedback(fwheelangle_feedback);
+            bHave0x221 = true;
+        }
+        if(praw->id() == 0x361)
+        {
+            unsigned char vdata[8];
+            memcpy(&vdata,praw->data().data(),8);
+ //           std::cout<<" vdata "<<(int)vdata[0]<<" "<<(int)vdata[1]<<" "<<(int)vdata[2]<<" "<<(int)vdata[3]<<" "<<std::endl;
+            fsoc = vdata[0];
+            memcpy(&vdata,praw->data().data() + 1,1);
+            fsoh = vdata[0];
+            memcpy(&vdata,praw->data().data()+2,2);
+            double fvh = vdata[0];
+            double fvl = vdata[1];
+            fbatv = (fvh * 256 + fvl) * 0.01f;
+
+            memcpy(&vdata,praw->data().data()+4,2);
+            unsigned char vswap[2];
+            vswap[0] = vdata[1];
+            vswap[1] = vdata[0];
+            unsigned short svalue;
+            memcpy(&svalue,vswap,2);
+            fbati = svalue;
+            fbati = fbati * 0.1f;
+
+            memcpy(&vdata,praw->data().data()+6,2);
+            vswap[0] = vdata[1];
+            vswap[1] = vdata[0];
+            memcpy(&svalue,vswap,2);
+            fbatt = svalue;
+            fbatt = fbatt * 0.1f;
+
+            bHave0x361 = true;
+        }
+        if(praw->id() == 0x311)
+        {
+            unsigned char vdata[4];
+            memcpy(&vdata,praw->data().data(),4);
+            unsigned char vswap[4];
+            vswap[0] = vdata[3];
+            vswap[1] = vdata[2];
+            vswap[2] = vdata[1];
+            vswap[3] = vdata[0];
+            int mil_left;
+            memcpy(&mil_left,vswap,4);
+            fmileage_left = mil_left;
+            fmileage_left = fmileage_left * 0.001f;
+
+            memcpy(&vdata,praw->data().data(),4);
+            vswap[0] = vdata[3];
+            vswap[1] = vdata[2];
+            vswap[2] = vdata[1];
+            vswap[3] = vdata[0];
+            int mil_right;
+            memcpy(&mil_right,vswap,4);
+            fmileage_right = mil_right;
+            fmileage_right = fmileage_right * 0.001f;
+        }
+        if(praw->id() == 0x358)
+        {
+
+
+        }
+        if(praw->id() == 0x3AA)
+        {
+
+
+        }
+
+        if(bHave0x221 && bHave0x361)
+        {
+            bHave0x221 = false;
+            bHave0x361 = false;
+            ShareChassis(pa,&xchassis);
+
+            std::cout<<"veh: "<<xchassis.vel()<<" wheel: "<<fwheelangle_feedback<<" soc: "<<fsoc<<" soh:"<<fsoh<< std::endl;
+            std::cout<<"bat v:"<<fbatv<<" i: "<<fbati<<" Temp:"<<fbatt<<" mil left:"<<fmileage_left<<" right: "<<fmileage_right<<std::endl;
+        }
+
+//        if(ghave0x13&&ghave0x14&&ghave0x15)
+//        {
+//            ghave0x13 = false;
+//            ghave0x14 = false;
+//            ghave0x15 = false;
+//            ShareChassis(pa,&xchassis);
+//            nRtn = 1;
+//        }
+    }
+
+
+    return 0;
+}
+
+

src/detection/detection_chassis/decodechassis.h

@@ -26,4 +26,8 @@ int ProcShenLanChassis(void * pa, iv::can::canmsg * pmsg);
 
 int ProcShenLanCANFDChassis(void *pa, iv::can::canmsg *pmsg);
 
+int ProcHunterChassis(void *pa, iv::can::canmsg *pmsg);
+
+void PrintCANRaw(const iv::can::canraw * praw );
+
 #endif // DECODECHASSIS_H

src/detection/detection_chassis/main.cpp

@@ -24,10 +24,12 @@ void * gpa ;
 
 QTime gTime;
 
+bool gbPrintRaw = false;
+
 
 namespace  iv {
 
-enum VehicleTypeDef {GE3,VV7,MIDCAR,PROBLUE,MIDBUS,HAPO,UNKNOWN, YUHESEN,SHENLAN,SHENLAN_CANFD} gVehicleType;  //车辆类型
+enum VehicleTypeDef {GE3,VV7,MIDCAR,PROBLUE,MIDBUS,HAPO,UNKNOWN, YUHESEN,SHENLAN,SHENLAN_CANFD,HUNTER} gVehicleType;  //车辆类型
 
 
 }
@@ -61,6 +63,16 @@ void Listencan0(const char * strdata,const unsigned int nSize,const unsigned int
         return;
     }
 
+    if(gbPrintRaw)
+    {
+        int i;
+        for(i=0;i<xmsg.rawmsg_size();i++)
+        {
+            const iv::can::canraw * praw = &(xmsg.rawmsg(i));
+            PrintCANRaw(praw);
+        }
+    }
+
     int nRtn = 0;
     switch (iv::gVehicleType) {
     case iv::GE3:
@@ -89,6 +101,9 @@ void Listencan0(const char * strdata,const unsigned int nSize,const unsigned int
     case iv::SHENLAN_CANFD:
         nRtn = ProcShenLanCANFDChassis(gpa,&xmsg);
         break;
+    case iv::HUNTER:
+        nRtn = ProcHunterChassis(gpa,&xmsg);
+        break;
     default:
         break;
     }
@@ -154,6 +169,7 @@ int main(int argc, char *argv[])
     std::string strvehicletype = xp.GetParam("vehicletype","GE3");
     std::string strmodulename = xp.GetParam("modulename","chassis"); //chassismsgname
     std::string strchassismsgname = xp.GetParam("chassismsgname","chassis");
+    gbPrintRaw = xp.GetParam("PrintRaw",false);
 
     iv::gVehicleType = iv::UNKNOWN;
     if(strncmp(strvehicletype.data(),"GE3",255) == 0)
@@ -197,6 +213,11 @@ int main(int argc, char *argv[])
         iv::gVehicleType = iv::SHENLAN_CANFD;
     }
 
+   if(strncmp(strvehicletype.data(),"HUNTER",255) == 0)
+    {
+        iv::gVehicleType = iv::HUNTER;
+    }
+
 //iv::gVehicleType = iv::MIDBUS;
 
     givlog = new iv::Ivlog(strmodulename.data());

src/detection/detection_lidar_centerpoint/detection_lidar_centerpoint.pro

@@ -83,9 +83,9 @@ LIBS += -L"/usr/local/lib" \
         -lcudart \
         -lcufft
 
-CUDA_SDK = "/usr/local/cuda-11.4/"   # cudaSDK路径
+CUDA_SDK = "/usr/local/cuda/"   # cudaSDK路径
 
-CUDA_DIR = "/usr/local/cuda-11.4/"            # CUDA tookit路径
+CUDA_DIR = "/usr/local/cuda/"            # CUDA tookit路径
 
 SYSTEM_NAME = linux         # 自己系统环境 'Win32', 'x64', or 'Win64'
 

src/detection/detection_lidar_centerpoint/main.cpp

@@ -24,10 +24,10 @@ void init()
     std::vector<double> yaw_norm_thresholds ;
     const std::string densification_world_frame_id = "map";
     const int densification_num_past_frames = 1;
-    const std::string trt_precision = "fp32";
-    const std::string encoder_onnx_path = "/home/nvidia/models/pts_voxel_encoder_centerpoint_tiny.onnx";//this->declare_parameter<std::string>("encoder_onnx_path");
+    const std::string trt_precision = "fp16";
+    const std::string encoder_onnx_path = "/home/nvidia/models/pts_voxel_encoder_centerpoint.onnx";//this->declare_parameter<std::string>("encoder_onnx_path");
     const std::string encoder_engine_path ="/home/nvidia/models/pts_voxel_encoder_centerpoint.eng";//this->declare_parameter<std::string>("encoder_engine_path");
-    const std::string head_onnx_path = "/home/nvidia/models/pts_backbone_neck_head_centerpoint_tiny.onnx";//this->declare_parameter<std::string>("head_onnx_path");
+    const std::string head_onnx_path = "/home/nvidia/models/pts_backbone_neck_head_centerpoint.onnx";//this->declare_parameter<std::string>("head_onnx_path");
     const std::string head_engine_path ="/home/nvidia/models/pts_backbone_neck_head_centerpoint.eng" ;//this->declare_parameter<std::string>("head_engine_path");
     const std::size_t point_feature_size =4;
     const std::size_t max_voxel_size =40000;

src/detection/detection_lidar_grid/detection_lidar_grid.pro

@@ -6,13 +6,14 @@
 
 QT       -= gui
 
-CONFIG += c++11 console
+CONFIG += c++14 console
 CONFIG -= app_bundle
 
 DEFINES += PERCEPTION_LIDAR_VV7_LIBRARY
 
 #CONFIG += plugin
 
+
 # The following define makes your compiler emit warnings if you use
 # any feature of Qt which has been marked as deprecated (the exact warnings
 # depend on your compiler). Please consult the documentation of the
@@ -58,11 +59,15 @@ DEFINES += QT_DEPRECATED_WARNINGS
 
 SOURCES += \
         perception_lidar_vv7.cpp \
-    perceptionoutput.cpp
+    perceptionoutput.cpp \
+    ../../include/msgtype/fusionobject.pb.cc \
+    ../../include/msgtype/fusionobjectarray.pb.cc
 
 HEADERS += \
         perception_lidar_vv7.h \
-    perceptionoutput.h
+    perceptionoutput.h \
+    ../../include/msgtype/fusionobject.pb.h \
+    ../../include/msgtype/fusionobjectarray.pb.h
 
 unix {
     target.path = /usr/lib

src/detection/detection_lidar_grid/perception_lidar_vv7.cpp

@@ -35,6 +35,8 @@ static void * g_lidar_pc_nofloor;
 static void * glidar_pc_p;
 static void * glidar_per;
 
+static void * gpafusion;
+
 iv::Ivfault *gfault = nullptr;
 iv::Ivlog *givlog = nullptr;
 
@@ -101,7 +103,9 @@ static void writepctosm(pcl::PointCloud<pcl::PointXYZI>::Ptr & point_cloud,void
     int * pHeadSize = (int *)strOut;
     *pHeadSize = 4 + point_cloud->header.frame_id.size()+4+8;
     memcpy(strOut+4,point_cloud->header.frame_id.c_str(),point_cloud->header.frame_id.size());
-    pcl::uint32_t * pu32 = (pcl::uint32_t *)(strOut+4+sizeof(point_cloud->header.frame_id.size()));
+//    pcl::uint32_t * pu32 = (pcl::uint32_t *)(strOut+4+sizeof(point_cloud->header.frame_id.size()));
+
+    unsigned int * pu32 = (unsigned int *)(strOut+4+sizeof(point_cloud->header.frame_id.size()));
     *pu32 = point_cloud->header.seq;
     memcpy(strOut+4+sizeof(point_cloud->header.frame_id.size()) + 4,&point_cloud->header.stamp,8);
     pcl::PointXYZI * p;
@@ -118,8 +122,8 @@ static void writepctosm(pcl::PointCloud<pcl::PointXYZI>::Ptr & point_cloud,void
 
 
 
-static float SENSOR_HEIGHT = 2.1;
-static float VEHICLE_HEIGHT = 2.6;
+static float SENSOR_HEIGHT = 1.9;
+static float VEHICLE_HEIGHT = 2.2;
 //static float local_max_slope_ = 10;
 //static float general_max_slope_ = 3;
 //static float concentric_divider_distance_ = 1.5;
@@ -278,7 +282,7 @@ static void GridGetObs(pcl::PointCloud<pcl::PointXYZI>::Ptr & point_cloud,std::s
         Cpos = (x-HOR_MIN)/GRID_SIZE;
         if((Rpos>=0)&&(Rpos<nVecSize)&&(Cpos>=0)&&(Cpos<nHorSize))
         {
-            if((z-floor_h[Rpos*nHorSize+Cpos])>0.3)
+            if((z-floor_h[Rpos*nHorSize+Cpos])>0.15)
             {
                point_cloud_nofloor->push_back(po);
                ++point_cloud_nofloor->width;
@@ -400,6 +404,59 @@ static void GridGetObs(pcl::PointCloud<pcl::PointXYZI>::Ptr & point_cloud,std::s
 
 #include <QTime>
 
+#include "fusionobjectarray.pb.h"
+
+iv::fusion::fusionobjectarray xfusion;
+
+void ShareFusionObject(void * pa ,std::shared_ptr<std::vector<iv::ObstacleBasic>> & lidar_obs)
+{
+
+    double timestamp = std::chrono::system_clock::now().time_since_epoch().count();
+    timestamp = timestamp/1000000.0;
+
+    xfusion.clear_obj();
+
+    xfusion.set_timestamp(timestamp);
+
+    iv::fusion::fusionobject * pobj =  xfusion.add_obj();
+
+    pobj->set_id(0);
+    pobj->set_type(0);
+    iv::fusion::VelXY *pxvel = new iv::fusion::VelXY;
+    pxvel->set_x(0);
+    pxvel->set_y(0);
+    pobj->set_allocated_vel_abs(pxvel);
+    iv::fusion::Dimension * pxdim = new iv::fusion::Dimension;
+    pxdim->set_x(0.2);
+    pxdim->set_y(0.2);
+    pxdim->set_z(2.0);
+
+    pobj->set_allocated_dimensions(pxdim);
+
+    int i;
+    int nsize = lidar_obs->size();
+    for(i=0;i<nsize;i++)
+    {
+        iv::fusion::NomalXYZ * pcen = pobj->add_nomal_centroid();
+        pcen->set_nomal_x(lidar_obs->at(i).nomal_x);
+        pcen->set_nomal_y(lidar_obs->at(i).nomal_y);
+        pcen->set_nomal_z(lidar_obs->at(i).nomal_z);
+    }
+
+    int nbytesize = xfusion.ByteSize();
+    std::shared_ptr<char> pstr_ptr = std::shared_ptr<char>(new char[nbytesize]);
+
+    if(xfusion.SerializeToArray(pstr_ptr.get(),nbytesize))
+    {
+        iv::modulecomm::ModuleSendMsg(pa,pstr_ptr.get(),nbytesize);
+    }
+    else
+    {
+        std::cout<<" serialzie fusion fail."<<std::endl;
+    }
+
+
+}
 
 
 static void ListenPointCloud(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
@@ -446,6 +503,8 @@ static void ListenPointCloud(const char * strdata,const unsigned int nSize,const
 
 //   std::cout<<"prc."<<std::endl;
 
+    ShareFusionObject(gpafusion,lidar_obs);
+
     if(g_robosys == 0)
     {
         if(lidar_obs->size() == 0)
@@ -484,6 +543,8 @@ void  PERCEPTION_LIDAR_VV7SHARED_EXPORT StartPerception_lidar_vv7()
 
     glidar_obs = iv::modulecomm::RegisterSend(gstr_outputmemname,20000000,3);
 
+    gpafusion = iv::modulecomm::RegisterSend("li_ra_fusion",10000000,1);
+
 #ifdef OUT_GROUND
     g_lidar_pc_floor = iv::modulecomm::RegisterSend("lidar_pc_floor",20000000,3);
     g_lidar_pc_nofloor = iv::modulecomm::RegisterSend("lidar_pc_nofloor",20000000,3);

src/detection/detection_ndt_matching/detection_ndt_matching.pro

@@ -0,0 +1,94 @@
+QT -= gui
+QT += network
+CONFIG += c++17 #console
+CONFIG -= app_bundle
+
+QMAKE_CXXFLAGS += -std=gnu++17
+
+QMAKE_LFLAGS += -no-pie
+
+QMAKE_CXXFLAGS +=  -g
+
+QMAKE_CXXFLAGS += -O3
+
+# The following define makes your compiler emit warnings if you use
+# any feature of Qt which as been marked deprecated (the exact warnings
+# depend on your compiler). Please consult the documentation of the
+# deprecated API in order to know how to port your code away from it.
+DEFINES += QT_DEPRECATED_WARNINGS
+
+# You can also make your code fail to compile if you use deprecated APIs.
+# In order to do so, uncomment the following line.
+# You can also select to disable deprecated APIs only up to a certain version of Qt.
+#DEFINES += QT_DISABLE_DEPRECATED_BEFORE=0x060000    # disables all the APIs deprecated before Qt 6.0.0
+
+SOURCES += main.cpp \
+    ndt_matching.cpp \
+    ../../include/msgtype/ndtpos.pb.cc \
+    gnss_coordinate_convert.cpp \
+    ../../include/msgtype/imu.pb.cc \
+    ../../include/msgtype/gpsimu.pb.cc \
+    ../../include/msgtype/gps.pb.cc \
+    ../../include/msgtype/ndtgpspos.pb.cc \
+    globalrelocation.cpp \
+    ../../include/msgtype/pcdmap.pb.cc
+
+HEADERS += \
+    ndt_matching.h \
+    ../../include/msgtype/ndtpos.pb.h \
+    gnss_coordinate_convert.h \
+    ../../include/msgtype/imu.pb.h \
+    ../../include/msgtype/gpsimu.pb.h \
+    ../../include/msgtype/gps.pb.h \
+    ../../include/msgtype/ndtgpspos.pb.h \
+    globalrelocation.h \
+    ../../include/msgtype/pcdmap.pb.h
+
+INCLUDEPATH += /opt/ros/melodic/include
+INCLUDEPATH += /opt/ros/kinetic/include
+INCLUDEPATH += /usr/include/pcl-1.8
+INCLUDEPATH += /usr/include/pcl-1.7
+INCLUDEPATH += /usr/include/eigen3
+unix:INCLUDEPATH += /usr/include/pcl-1.10
+INCLUDEPATH += /opt/ros/noetic/include
+
+INCLUDEPATH += $$PWD/../../include/msgtype/
+
+
+
+unix:LIBS += -lboost_thread -lboost_system -lprotobuf
+
+unix:LIBS +=  -lpcl_common\
+        -lpcl_features\
+        -lpcl_filters\
+        -lpcl_io\
+        -lpcl_io_ply\
+        -lpcl_kdtree\
+        -lpcl_keypoints\
+        -lpcl_octree\
+        -lpcl_outofcore\
+        -lpcl_people\
+        -lpcl_recognition\
+        -lpcl_registration\
+        -lpcl_sample_consensus\
+        -lpcl_search\
+        -lpcl_segmentation\
+        -lpcl_surface\
+        -lpcl_tracking\
+        -lpcl_visualization
+
+
+
+!include(../../../include/common.pri ) {
+    error( "Couldn't find the common.pri file!" )
+}
+
+INCLUDEPATH += $$PWD/../../common/ndt_cpu/
+
+
+#DEFINES+= TEST_CALCTIME
+
+INCLUDEPATH += $$PWD/../../../include/
+LIBS += -L$$PWD/../../../bin/ -lxmlparam -lmodulecomm -livlog -livfault -lndt_cpu  -livexit -livbacktrace
+
+

src/detection/detection_ndt_matching/detection_ndt_matching.xml

@@ -0,0 +1,14 @@
+<xml>	
+	<node name="detection_ndt_matching.xml">
+		<param name="lidarmsg" value="lidar_pc" />
+		<param name="gpsmsg" value="hcp2_gpsimu" />
+		<param name="ndtposmsg" value="ndtpos" />
+		<param name="ndtgpsposmsg" value="ndtgpspos" />
+		<param name="AngleCorrect" value="0.0" />
+		<param name="HeightCorrect" value="0.0" />
+		<param name="SaveLastPos" value="true" />
+		<param name="Arm_LidarGPSBase_x" value="0.0" />
+		<param name="Arm_LidarGPSBase_y" value="0.0" />
+		<param name="useanh" value="false" />
+	</node>
+</xml>

src/detection/detection_ndt_matching/globalrelocation.cpp

@@ -0,0 +1,484 @@
+#include "globalrelocation.h"
+
+#include <pcl/filters/voxel_grid.h>
+#include <tf/tf.h>
+#include <pcl/io/pcd_io.h>
+
+GlobalRelocation::GlobalRelocation()
+{
+//    std::cout<<" run hear."<<std::endl;
+    if(mnThreadNum > MAX_NDT)mnThreadNum = MAX_NDT;
+    int i;
+    for(i=0;i<MAX_NDT;i++)mpthread[i] = NULL;
+}
+
+void GlobalRelocation::threadtest(int n,iv::Reloc & xReloc)
+{
+
+}
+
+void GlobalRelocation::threadReloc(int nThread,cpu::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> * pndt,
+                                   pcl::PointCloud<pcl::PointXYZ>::Ptr raw_scan,
+                                   iv::Reloc  xReloc)
+{
+
+    xReloc.x_calc = xReloc.x_guess;
+    xReloc.y_calc = xReloc.y_guess;
+    xReloc.z_calc = xReloc.z_guess;
+    xReloc.yaw_calc = xReloc.yaw_guess;
+    xReloc.pitch_calc = 0;// xReloc.pitch_guess;
+    xReloc.roll_calc = 0;//xReloc.roll_guess;
+    xReloc.trans_prob = 0;
+
+
+    Eigen::Matrix4f tf_btol;
+    Eigen::Translation3f tl_btol(0, 0, 0);                 // tl: translation
+    Eigen::AngleAxisf rot_x_btol(0, Eigen::Vector3f::UnitX());  // rot: rotation
+    Eigen::AngleAxisf rot_y_btol(0, Eigen::Vector3f::UnitY());
+    Eigen::AngleAxisf rot_z_btol(0, Eigen::Vector3f::UnitZ());
+    tf_btol = (tl_btol * rot_z_btol * rot_y_btol * rot_x_btol).matrix();
+
+    double fLastProb = 0.0;
+    int i;
+    for(i=0;i<50;i++)
+    {
+
+        double voxel_leaf_size = 2.0;
+
+        pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_scan(new pcl::PointCloud<pcl::PointXYZ>());
+
+        pcl::VoxelGrid<pcl::PointXYZ> voxel_grid_filter;
+        voxel_grid_filter.setLeafSize(voxel_leaf_size, voxel_leaf_size, voxel_leaf_size);
+        voxel_grid_filter.setInputCloud(raw_scan);
+        voxel_grid_filter.filter(*filtered_scan);
+
+        pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_scan_ptr(new pcl::PointCloud<pcl::PointXYZ>(*filtered_scan));
+
+        pndt->setInputSource(filtered_scan_ptr);
+
+        Eigen::Translation3f init_translation(xReloc.x_calc,xReloc.y_calc, xReloc.z_calc);
+        Eigen::AngleAxisf init_rotation_x(xReloc.roll_calc, Eigen::Vector3f::UnitX());
+        Eigen::AngleAxisf init_rotation_y(xReloc.pitch_calc, Eigen::Vector3f::UnitY());
+        Eigen::AngleAxisf init_rotation_z(xReloc.yaw_calc, Eigen::Vector3f::UnitZ());
+        Eigen::Matrix4f init_guess = (init_translation * init_rotation_z * init_rotation_y * init_rotation_x) * tf_btol;
+
+        pcl::PointCloud<pcl::PointXYZ>::Ptr aligned(new pcl::PointCloud<pcl::PointXYZ>());
+        pndt->align(*aligned,init_guess);
+
+        bool has_converged;
+        int iteration = 0;
+        double fitness_score = 0.0;
+        double trans_probability = 0.0;
+        Eigen::Matrix4f t(Eigen::Matrix4f::Identity());
+        Eigen::Matrix4f t2(Eigen::Matrix4f::Identity());
+
+
+        has_converged = pndt->hasConverged();
+        t = pndt->getFinalTransformation();
+        iteration = pndt->getFinalNumIteration();
+        fitness_score = pndt->getFitnessScore();
+        trans_probability = pndt->getTransformationProbability();
+
+        t2 = t * tf_btol.inverse();
+
+        double x,y,z,yaw,pitch,roll;
+
+        tf::Matrix3x3 mat_b;  // base_link
+        mat_b.setValue(static_cast<double>(t2(0, 0)), static_cast<double>(t2(0, 1)), static_cast<double>(t2(0, 2)),
+                       static_cast<double>(t2(1, 0)), static_cast<double>(t2(1, 1)), static_cast<double>(t2(1, 2)),
+                       static_cast<double>(t2(2, 0)), static_cast<double>(t2(2, 1)), static_cast<double>(t2(2, 2)));
+
+        // Update ndt_pose
+        x = t2(0, 3);
+        y = t2(1, 3);
+        z = t2(2, 3);
+
+        //      ndt_pose.x = localizer_pose.x;
+        //      ndt_pose.y = localizer_pose.y;
+        //      ndt_pose.z = localizer_pose.z;
+        mat_b.getRPY(roll, pitch, yaw, 1);
+
+        xReloc.x_calc = x;
+        xReloc.y_calc = y;
+        xReloc.z_calc = z;
+        xReloc.roll_calc = roll;
+        xReloc.pitch_calc = pitch;
+        xReloc.yaw_calc =yaw;
+        xReloc.trans_prob = trans_probability;
+
+
+ //       std::cout<<"guass x:"<<xReloc.x_guess<<" res:"<<" x: "<<xReloc.x_calc<<" y: "<<xReloc.y_calc<<" prob: "<<xReloc.trans_prob<<" step: "<<i<<std::endl;
+
+        if((i>=10)&&(xReloc.trans_prob<1.0))
+        {
+            break;
+        }
+
+        if((xReloc.trans_prob>3.0) && ((xReloc.trans_prob - fLastProb)<0.01) && (i>20))
+        {
+            break;
+        }
+
+        if((xReloc.trans_prob<1.5) && ((xReloc.trans_prob - fLastProb)<0.01) && (i>20))
+        {
+            break;
+        }
+
+        fLastProb = xReloc.trans_prob;
+
+    }
+
+    mmutexReloc.lock();
+    mvectorReloc.push_back(xReloc);
+    mmutexReloc.unlock();
+
+    std::cout<<"guass x:"<<xReloc.x_guess<<" y:"<<xReloc.y_guess<<" res:"<<" x: "<<xReloc.x_calc<<" y: "<<xReloc.y_calc<<" prob: "<<xReloc.trans_prob<<std::endl;
+
+    mthreadcomplete[nThread] = true;
+
+}
+
+void GlobalRelocation::RunReloc(std::vector<iv::ndttracepoint> & xtrace,pcl::PointCloud<pcl::PointXYZ>::Ptr raw_scan,bool bCheckComplete)
+{
+    int i;
+    bool bComplete = false;
+    bool bHaveTask = true;
+    int ntracesize = static_cast<int>(xtrace.size());
+    int j = 0;
+    if(j == ntracesize)bHaveTask = false;
+    while(bComplete == false)
+    {
+        if(bHaveTask)
+        {
+            for(i=0;i<mnThreadNum;i++)
+            {
+                if((mthreadcomplete[i] == true) &&(bHaveTask))
+                {
+                    if(mpthread[i] != NULL)
+                    {
+                        mpthread[i]->join();
+                        delete mpthread[i];
+                        mpthread[i] = NULL;
+                    }
+                    iv::Reloc xReloc;
+                    xReloc.x_guess = xtrace[j].x;
+                    xReloc.y_guess = xtrace[j].y;
+                    xReloc.z_guess = xtrace[j].z;
+                    xReloc.yaw_guess = xtrace[j].yaw;
+                    xReloc.pitch_guess = 0;
+                    xReloc.roll_guess = 0;
+                    if(j == ntracesize)bHaveTask = false;
+    //                std::thread * pthread = new std::thread(&GlobalRelocation::threadtest,this,i,xReloc);
+                    mthreadcomplete[i] = false;
+                    mpthread[i] = new std::thread(&GlobalRelocation::threadReloc,this,i,&mndt[i],raw_scan,xReloc);
+
+                    j++;
+                    if(j == ntracesize)bHaveTask = false;
+                }
+            }
+        }
+        else
+        {
+            for(i=0;i<mnThreadNum;i++)
+            {
+                if((mthreadcomplete[i] == true))
+                {
+                    if(mpthread[i] != NULL)
+                    {
+                        mpthread[i]->join();
+                        delete mpthread[i];
+                        mpthread[i] = NULL;
+                    }
+                }
+                else
+                {
+                    break;
+                }
+            }
+            if(i == mnThreadNum)
+            {
+                bComplete = true;
+                break;
+            }
+        }
+
+        if(bCheckComplete)
+        {
+            for(i=0;i<mvectorReloc.size();i++)
+            {
+                if(mvectorReloc[i].trans_prob >= 3.0)
+                {
+                    bHaveTask = false;
+                    std::cout<<" Complete. "<<std::endl;
+                }
+            }
+        }
+
+        std::this_thread::sleep_for(std::chrono::milliseconds(1));
+    }
+
+}
+
+
+iv::Reloc GlobalRelocation::pointreloc(pcl::PointCloud<pcl::PointXYZ>::Ptr raw_scan)
+{
+    iv::Reloc xreloczero;
+    xreloczero.trans_prob = 0.0;
+
+    if(mvectorndtmap.size() > 1)
+    {
+        std::cout<<" only one map, can use relocation."<<std::endl;
+
+        return xreloczero;
+    }
+
+    if(mvectorndtmap.size() == 0)
+    {
+        std::cout<<" no map."<<std::endl;
+        return xreloczero;
+    }
+
+    mvectorReloc.clear();
+
+    int i;
+    int j;
+    for(i=0;i<mnThreadNum;i++)
+    {
+        mthreadcomplete[i] = true;
+    }
+
+    std::vector<iv::ndttracepoint> xtrace = mvectorseedpoint;
+
+    RunReloc(xtrace,raw_scan,false);
+
+    int nrelocsize = mvectorReloc.size();
+    int indexmax = 0;
+    double fMaxProb = 0.0;
+    for(i=0;i<nrelocsize;i++)
+    {
+        if(mvectorReloc[i].trans_prob>fMaxProb)
+        {
+            fMaxProb  = mvectorReloc[i].trans_prob;
+            indexmax = i;
+        }
+    }
+
+    if(fMaxProb > 2.0)
+    {
+
+        return mvectorReloc[indexmax];
+    }
+
+    std::cout<<" use diff yaw"<<std::endl;
+
+    std::vector<iv::ndttracepoint> xtraceraw = mvectorseedpoint;
+    xtrace.clear();
+
+    for(i=0;i<xtraceraw.size();i++)
+    {
+        iv::ndttracepoint ntp = xtraceraw[i];
+
+        for(j=1;j<mnCirclDivide;j++)
+        {
+            double yawadd = M_PI*2.0/mnCirclDivide;
+            yawadd = yawadd * j;
+            iv::ndttracepoint ntpyaw = ntp;
+            ntpyaw.yaw = ntp.yaw + yawadd;
+            if(ntpyaw.yaw >= 2.0*M_PI)ntpyaw.yaw = ntpyaw.yaw - 2.0*M_PI;
+            xtrace.push_back(ntpyaw);
+        }
+    }
+
+
+    RunReloc(xtrace,raw_scan,false);
+
+    nrelocsize = mvectorReloc.size();
+    indexmax = 0;
+    fMaxProb = 0.0;
+    for(i=0;i<nrelocsize;i++)
+    {
+        if(mvectorReloc[i].trans_prob>fMaxProb)
+        {
+            fMaxProb  = mvectorReloc[i].trans_prob;
+            indexmax = i;
+        }
+    }
+
+    if(fMaxProb > 2.0)
+    {
+        return mvectorReloc[indexmax];
+    }
+
+    std::cout<<" use dis yaw"<<std::endl;
+
+    xtraceraw = mvectorseedpoint;
+    xtrace.clear();
+
+    int k;
+    for(k=1;k<10;k++)
+    {
+    for(i=0;i<xtraceraw.size();i++)
+    {
+        iv::ndttracepoint ntpraw = xtraceraw[i];
+        iv::ndttracepoint ntp = ntpraw;
+        ntp.x = ntpraw.x + k*mfSeedDisThresh * cos(ntpraw.yaw + M_PI/2.0);
+        ntp.y = ntpraw.y + k*mfSeedDisThresh * sin(ntpraw.yaw + M_PI/2.0);
+        for(j=0;j<mnCirclDivide;j++)
+        {
+            double yawadd = M_PI*2.0/mnCirclDivide;
+            yawadd = yawadd * j;
+            iv::ndttracepoint ntpyaw = ntp;
+            ntpyaw.yaw = ntp.yaw + yawadd;
+            if(ntpyaw.yaw >= 2.0*M_PI)ntpyaw.yaw = ntpyaw.yaw - 2.0*M_PI;
+            xtrace.push_back(ntpyaw);
+        }
+        ntp = ntpraw;
+        ntp.x = ntpraw.x + k*mfSeedDisThresh * cos(ntpraw.yaw - M_PI/2.0);
+        ntp.y = ntpraw.y + k*mfSeedDisThresh * sin(ntpraw.yaw - M_PI/2.0);
+        for(j=0;j<mnCirclDivide;j++)
+        {
+            double yawadd = M_PI*2.0/mnCirclDivide;
+            yawadd = yawadd * j;
+            iv::ndttracepoint ntpyaw = ntp;
+            ntpyaw.yaw = ntp.yaw + yawadd;
+            if(ntpyaw.yaw >= 2.0*M_PI)ntpyaw.yaw = ntpyaw.yaw - 2.0*M_PI;
+            xtrace.push_back(ntpyaw);
+        }
+    }
+    }
+
+
+    RunReloc(xtrace,raw_scan,true);
+
+    nrelocsize = mvectorReloc.size();
+    indexmax = 0;
+    fMaxProb = 0.0;
+    for(i=0;i<nrelocsize;i++)
+    {
+        if(mvectorReloc[i].trans_prob>fMaxProb)
+        {
+            fMaxProb  = mvectorReloc[i].trans_prob;
+            indexmax = i;
+        }
+    }
+
+    if(fMaxProb > 2.0)
+    {
+        std::cout<<" find pos."<<std::endl;
+        return mvectorReloc[indexmax];
+    }
+
+
+    if(mvectorReloc.size() > 0)
+    {
+        return mvectorReloc[indexmax];
+    }
+
+
+    return xreloczero;
+
+
+}
+
+void GlobalRelocation::setndtmap(std::vector<iv::ndtmaptrace> & xvectorndtmap)
+{
+    mvectorndtmap = xvectorndtmap;
+    if(mvectorndtmap.size() == 0)
+    {
+        return;
+    }
+    int i;
+    for(i=0;i<mnThreadNum;i++)
+    {
+        pcl::PointCloud<pcl::PointXYZ>::Ptr xmap;
+
+        xmap = boost::shared_ptr<pcl::PointCloud<pcl::PointXYZ>>(new pcl::PointCloud<pcl::PointXYZ>);
+
+
+        QTime xTime;
+        xTime.start();
+        if(0 == pcl::io::loadPCDFile(mvectorndtmap[0].mstrpcdpath.data(),*xmap))
+        {
+
+        }
+        else
+        {
+            std::cout<<"map size is 0"<<std::endl;
+            return;
+        }
+        mbRelocAvail = true;
+        mndt[i].setInputTarget(xmap);
+        mthreadcomplete[i] = true;
+    }
+
+
+    int npointsize = mvectorndtmap[0].mvector_trace.size();
+    if(npointsize == 0)
+    {
+        return;
+    }
+    mvectorseedpoint.clear();
+    std::cout<<" load trace seed."<<std::endl;
+    iv::ndttracepoint ntp = mvectorndtmap[0].mvector_trace[0];
+    mvectorseedpoint.push_back(ntp);
+
+    for(i=1;i<npointsize;i++)
+    {
+        iv::ndttracepoint ntpnow = mvectorndtmap[0].mvector_trace[i];
+        iv::ndttracepoint ntplast = mvectorseedpoint[mvectorseedpoint.size() -1];
+        double fdis = sqrt(pow(ntpnow.x - ntplast.x,2) + pow(ntpnow.y - ntplast.y,2));
+        double fyawdiff = ntpnow.yaw - ntplast.yaw;
+        while(fyawdiff<(-M_PI))fyawdiff = fyawdiff + 2.0*M_PI;
+        while(fyawdiff >(M_PI))fyawdiff = fyawdiff - 2.0*M_PI;
+        if(fdis>=mfSeedDisThresh)
+        {
+            mvectorseedpoint.push_back(ntpnow);
+        }
+        else
+        {
+            if(fabs(fyawdiff)>0.1)
+            {
+                mvectorseedpoint.push_back(ntpnow);
+            }
+            else
+            {
+                if(i== (npointsize -1))
+                {
+                    mvectorseedpoint.push_back(ntpnow);
+                }
+            }
+        }
+    }
+
+    if(mvectorseedpoint.size()>0)
+    {
+        const int nfrontadd = 3;
+        const int nrearadd  = 3;
+        iv::ndttracepoint ntpfirst = mvectorseedpoint[0];
+        iv::ndttracepoint ntplast = mvectorseedpoint[mvectorseedpoint.size() -1];
+        for(i=0;i<nfrontadd;i++)
+        {
+            iv::ndttracepoint ntpnow = ntpfirst;
+            double foff = (i+1)*mfSeedDisThresh;
+            ntpnow.x = ntpfirst.x + foff*cos(ntpfirst.yaw + M_PI);
+            ntpnow.y = ntpfirst.y + foff*sin(ntpfirst.yaw + M_PI);
+            mvectorseedpoint.insert(mvectorseedpoint.begin(),ntpnow);
+        }
+        for(i=0;i<nrearadd;i++)
+        {
+            iv::ndttracepoint ntpnow = ntplast;
+            double foff = (i+1)*mfSeedDisThresh;
+            ntpnow.x = ntplast.x + foff*cos(ntpfirst.yaw);
+            ntpnow.y = ntplast.y + foff*sin(ntpfirst.yaw );
+            mvectorseedpoint.push_back(ntpnow);
+        }
+    }
+
+//        for(i=0;i<mvectorseedpoint.size();i++)
+//        {
+//            mvectorseedpoint[i].yaw = mvectorseedpoint[i].yaw + M_PI;
+//            mvectorseedpoint[i].y = mvectorseedpoint[i].y + 10.0;
+//        }
+
+
+}

src/detection/detection_ndt_matching/globalrelocation.h

@@ -0,0 +1,76 @@
+#ifndef GLOBALRELOCATION_H
+#define GLOBALRELOCATION_H
+
+#include <ndt_cpu/NormalDistributionsTransform.h>
+
+#include <thread>
+#include <mutex>
+#include "ndt_matching.h"
+
+#define MAX_NDT 100
+
+
+namespace  iv {
+struct Reloc
+{
+    double x_guess;
+    double y_guess;
+    double z_guess;
+    double yaw_guess;
+    double pitch_guess;
+    double roll_guess;
+    double x_calc;
+    double y_calc;
+    double z_calc;
+    double yaw_calc;
+    double pitch_calc;
+    double roll_calc;
+    double trans_prob;
+};
+}
+
+class GlobalRelocation
+{
+public:
+    GlobalRelocation();
+
+    iv::Reloc pointreloc(pcl::PointCloud<pcl::PointXYZ>::Ptr raw_scan);
+
+    void setndtmap(std::vector<iv::ndtmaptrace> & xvectorndtmap);
+
+private:
+    cpu::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> mndt[MAX_NDT];
+
+    std::vector<iv::ndtmaptrace> mvectorndtmap;
+
+    std::vector<iv::Reloc> mvectorReloc;
+    std::mutex mmutexReloc;
+
+    std::vector<iv::ndttracepoint> mvectorseedpoint;
+
+    bool mbRelocAvail = false;
+
+
+
+    bool mthreadcomplete[MAX_NDT];
+
+
+
+    void threadtest(int n,iv::Reloc & xReloc);
+
+    void threadReloc(int nThread,cpu::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> * pndt,
+                     pcl::PointCloud<pcl::PointXYZ>::Ptr raw_scan,
+                     iv::Reloc xReloc);
+
+    void RunReloc(std::vector<iv::ndttracepoint> & xtrace,pcl::PointCloud<pcl::PointXYZ>::Ptr raw_scan,bool bCheckComplete);
+
+    int mnThreadNum = 8;
+
+    std::thread * mpthread[MAX_NDT];
+
+    const double mfSeedDisThresh = 3.0;
+    const int mnCirclDivide  = 20;
+
+};
+
+#endif // GLOBALRELOCATION_H

src/detection/detection_ndt_matching/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/detection/detection_ndt_matching/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/detection/detection_ndt_matching/main.cpp

@@ -0,0 +1,687 @@
+#include <QCoreApplication>
+
+#include "modulecomm.h"
+
+#include <pcl/io/pcd_io.h>
+#include <pcl/point_types.h>
+
+#include <QTime>
+#include <QDir>
+#include <QFile>
+
+#include <thread>
+
+#include "ivfault.h"
+#include "ivlog.h"
+#include "ndt_matching.h"
+#include "gpsimu.pb.h"
+#include "pcdmap.pb.h"
+
+#include "ivexit.h"
+#include "ivbacktrace.h"
+#include "xmlparam.h"
+#include "ivversion.h"
+
+#include "globalrelocation.h"
+
+extern bool gbuseanh;
+
+
+QTime gTime;    //a Time function. use calculate elapsed.
+
+int gndttime;    //ndt calculate time
+
+void * gpa,* gpb ,* gpc, * gpd;   //Modulecomm pointer
+
+void * gparelocate;
+
+
+void * gpapcdmap;
+
+iv::Ivfault *gfault = nullptr;   //fault diag
+iv::Ivlog *givlog = nullptr;   //log
+
+std::vector<iv::ndtmaptrace> gvector_trace;  //trace and map origin. use shift map
+
+iv::gpspos glastndtgpspos;    //last ndt pos(convert to gps)
+iv::gpspos gcurndtgpspos;     //cur ndt pos(conver to gps)
+iv::gpspos gcurgpsgpspos;    //cur gps pos
+bool gbGPSFix = false;   //GPS is Fix. If 300 times report rtk 6.
+int64_t gnLastGPSTime = 0; //ms
+
+std::thread * gpthread;   //State thread pointer.
+
+std::string gstr_lidarmsgname;   //lidar message name
+std::string gstr_gpsmsgname;   // gps message name
+std::string gstr_ndtposmsgname;  //ndtpos message name
+std::string gstr_ndtgpsposmsgname;   //gpspos message name
+std::string gstr_ndtmainsensormsgname;
+std::string gstr_arm_x;
+std::string gstr_arm_y;
+
+std::string gstr_yawcorrect;   //lidar yaw correct.
+std::string gstr_heightcorrect;  //gps height correct.
+double gyawcorrect = 0.0;        //lidar yaw correct.
+double gheightcorrect = 0.0;      //gps height correct.
+
+std::string gstr_savelastpos; //set save last pos. default true
+bool gbSaveLastPos = true;
+
+double garm_x = 0.0;
+double garm_y = 0.0;
+
+QFile * gpFileLastPos = 0;//Save Last Positin
+
+GlobalRelocation gGlobalRelocation;
+
+bool gbNeedReloc = false;
+
+bool gbEnableReloc = true; //if have rtk position, is false;
+
+/**
+ * @brief readtrace read trace
+ * @param pFile
+ * @return
+ */
+std::vector<iv::ndttracepoint> readtrace(QFile * pFile)
+{
+    std::vector<iv::ndttracepoint> ntp;
+    QByteArray ba;
+    ba = pFile->readLine();
+
+    do
+    {
+        QString str;
+        str = ba;
+        QStringList strlist;
+        strlist = str.split("\t");
+        if(strlist.size() == 6)
+        {
+            iv::ndttracepoint tp;
+            QString strx;
+            int j;
+            double fv[6];
+            for(j=0;j<6;j++)
+            {
+                strx = strlist.at(j);
+                fv[j] = strx.toDouble();
+            }
+            tp.x = fv[0];
+            tp.y = fv[1];
+            tp.z = fv[2];
+            tp.pitch = fv[3];
+            tp.roll = fv[4];
+            tp.yaw = fv[5];
+            ntp.push_back(tp);
+        }
+        else
+        {
+            qDebug("len is %d, %d",ba.length(),strlist.size());
+        }
+        ba = pFile->readLine();
+    }while(ba.length()>0);
+
+    return ntp;
+}
+
+/**
+ * @brief readndtorigin read .pcd file's origin gps position.
+ * @param pFile
+ * @param pnori
+ * @return
+ */
+int readndtorigin(QFile * pFile,iv::gpspos * pnori)
+{
+    int nrtn = -1;
+    QByteArray ba;
+    ba = pFile->readLine();
+    QString str;
+    str = ba;
+    QStringList strlist;
+    strlist = str.split("\t");
+    if(strlist.size() == 6)
+    {
+        QString xstr[6];
+        int i;
+        for(i=0;i<6;i++)xstr[i] = strlist.at(i);
+        pnori->lon = xstr[0].toDouble();
+        pnori->lat = xstr[1].toDouble();
+        pnori->height = xstr[2].toDouble();
+        pnori->heading = xstr[3].toDouble();
+        pnori->pitch = xstr[4].toDouble();
+        pnori->roll = xstr[5].toDouble();
+        nrtn = 0;
+    }
+    return nrtn;
+
+}
+
+/**
+ * @brief LoadLastPos Load last position
+ * @param strlastposfilepath
+ */
+static int LoadLastPos(const char * strlastposfilepath)
+{
+    int nres = -1;
+    QFile * xFile = new QFile();
+    xFile->setFileName(strlastposfilepath);
+    if(xFile->open(QIODevice::ReadWrite))
+    {
+        int nrtn = readndtorigin(xFile,&glastndtgpspos);
+        if(nrtn < 0)
+        {
+            givlog->warn("load last pos fail.");
+        }
+        else
+        {
+            nres = 1;
+        }
+        if(gbSaveLastPos)gpFileLastPos = xFile;
+        else
+        {
+            gpFileLastPos = 0;
+            xFile->close();
+        }
+    }
+
+    return nres;
+}
+
+/**
+ * @brief LoadTrace
+ */
+static void LoadTrace()
+{
+    std::string strpath = getenv("HOME");
+    strpath = strpath + "/map/";
+
+    std::string strlastposfile = strpath;
+    strlastposfile = strlastposfile + "lastpos.txt";
+    int nload = LoadLastPos(strlastposfile.data());
+
+    QString strpathdir = strpath.data();
+    QDir dir(strpath.data());
+    QStringList xfilter;
+    xfilter<<"*.pcd";
+    foreach(QString strfilename,dir.entryList(xfilter,QDir::Files))
+    {
+        qDebug(strfilename.toLatin1().data());
+        QString stroripath;
+        QString strtracepath;
+        QString strpcdpath;
+        stroripath = strfilename;
+        stroripath = stroripath.left(stroripath.length() - 4);
+        stroripath = strpathdir +  stroripath + ".ori";
+        strtracepath = strfilename;
+        strtracepath = strtracepath.left(strtracepath.length() - 4);
+        strtracepath = strpathdir +  strtracepath + ".txt";
+        strpcdpath = strfilename;
+        strpcdpath = strpathdir + strpcdpath;
+        QFile xFileori,xFile,xFilepcd;
+
+        xFileori.setFileName(stroripath);
+        xFilepcd.setFileName(strpcdpath);
+        xFile.setFileName(strtracepath);
+        if(xFileori.exists() && xFile.exists())
+        {
+            qDebug("file name ok.");
+            if(xFile.open(QIODevice::ReadOnly) && xFileori.open(QIODevice::ReadOnly))
+            {
+                std::vector<iv::ndttracepoint> xv =  readtrace(&xFile);
+                iv::gpspos xnori;
+                int nrtn = readndtorigin(&xFileori,&xnori);
+                if((xv.size() > 0)&&(nrtn == 0))
+                {
+                    iv::ndtmaptrace nmt;
+                    nmt.mvector_trace = xv;
+                    nmt.mstrpcdpath = strpcdpath.toLatin1().data();
+                    nmt.mndtorigin = xnori;
+                    gvector_trace.push_back(nmt);
+                    qDebug("this is ok.");
+                }
+
+            }
+            xFile.close();
+            xFileori.close();
+
+        }
+        else
+        {
+            qDebug(" file not ok.");
+        }
+    }
+
+    if(nload != 1)
+    {
+        std::cout<<" no lastpos.txt file."<<std::endl;
+        if(gvector_trace.size() == 1)
+        {
+            std::cout<<" only one .pcd. so use .ori pos as start pos."<<std::endl;
+            glastndtgpspos = gvector_trace[0].mndtorigin;
+            std::cout<<" use ori pos as lastpos."<<std::endl;
+        }
+
+    }
+
+    return;
+}
+
+int gnothavedatatime = 0;
+/**
+ * @brief ListenPointCloud
+ * @param strdata
+ * @param nSize
+ * @param index
+ * @param dt
+ * @param strmemname
+ */
+void ListenPointCloud(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
+{
+
+    QTime xTime;
+    xTime.start();
+
+    gnothavedatatime = 0;
+    if(nSize <=16)return;
+    unsigned int * pHeadSize = (unsigned int *)strdata;
+    if(*pHeadSize > nSize)
+    {
+        std::cout<<"ListenPointCloud data is small headsize ="<<*pHeadSize<<"  data size is"<<nSize<<std::endl;
+    }
+
+
+    pcl::PointCloud<pcl::PointXYZ>::Ptr point_cloud(
+                new pcl::PointCloud<pcl::PointXYZ>());
+    int nNameSize;
+    nNameSize = *pHeadSize - 4-4-8;
+    char * strName = new char[nNameSize+1];strName[nNameSize] = 0;
+    memcpy(strName,(char *)((char *)strdata +4),nNameSize);
+    point_cloud->header.frame_id = strName;
+    memcpy(&point_cloud->header.seq,(char *)strdata+4+nNameSize,4);
+    memcpy(&point_cloud->header.stamp,(char *)strdata+4+nNameSize+4,8);
+    int nPCount = (nSize - *pHeadSize)/sizeof(pcl::PointXYZI);
+    int i;
+    pcl::PointXYZI * p;
+    p = (pcl::PointXYZI *)((char *)strdata + *pHeadSize);
+    for(i=0;i<nPCount;i++)
+    {
+        pcl::PointXYZ xp;
+        xp.x = p->y;
+        xp.y = p->x * (-1.0);
+        xp.z = p->z;
+
+        if(gyawcorrect != 0.0)
+        {
+            double yaw =  gyawcorrect;
+            double x1,y1;
+            x1 = xp.x;
+            y1 = xp.y;
+            xp.x = x1*cos(yaw) -y1*sin(yaw);
+            xp.y = x1*sin(yaw) + y1*cos(yaw);
+
+        }
+
+ //       xp.intensity = p->intensity;
+        point_cloud->push_back(xp);
+        p++;
+    }
+
+    givlog->verbose("point cloud size is %d",
+                    point_cloud->size());
+
+
+    if(gbNeedReloc && gbEnableReloc)
+    {
+        static int nRelocFail = 0;
+        iv::Reloc xreloc = gGlobalRelocation.pointreloc(point_cloud);
+        restartndtfailcount();
+        if(xreloc.trans_prob>=2.0)
+        {
+            pose posereloc;
+            posereloc.vx = 0;
+            posereloc.vy = 0;
+            posereloc.x = xreloc.x_calc;
+            posereloc.y = xreloc.y_calc;
+            posereloc.z = xreloc.z_calc;
+            posereloc.yaw = xreloc.yaw_calc;
+            posereloc.pitch = xreloc.pitch_calc;
+            posereloc.roll = xreloc.roll_calc;
+            setrelocpose(posereloc);
+        }
+        else
+        {
+            std::cout<<" reloc fail."<<std::endl;
+            nRelocFail++;
+        }
+        if(nRelocFail >= 3)
+        {
+            std::cout<<" more than 3 reloc fail. disable reloc."<<std::endl;
+            gbEnableReloc = false;
+        }
+        gbNeedReloc = false;
+    }
+ //
+
+    point_ndt(point_cloud);
+    gndttime = xTime.elapsed();
+    givlog->verbose("ndt time is %d, gtime is %d", xTime.elapsed(), gTime.elapsed());
+ //   return;
+
+
+ //   gw->UpdatePointCloud(point_cloud);
+ //   DBSCAN_PC(point_cloud);
+
+    gfault->SetFaultState(0, 0, "ok");
+}
+
+/**
+ * @brief ListenMapUpdate
+ * @param strdata
+ * @param nSize
+ * @param index
+ * @param dt
+ * @param strmemname
+ */
+void ListenMapUpdate(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
+{
+    char * strpath = new char[nSize+1];
+    memcpy(strpath,strdata,nSize);
+    strpath[nSize] = 0;
+
+    pcl::PointCloud<pcl::PointXYZ>::Ptr mapx = boost::shared_ptr<pcl::PointCloud<pcl::PointXYZ>>(new pcl::PointCloud<pcl::PointXYZ>);;
+    if(0 == pcl::io::loadPCDFile(strpath,*mapx))
+    {
+        int size = mapx->size();
+        givlog->verbose("mapx size = %d", size);
+        ndt_match_SetMap(mapx);
+
+    }
+
+    gfault->SetFaultState(0, 0, "ok");
+    delete strpath;
+}
+
+/**
+ * @brief ListenNDTRunstate
+ * @param strdata
+ * @param nSize
+ * @param index
+ * @param dt
+ * @param strmemname
+ */
+void ListenNDTRunstate(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
+{
+    bool bState;
+    if(nSize < sizeof(bool))
+    {
+        gfault->SetFaultState(1, 0, "ListenNDTRunstate data size is small");
+        givlog->error("ListenNDTRunstate data size is small");
+        return;
+    }
+    memcpy(&bState,strdata,sizeof(bool));
+
+    SetRunState(bState);
+    gfault->SetFaultState(0, 0, "ok");
+}
+
+
+void ListenRelocate(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
+{
+    gbNeedReloc = true;
+}
+
+/**
+ * @brief pausecomm
+ */
+void pausecomm()
+{
+    iv::modulecomm::PauseComm(gpa);
+    iv::modulecomm::PauseComm(gpb);
+    iv::modulecomm::PauseComm(gpc);
+    iv::modulecomm::PauseComm(gpd);
+    iv::modulecomm::PauseComm(gparelocate);
+    iv::modulecomm::PauseComm(gpapcdmap);
+}
+
+/**
+ * @brief continuecomm
+ */
+void continuecomm()
+{
+    iv::modulecomm::ContintuComm(gpa);
+    iv::modulecomm::ContintuComm(gpb);
+    iv::modulecomm::ContintuComm(gpc);
+    iv::modulecomm::ContintuComm(gpd);
+    iv::modulecomm::ContintuComm(gparelocate);
+    iv::modulecomm::ContintuComm(gpapcdmap);
+}
+
+/**
+ * @brief ListenRaw
+ * @param strdata
+ * @param nSize
+ * @param index
+ * @param dt
+ * @param strmemname
+ */
+void ListenRaw(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
+{
+    static unsigned int nFixCount = 0;
+    iv::gps::gpsimu xgpsimu;
+    if(!xgpsimu.ParseFromArray(strdata,nSize))
+    {
+        std::cout<<"ListenRaw Parse error."<<std::endl;
+    }
+
+    gcurgpsgpspos.lat = xgpsimu.lat();
+    gcurgpsgpspos.lon = xgpsimu.lon();
+    gcurgpsgpspos.height = xgpsimu.height() + gheightcorrect;
+    gcurgpsgpspos.heading = xgpsimu.heading();
+    gcurgpsgpspos.pitch = xgpsimu.pitch();
+    gcurgpsgpspos.roll = xgpsimu.roll();
+    gcurgpsgpspos.ve = xgpsimu.ve();
+    gcurgpsgpspos.vn = xgpsimu.vn();
+
+    givlog->verbose("gps lat:%11.7f lon:%11.7f heading:%11.7f height:%6.3f rtk:%d",
+                    xgpsimu.lat(),xgpsimu.lon(),xgpsimu.heading(),
+                    xgpsimu.height(),xgpsimu.rtk_state());
+
+    if(xgpsimu.has_acce_x())
+    {
+        setuseimu(true);
+        imu_update(xgpsimu.time(),
+                   xgpsimu.roll(),xgpsimu.pitch(),
+                   xgpsimu.heading(),xgpsimu.acce_x(),
+                   xgpsimu.acce_y(),xgpsimu.acce_z());
+    }
+
+
+    if(xgpsimu.rtk_state() == 6)
+    {
+        nFixCount++;
+        gbEnableReloc = false;
+    }
+    else
+    {
+        nFixCount = 0;
+    }
+
+    if(nFixCount < 300)gbGPSFix = false;
+    else gbGPSFix = true;
+
+    gnLastGPSTime = std::chrono::system_clock::now().time_since_epoch().count()/1000000;
+
+}
+
+
+bool gbstate = true;
+
+/**
+ * @brief statethread Monitor ndt state thread.
+ */
+void statethread()
+{
+    int nstate = 0;
+    int nlaststate = 0;
+    while(gbstate)
+    {
+        std::this_thread::sleep_for(std::chrono::milliseconds(10));
+        if(gnothavedatatime < 100000) gnothavedatatime++;
+
+        if(gnothavedatatime < 100)
+        {
+            nstate = 0;
+        }
+        if(gnothavedatatime > 1000)
+        {
+            nstate = 1;
+        }
+        if(gnothavedatatime > 6000)
+        {
+            nstate = 2;
+        }
+        if(nstate != nlaststate)
+        {
+            nlaststate = nstate;
+            switch (nstate) {
+            case 0:
+                givlog->info("ndt matching  is ok");
+                gfault->SetFaultState(0,0,"data is ok.");
+                break;
+            case 1:
+                givlog->warn("more than 10 seconds not have lidar pointcloud.");
+                gfault->SetFaultState(1,1,"more than 10 seconds not have lidar pointcloud.");
+                break;
+            case 2:
+                givlog->error("more than 60 seconds not have lidar pointcloud.");
+                gfault->SetFaultState(2,2,"more than 60 seconds not have lidar pointcloud.");
+                break;
+            default:
+                break;
+            }
+        }
+    }
+}
+
+/**
+ * @brief exitfunc when receive exit command.
+ */
+void exitfunc()
+{
+    std::cout<<"enter exitfunc."<<std::endl;
+    gbstate = false;
+    gpthread->join();
+    iv::modulecomm::Unregister(gpa);
+    iv::modulecomm::Unregister(gpb);
+    iv::modulecomm::Unregister(gpc);
+    iv::modulecomm::Unregister(gpd);
+    iv::modulecomm::Unregister(gparelocate);
+    iv::modulecomm::Unregister(gpapcdmap);
+    if(gpFileLastPos != 0)gpFileLastPos->close();
+    std::cout<<"Complete exitfunc."<<std::endl;
+}
+
+void threadrelocation()
+{
+    while(gbstate)
+    {
+        std::this_thread::sleep_for(std::chrono::milliseconds(10));
+    }
+}
+
+int main(int argc, char *argv[])
+{
+    showversion("detection_ndt_matching");
+    RegisterIVBackTrace();
+    QCoreApplication a(argc, argv);
+
+    QString strpath = QCoreApplication::applicationDirPath();
+
+   if(argc < 2)
+       strpath = strpath + "/detection_ndt_matching.xml";
+   else
+       strpath = argv[1];
+   std::cout<<strpath.toStdString()<<std::endl;
+
+   iv::xmlparam::Xmlparam xparam(strpath.toStdString());
+
+    gpapcdmap = iv::modulecomm::RegisterSend("pcdmap",1000,1);
+
+   gstr_lidarmsgname = xparam.GetParam("lidarmsg","lidar_pc");
+   gstr_gpsmsgname = xparam.GetParam("gpsmsg","hcp2_gpsimu");
+   gstr_ndtposmsgname = xparam.GetParam("ndtposmsg","ndtpos");
+   gstr_ndtgpsposmsgname = xparam.GetParam("ndtgpsposmsg","ndtgpspos");
+   gstr_ndtmainsensormsgname = xparam.GetParam("mainsensor","gps");
+
+   gstr_yawcorrect = xparam.GetParam("AngleCorrect","0.0");
+   gstr_heightcorrect = xparam.GetParam("HeightCorrect","0.0");
+   gstr_savelastpos = xparam.GetParam("SaveLastPos","true");
+
+   gstr_arm_x = xparam.GetParam("Arm_LidarGPSBase_x","0.0");
+   gstr_arm_y = xparam.GetParam("Arm_LidarGPSBase_y","0.0");
+
+   gbuseanh = xparam.GetParam("useanh",false);
+
+   gyawcorrect = atof(gstr_yawcorrect.data()) * M_PI/180.0;
+   gheightcorrect = atof(gstr_heightcorrect.data());
+
+   if(gstr_savelastpos == "true")gbSaveLastPos = true;
+   else gbSaveLastPos = false;
+
+   garm_x = atof(gstr_arm_x.data());
+   garm_y = atof(gstr_arm_y.data());
+
+
+   std::cout<<"lidar message is "<<gstr_lidarmsgname<<std::endl;
+   std::cout<<"gps message is "<<gstr_gpsmsgname<<std::endl;
+   std::cout<<"ndtpos message is "<<gstr_ndtposmsgname<<std::endl;
+   std::cout<<"ndtgpspos message is "<<gstr_ndtgpsposmsgname<<std::endl;
+   std::cout<<"AngleCorrect  is "<<gstr_yawcorrect<<std::endl;
+   std::cout<<"HeightCorrect is "<<gstr_heightcorrect<<std::endl;
+   std::cout<<"SaveLastPos is "<<gstr_savelastpos<<std::endl;
+   std::cout<<"Arm_LidarGPSBase_x is "<<gstr_arm_x<<std::endl;
+   std::cout<<"Arm_LidarGPSBase_x is "<<gstr_arm_y<<std::endl;
+
+
+    gfault = new iv::Ivfault("detection_ndt_matching_gpu");
+    givlog = new iv::Ivlog("detection_ndt_matching_gpu");
+
+    gfault->SetFaultState(0,0,"Initialize.");
+
+    glastndtgpspos.lat = 39;
+    glastndtgpspos.lon = 119;
+    glastndtgpspos.heading = 0;
+    glastndtgpspos.height = 0;
+    glastndtgpspos.pitch = 0;
+    glastndtgpspos.roll = 0;
+    gcurndtgpspos = glastndtgpspos;
+
+    iv::gpspos xpos = glastndtgpspos;
+    xpos.lat = 39.1;
+    xpos.heading = 90;
+    pose xp = CalcPose(glastndtgpspos,xpos);
+    iv::gpspos xx = PoseToGPS(glastndtgpspos,xp);
+    LoadTrace();
+    std::cout<<"trace size  is "<<gvector_trace.size()<<std::endl;
+
+    gGlobalRelocation.setndtmap(gvector_trace);
+
+    gpa = iv::modulecomm::RegisterRecv(gstr_lidarmsgname.data(),ListenPointCloud);
+    gpb = iv::modulecomm::RegisterRecv(gstr_gpsmsgname.data(),ListenRaw);
+    gpc = iv::modulecomm::RegisterSend(gstr_ndtposmsgname.data(),10000,1);
+    gpd = iv::modulecomm::RegisterSend(gstr_ndtgpsposmsgname.data(),10000,1);
+    gparelocate = iv::modulecomm::RegisterRecv("relocate",ListenRelocate);
+
+    ndt_match_Init_nomap();
+
+    iv::ivexit::RegIVExitCall(exitfunc);
+
+
+    gpthread = new std::thread(statethread);
+//    gpb = iv::modulecomm::RegisterRecv("ndt_mappath",ListenMapUpdate);
+ //   gpc = iv::modulecomm::RegisterRecv("ndt_runstate",ListenNDTRunstate);
+
+
+
+    return a.exec();
+}

src/detection/detection_ndt_matching/ndt_matching.cpp

@@ -0,0 +1,1968 @@
+/*
+ * Copyright 2015-2019 Autoware Foundation. All rights reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+/*
+ Localization program using Normal Distributions Transform
+
+ Yuki KITSUKAWA
+ */
+
+#include <pthread.h>
+#include <chrono>
+#include <fstream>
+#include <iostream>
+#include <memory>
+#include <sstream>
+#include <string>
+
+#include <thread>
+
+#include <boost/filesystem.hpp>
+
+
+#include <tf/tf.h>
+#include <tf/transform_broadcaster.h>
+#include <tf/transform_datatypes.h>
+#include <tf/transform_listener.h>
+
+#include <pcl/io/io.h>
+#include <pcl/io/pcd_io.h>
+#include <pcl/point_types.h>
+//#include <pcl_conversions/pcl_conversions.h>
+
+//#include <ndt_cpu/NormalDistributionsTransform.h>
+#include <pcl/registration/ndt.h>
+
+
+#include <pcl/io/pcd_io.h>
+#include <pcl/point_types.h>
+#include <pcl/point_cloud.h>
+#include <pcl/registration/ndt.h>
+#include <pcl/registration/gicp.h>
+#include <pcl/filters/voxel_grid.h>
+//#include <pcl/visualization/pcl_visualizer.h>
+
+#include <QFile>
+
+//#include <ndt_gpu/NormalDistributionsTransform.h>
+#include "ndtpos.pb.h"
+#include "ndtgpspos.pb.h"
+#include "pcdmap.pb.h"
+
+//#include <pcl_ros/point_cloud.h>
+//#include <pcl_ros/transforms.h>
+
+#include <ndt_cpu/NormalDistributionsTransform.h>
+
+#include "ndt_matching.h"
+
+#include "modulecomm.h"
+
+
+
+#include "gnss_coordinate_convert.h"
+
+//the following are UBUNTU/LINUX ONLY terminal color codes.
+#define RESET   "\033[0m"
+#define BLACK   "\033[30m"      /* Black */
+#define RED     "\033[31m"      /* Red */
+#define GREEN   "\033[32m"      /* Green */
+#define YELLOW  "\033[33m"      /* Yellow */
+#define BLUE    "\033[34m"      /* Blue */
+#define MAGENTA "\033[35m"      /* Magenta */
+#define CYAN    "\033[36m"      /* Cyan */
+#define WHITE   "\033[37m"      /* White */
+#define BOLDBLACK   "\033[1m\033[30m"      /* Bold Black */
+#define BOLDRED     "\033[1m\033[31m"      /* Bold Red */
+#define BOLDGREEN   "\033[1m\033[32m"      /* Bold Green */
+#define BOLDYELLOW  "\033[1m\033[33m"      /* Bold Yellow */
+#define BOLDBLUE    "\033[1m\033[34m"      /* Bold Blue */
+#define BOLDMAGENTA "\033[1m\033[35m"      /* Bold Magenta */
+#define BOLDCYAN    "\033[1m\033[36m"      /* Bold Cyan */
+#define BOLDWHITE   "\033[1m\033[37m"      /* Bold White */
+
+
+#define PREDICT_POSE_THRESHOLD 0.5
+
+#define Wa 0.4
+#define Wb 0.3
+#define Wc 0.3
+
+#include "ivfault.h"
+#include "ivlog.h"
+
+extern iv::Ivfault *gfault ;
+extern iv::Ivlog *givlog;
+
+extern void * gpapcdmap;
+
+static int gindex = 0;
+iv::lidar_pose glidar_pose;
+
+void * gpset;
+void * gppose;
+
+static bool g_hasmap = false;
+
+extern bool gbNeedReloc;
+
+enum class MethodType
+{
+  PCL_GENERIC = 0,
+  PCL_ANH = 1,
+  PCL_ANH_GPU = 2,
+  PCL_OPENMP = 3,
+};
+static MethodType _method_type = MethodType::PCL_GENERIC;
+
+static pose initial_pose, predict_pose, predict_pose_imu, predict_pose_odom, predict_pose_imu_odom, previous_pose,
+    ndt_pose, current_pose, current_pose_imu, current_pose_odom, current_pose_imu_odom, localizer_pose;
+
+static double offset_x, offset_y, offset_z, offset_yaw;  // current_pos - previous_pose
+static double offset_imu_x, offset_imu_y, offset_imu_z, offset_imu_roll, offset_imu_pitch, offset_imu_yaw;
+static double offset_odom_x, offset_odom_y, offset_odom_z, offset_odom_roll, offset_odom_pitch, offset_odom_yaw;
+static double offset_imu_odom_x, offset_imu_odom_y, offset_imu_odom_z, offset_imu_odom_roll, offset_imu_odom_pitch,
+    offset_imu_odom_yaw;
+
+// Can't load if typed "pcl::PointCloud<pcl::PointXYZRGB> map, add;"
+static pcl::PointCloud<pcl::PointXYZ> map, add;
+
+// If the map is loaded, map_loaded will be 1.
+static int map_loaded = 0;
+static int _use_gnss = 1;
+static int init_pos_set = 0;
+
+bool gbuseanh = true;
+
+static pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> ndt;
+
+static cpu::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> anh_ndt;
+static cpu::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> glocalanh_ndt;
+
+static pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>::Ptr ndtraw(new pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>());
+static pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>::Ptr glocalndtraw(new pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>());
+
+// Default values
+static int max_iter = 30;        // Maximum iterations
+static float ndt_res = 1.0;      // Resolution
+static double step_size = 0.1;   // Step size
+static double trans_eps = 0.01;  // Transformation epsilon
+
+
+static double exe_time = 0.0;
+static bool has_converged;
+static int iteration = 0;
+static double fitness_score = 0.0;
+static double trans_probability = 0.0;
+
+static double diff = 0.0;
+static double diff_x = 0.0, diff_y = 0.0, diff_z = 0.0, diff_yaw;
+
+static double current_velocity = 0.0, previous_velocity = 0.0, previous_previous_velocity = 0.0;  // [m/s]
+static double current_velocity_x = 0.0, previous_velocity_x = 0.0;
+static double current_velocity_y = 0.0, previous_velocity_y = 0.0;
+static double current_velocity_z = 0.0, previous_velocity_z = 0.0;
+// static double current_velocity_yaw = 0.0, previous_velocity_yaw = 0.0;
+static double current_velocity_smooth = 0.0;
+
+static double current_velocity_imu_x = 0.0;
+static double current_velocity_imu_y = 0.0;
+static double current_velocity_imu_z = 0.0;
+
+static double current_accel = 0.0, previous_accel = 0.0;  // [m/s^2]
+static double current_accel_x = 0.0;
+static double current_accel_y = 0.0;
+static double current_accel_z = 0.0;
+// static double current_accel_yaw = 0.0;
+
+static double angular_velocity = 0.0;
+
+static int use_predict_pose = 0;
+
+
+static std::chrono::time_point<std::chrono::system_clock> matching_start, matching_end;
+
+
+static int _queue_size = 1000;
+
+
+static double predict_pose_error = 0.0;
+
+static double _tf_x, _tf_y, _tf_z, _tf_roll, _tf_pitch, _tf_yaw;
+static Eigen::Matrix4f tf_btol;
+
+static std::string _localizer = "velodyne";
+static std::string _offset = "linear";  // linear, zero, quadratic
+
+
+
+static bool _get_height = false;
+static bool _use_local_transform = false;
+static bool _use_imu = false;
+static bool _use_odom = false;
+static bool _imu_upside_down = false;
+static bool _output_log_data = false;
+
+static std::string _imu_topic = "/imu_raw";
+
+static std::ofstream ofs;
+static std::string filename;
+
+//static sensor_msgs::Imu imu;
+//static nav_msgs::Odometry odom;
+
+// static tf::TransformListener local_transform_listener;
+static tf::StampedTransform local_transform;
+
+static unsigned int points_map_num = 0;
+
+pthread_mutex_t mutex;
+
+pthread_mutex_t mutex_read;
+
+pthread_mutex_t mutex_pose;
+
+bool gbNeedGPSUpdateNDT = false;
+
+pcl::PointCloud<pcl::PointXYZ>::Ptr local_map_ptr;
+pcl::PointCloud<pcl::PointXYZ>::Ptr global_map_ptr;
+
+pose glastmappose;
+
+static double imu_angular_velocity_x=0;
+static double imu_angular_velocity_y=0;
+static double imu_angular_velocity_z=0;
+static double imu_linear_acceleration_x=0;
+static double imu_linear_acceleration_y=0;
+static double imu_linear_acceleration_z =0;
+
+extern QFile * gpFileLastPos;//Save Last Positin
+static bool gbFileNDTUpdate;
+
+extern double garm_x ;
+extern double garm_y ;
+
+static int gndtcalcfailcount = 0;
+static pose gPoseReloc;
+static bool gbPoseReloc = false;
+
+#include <QDateTime>
+
+//cv::Mat gmatimage;
+void ListenPoseSet(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
+{
+    iv::lidar::ndtpos pos;
+
+    if(false == pos.ParseFromArray(strdata,nSize))
+    {
+        std::cout<<" ndtpos parse error."<<std::endl;
+        return;
+    }
+
+    SetCurPose(pos.pose_x(),pos.pose_y(),pos.pose_yaw(),pos.pose_z(),pos.pose_pitch(),pos.pose_roll());
+
+
+}
+
+static void SetLocalMap()
+{
+    pcl::PointCloud<pcl::PointXYZ>::Ptr local_ptr(new pcl::PointCloud<pcl::PointXYZ>());
+
+    int nSize = global_map_ptr->size();
+
+    int i;
+    for(i=0;i<nSize;i++)
+    {
+        pcl::PointXYZ xp = global_map_ptr->at(i);
+        if(sqrt(pow(xp.x - current_pose.x,2)+pow(xp.y-current_pose.y,2)+pow(xp.z-current_pose.z,2))<30)
+        {
+            local_ptr->push_back(xp);
+        }
+    }
+
+    glastmappose = current_pose;
+
+    local_map_ptr = local_ptr;
+    std::cout<<"current map size is "<<local_map_ptr->size()<<std::endl;
+}
+
+static bool gbNDTRun = true;
+
+static bool gbGFRun = true;
+static bool gbLMRun= true;
+static std::thread * gpmapthread, * gpgpsfixthread,*gpsavelastthread;
+static bool gbNeedUpdate = false;
+
+extern iv::gpspos glastndtgpspos;
+extern iv::gpspos gcurndtgpspos;
+extern iv::gpspos gcurgpsgpspos;
+extern bool gbGPSFix;
+extern int64_t gnLastGPSTime;
+
+static bool gbgpsupdatendt = false;
+
+static int gusemapindex = -1;
+static int gcurmapindex = -1;
+
+extern std::vector<iv::ndtmaptrace> gvector_trace;
+
+extern void * gpa,* gpb ,* gpc, * gpd;
+
+iv::gpspos PoseToGPS(iv::gpspos xori,pose xpose)
+{
+    double x_o,y_o;
+    GaussProjCal(xori.lon,xori.lat,&x_o,&y_o);
+    double lon,lat;
+    double hdg_o = (90 - xori.heading)*M_PI/180.0;
+    double rel_x = xpose.x * cos(hdg_o) - xpose.y * sin(hdg_o);
+    double rel_y = xpose.x * sin(hdg_o) + xpose.y * cos(hdg_o);
+    GaussProjInvCal(x_o + rel_x,y_o + rel_y,&lon,&lat);
+    double hdg_c = hdg_o + xpose.yaw;
+
+    hdg_c = M_PI/2.0 - hdg_c;
+    double heading = hdg_c * 180.0/M_PI;
+    while(heading < 0)heading = heading + 360;
+    while(heading >360)heading = heading - 360;
+    iv::gpspos xgpspos;
+    xgpspos.lon = lon;
+    xgpspos.lat = lat;
+    xgpspos.height = xpose.z + xori.height;
+    xgpspos.heading = heading;
+    xgpspos.pitch = xpose.pitch*180.0/M_PI  + xori.pitch;
+    xgpspos.roll  = xpose.roll*180.0/M_PI + xori.roll;
+    xgpspos.ve = xpose.vx * cos(hdg_o) - xpose.vy * sin(hdg_o);
+    xgpspos.vn = xpose.vx * sin(hdg_o) + xpose.vy * cos(hdg_o);
+
+
+//    std::cout<< " ori heading :  "<<xori.heading<<std::endl;
+
+//    std::cout<<" pose yaw: "<<xpose.yaw<<std::endl;
+
+    if((garm_x != 0)||(garm_y != 0)) //arm use to Convert pos to GPS Posision
+    {
+        GaussProjCal(xgpspos.lon,xgpspos.lat,&x_o,&y_o);
+        hdg_o = (90 - xgpspos.heading)*M_PI/180.0;
+        rel_x = garm_x *(-1) * cos(hdg_o) - garm_y *(-1) * sin(hdg_o);
+        rel_y = garm_x *(-1) * sin(hdg_o) + garm_y *(-1) * cos(hdg_o);
+        GaussProjInvCal(x_o + rel_x,y_o + rel_y,&lon,&lat);
+        xgpspos.lon = lon;
+        xgpspos.lat = lat;
+    }
+    return xgpspos;
+
+}
+
+pose CalcPose(iv::gpspos xori, iv::gpspos xcur)
+{
+    double lon,lat;
+    if((garm_x != 0)||(garm_y != 0)) //arm use to Convert pos to GPS Posision
+    {
+        double x_o,y_o;
+        GaussProjCal(xcur.lon,xcur.lat,&x_o,&y_o);
+        double hdg_o = (90 - xcur.heading)*M_PI/180.0;
+        double rel_x = garm_x *(1) * cos(hdg_o) - garm_y *(1) * sin(hdg_o);
+        double rel_y = garm_x *(1) * sin(hdg_o) + garm_y *(1) * cos(hdg_o);
+        GaussProjInvCal(x_o + rel_x,y_o + rel_y,&lon,&lat);
+        xcur.lon = lon;
+        xcur.lat = lat;
+    }
+    double x_o,y_o,hdg_o;
+    double x_c,y_c,hdg_c;
+    GaussProjCal(xori.lon,xori.lat,&x_o,&y_o);
+    GaussProjCal(xcur.lon,xcur.lat,&x_c,&y_c);
+    double dis = sqrt(pow(x_c-x_o,2)+ pow(y_c-y_o,2));
+    double rel_x0,rel_y0;
+    rel_x0 = x_c -x_o;
+    rel_y0 = y_c -y_o;
+    double rel_x,rel_y;
+
+    hdg_o = (90 - xori.heading)*M_PI/180.0;
+    hdg_c = (90 - xcur.heading)*M_PI/180.0;
+    rel_x = rel_x0 * cos(-hdg_o) - rel_y0 * sin(-hdg_o);
+    rel_y = rel_x0 * sin(-hdg_o) + rel_y0 * cos(-hdg_o);
+
+
+    double rel_hdg;
+    rel_hdg = hdg_c - hdg_o;
+    pose posex;
+    posex.x = rel_x;
+    posex.y = rel_y;
+    posex.z = xcur.height - xori.height;
+    posex.pitch = (xcur.pitch - xori.pitch)*M_PI/180.0;
+    posex.roll = (xcur.roll - xori.roll)*M_PI/180.0;
+    posex.yaw = rel_hdg;
+
+    posex.vx = xcur.ve * cos(-hdg_o) - xcur.vn * sin(-hdg_o);
+    posex.vy = xcur.ve * sin(-hdg_o) + xcur.vn * cos(-hdg_o);
+
+    return posex;
+
+}
+
+static double gettracedis(int index,pose posex)
+{
+    double fdismin = 1000000.;
+    double zdiff = 0;
+    int neari;
+    if(index < 0)return 1000000.0;
+    if(index>= gvector_trace.size())
+    {
+        return 1000000.0;
+    }
+    int i;
+    std::vector<iv::ndttracepoint> vt = gvector_trace.at(index).mvector_trace;
+    int nsize = vt.size();
+//    std::cout<<"size is "<<nsize<<std::endl;
+    for(i=0;i<nsize;i++)
+    {
+        double fdis = sqrt(pow(posex.x - vt.at(i).x,2) + pow(posex.y - vt.at(i).y,2));
+//        std::cout<<"fdis is "<<fdis<<std::endl;
+        if((fdis < fdismin) &&(fabs(posex.z - vt.at(i).z)<5))
+        {
+            fdismin = fdis;
+            neari = i;
+            zdiff = fabs(posex.z - vt.at(i).z);
+        }
+    }
+
+    return fdismin;
+}
+
+static void getmindistrace(int & index, double & ftracedis)
+{
+    double fdismin = 1000000.0;
+    int xindexmin = -1;
+    int i;
+    int nsize = gvector_trace.size();
+
+
+    for(i=0;i<nsize;i++)
+    {
+        pose posex = CalcPose(gvector_trace[i].mndtorigin,gcurndtgpspos);
+        double fdis = gettracedis(i,posex);
+        if(fdis<fdismin)
+        {
+            fdismin = fdis;
+            xindexmin = i;
+        }
+    }
+
+    if(xindexmin != -1)
+    {
+        ftracedis = fdismin;
+        index = xindexmin;
+    }
+    else
+    {
+        index = -1;
+        ftracedis = 100000.0;
+    }
+}
+
+#include <QTime>
+
+extern void pausecomm();
+extern void continuecomm();
+static void UseMap(int index)
+{
+    pcl::PointCloud<pcl::PointXYZ>::Ptr xmap;
+
+    xmap = boost::shared_ptr<pcl::PointCloud<pcl::PointXYZ>>(new pcl::PointCloud<pcl::PointXYZ>);
+
+
+
+
+    QTime xTime;
+    xTime.start();
+    if(0 == pcl::io::loadPCDFile(gvector_trace.at(index).mstrpcdpath.data(),*xmap))
+    {
+
+    }
+    else
+    {
+        std::cout<<"map size is 0"<<std::endl;
+        return;
+    }
+
+    qDebug("load map time is %d",xTime.elapsed());
+
+    pcl::PointCloud<pcl::PointXYZ>::Ptr map_ptr(new pcl::PointCloud<pcl::PointXYZ>(*xmap));
+//    gvectoranh.push_back(new_anh_gpu_ndt_ptr);
+
+    qDebug("ndt load finish time is %d",xTime.elapsed());
+
+    gcurmapindex = index;
+
+
+    if(gbuseanh)
+    {
+        cpu::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> localanhraw;
+
+        localanhraw.setResolution(ndt_res);
+        localanhraw.setInputTarget(map_ptr);
+        localanhraw.setMaximumIterations(max_iter);
+        localanhraw.setStepSize(step_size);
+        localanhraw.setTransformationEpsilon(trans_eps);
+
+        pthread_mutex_lock(&mutex);
+        //        ndt = glocal_ndt;
+        glocalanh_ndt = localanhraw;
+        pthread_mutex_unlock(&mutex);
+
+        gbNeedUpdate = true;
+    }
+    else
+    {
+
+        pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>::Ptr localndtraw(new pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>());
+
+        localndtraw->setResolution(ndt_res);
+        localndtraw->setInputTarget(map_ptr);
+        localndtraw->setMaximumIterations(max_iter);
+        localndtraw->setStepSize(step_size);
+        localndtraw->setTransformationEpsilon(trans_eps);
+
+        pthread_mutex_lock(&mutex);
+        //        ndt = glocal_ndt;
+        glocalndtraw = localndtraw;
+        pthread_mutex_unlock(&mutex);
+
+        gbNeedUpdate = true;
+    }
+
+
+    std::cout<<"change map, index is "<<index<<std::endl;
+}
+
+void LocalMapUpdate(int n)
+{
+    std::cout<<"LocalMap n is "<<n<<std::endl;
+
+    int index;
+    double ftracedis;
+    int ncurindex = -1;
+
+    int i;
+    for(i=0;i<gvector_trace.size();i++)
+    {
+//        UseMap(i);
+//        std::this_thread::sleep_for(std::chrono::milliseconds(1000));
+    }
+    while(gbLMRun)
+    {
+
+        bool bsharemap = false;
+
+        getmindistrace(index,ftracedis);
+
+        if(g_hasmap == false)
+        {
+
+            if(index >= 0)
+            {
+                if(ftracedis < 30)
+                {
+                    UseMap(index);
+                    ncurindex = index;
+                    g_hasmap = true;
+
+                    bsharemap = true;
+
+
+                }
+            }
+        }
+        else
+        {
+           if(index != ncurindex)
+           {
+                pose posex = CalcPose(gvector_trace[ncurindex].mndtorigin,gcurndtgpspos);
+               double fnowdis = gettracedis(ncurindex,posex);
+               if((fnowdis - ftracedis)>5)
+               {
+                   UseMap(index);
+                   ncurindex = index;
+                   g_hasmap = true;
+                   bsharemap = true;
+               }
+           }
+        }
+
+        if(ftracedis > 50)
+        {
+            std::cout<<std::chrono::system_clock::now().time_since_epoch().count()/1000000<<" trace dis is "<<ftracedis<<std::endl;
+            std::cout<<" Out Range."<<std::endl;
+            g_hasmap = false;
+        }
+
+
+        std::this_thread::sleep_for(std::chrono::milliseconds(100));
+
+        if(bsharemap)
+        {
+            std::this_thread::sleep_for(std::chrono::milliseconds(100));
+            iv::ndt::pcdmap xpcdmappath;
+            xpcdmappath.set_strpath(gvector_trace.at(index).mstrpcdpath);
+            int nbytesize = xpcdmappath.ByteSize();
+            std::shared_ptr<char> pstr_ptr = std::shared_ptr<char>(new char[nbytesize]);
+            if(xpcdmappath.SerializeToArray(pstr_ptr.get(),nbytesize))
+            {
+                std::cout<<" share pcd map info ."<<std::endl;
+                iv::modulecomm::ModuleSendMsg(gpapcdmap,pstr_ptr.get(),nbytesize);
+                std::this_thread::sleep_for(std::chrono::milliseconds(100));
+                iv::modulecomm::ModuleSendMsg(gpapcdmap,pstr_ptr.get(),nbytesize);
+            }
+            else
+            {
+                std::cout<<" pcdmap SerializeToArray faile. "<<std::endl;
+            }
+        }
+    }
+}
+
+
+void SaveMonitor(bool * pbRun)
+{
+    iv::gpspos xoldgpspos;
+    xoldgpspos.lat = 39;
+    xoldgpspos.lon = 117;
+    while(*pbRun)
+    {
+       if(gpFileLastPos != 0)
+       {
+           if(gbFileNDTUpdate)
+           {
+               if((fabs(xoldgpspos.lat - gcurndtgpspos.lat)>0.0000005)||((fabs(xoldgpspos.lon - gcurndtgpspos.lon)>0.0000005)))
+               {
+                   xoldgpspos = gcurndtgpspos;
+                   char str[1000];
+                   snprintf(str,1000,"%11.7f\t%11.7f\t%9.3f\t%6.2f\t%6.2f\t%6.2f\n                ",
+                            xoldgpspos.lon,xoldgpspos.lat,xoldgpspos.height,
+                            xoldgpspos.heading,xoldgpspos.pitch,xoldgpspos.roll);
+                   gpFileLastPos->seek(0);
+                   gpFileLastPos->write(str,strnlen(str,1000));
+                   gpFileLastPos->flush();
+               }
+               gbFileNDTUpdate = false;
+           }
+       }
+       std::this_thread::sleep_for(std::chrono::milliseconds(100));
+    }
+}
+
+void GPSPosMonitor(bool * pbRun)
+{
+    if(gbGPSFix == false)
+    {
+        gcurndtgpspos =  glastndtgpspos;
+    }
+    while(*pbRun)
+    {
+        int64_t nmsnow = std::chrono::system_clock::now().time_since_epoch().count()/1000000;
+        if(abs(nmsnow - gnLastGPSTime)>1000)
+        {
+            std::this_thread::sleep_for(std::chrono::milliseconds(10));
+            continue;
+        }
+        if(gbGPSFix == true)
+        {
+            double x0,y0;
+            double x,y;
+            GaussProjCal(gcurndtgpspos.lon,gcurndtgpspos.lat,&x0,&y0);
+            GaussProjCal(gcurgpsgpspos.lon,gcurgpsgpspos.lat,&x,&y);
+            double dis = sqrt(pow(x-x0,2)+ pow(y-y0,2));
+            double headdiff = fabs(gcurgpsgpspos.heading - gcurndtgpspos.heading);
+            double zdiff = fabs(gcurgpsgpspos.height - gcurndtgpspos.height);
+            if((dis> 10)||((headdiff>10)&&(headdiff<350))||(zdiff>5)||(g_hasmap == false)||(gbNeedGPSUpdateNDT))
+            {
+                givlog->info("NDTFIX","gps fix ndt pos. dis:%f headdiff:%f zdiff:%f map %d ndt %d",
+                             dis,headdiff,zdiff,g_hasmap,gbNeedGPSUpdateNDT);
+                std::cout<<"gps fix ndt pos "<<" dis: "<<dis<<" headdiff: "<<headdiff<<" zdiff: "<<zdiff<<std::endl;
+//                gcurndtgpspos = gcurgpsgpspos;
+                  gbgpsupdatendt = true;
+//                current_velocity_x = 0;
+//                current_velocity_y = 0;
+//                current_velocity_z = 0;
+//                angular_velocity = 0;
+//                gbNeedGPSUpdateNDT = false;
+                if(g_hasmap == true)
+                {
+                    int index = gcurmapindex;
+                    if((index >=0)&&(index < gvector_trace.size()))
+                    {
+
+                    }
+                }
+            }
+//            else
+//            {
+//                if(gbgpsupdatendt)
+//                {
+//                    gcurndtgpspos = gcurgpsgpspos;
+//                    gbgpsupdatendt = true;
+//                    current_velocity_x = 0;
+//                    current_velocity_y = 0;
+//                    current_velocity_z = 0;
+//                    angular_velocity = 0;
+//                }
+//            }
+
+        }
+
+        std::this_thread::sleep_for(std::chrono::milliseconds(10));
+    }
+}
+
+
+void ndt_match_Init(pcl::PointCloud<pcl::PointXYZ>::Ptr mappcd)
+{
+    _tf_x = 0;
+    _tf_y = 0;
+    _tf_z = 0;
+    _tf_roll = 0;
+    _tf_pitch = 0;
+    _tf_yaw = 0;
+
+    Eigen::Translation3f tl_btol(_tf_x, _tf_y, _tf_z);                 // tl: translation
+    Eigen::AngleAxisf rot_x_btol(_tf_roll, Eigen::Vector3f::UnitX());  // rot: rotation
+    Eigen::AngleAxisf rot_y_btol(_tf_pitch, Eigen::Vector3f::UnitY());
+    Eigen::AngleAxisf rot_z_btol(_tf_yaw, Eigen::Vector3f::UnitZ());
+    tf_btol = (tl_btol * rot_z_btol * rot_y_btol * rot_x_btol).matrix();
+
+
+
+    init_pos_set = 1;
+
+    initial_pose.x = 0;
+    initial_pose.y = 0;
+    initial_pose.z = 0;
+    initial_pose.roll = 0;
+    initial_pose.pitch = 0;
+    initial_pose.yaw = 0;
+
+    localizer_pose.x = initial_pose.x;
+    localizer_pose.y = initial_pose.y;
+    localizer_pose.z = initial_pose.z;
+    localizer_pose.roll = initial_pose.roll;
+    localizer_pose.pitch = initial_pose.pitch;
+    localizer_pose.yaw = initial_pose.yaw;
+
+    previous_pose.x = initial_pose.x;
+    previous_pose.y = initial_pose.y;
+    previous_pose.z = initial_pose.z;
+    previous_pose.roll = initial_pose.roll;
+    previous_pose.pitch = initial_pose.pitch;
+    previous_pose.yaw = initial_pose.yaw;
+
+    current_pose.x = initial_pose.x;
+    current_pose.y = initial_pose.y;
+    current_pose.z = initial_pose.z;
+    current_pose.roll = initial_pose.roll;
+    current_pose.pitch = initial_pose.pitch;
+    current_pose.yaw = initial_pose.yaw;
+
+    current_velocity = 0;
+    current_velocity_x = 0;
+    current_velocity_y = 0;
+    current_velocity_z = 0;
+    angular_velocity = 0;
+
+
+
+
+//    ndt.align(*output_cloud, Eigen::Matrix4f::Identity());
+
+
+    pcl::PointCloud<pcl::PointXYZ>::Ptr map_ptr(new pcl::PointCloud<pcl::PointXYZ>(*mappcd));
+
+
+    std::cout<<"map size is"<<mappcd->size()<<std::endl;
+    std::cout<<"ptr size is "<<map_ptr->size()<<std::endl;;
+
+    global_map_ptr = map_ptr;
+
+ //   SetLocalMap();
+
+
+//    pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_scan(new pcl::PointCloud<pcl::PointXYZ>());
+
+//    pcl::VoxelGrid<pcl::PointXYZ> voxel_grid_filter;
+//    voxel_grid_filter.setLeafSize(1.0,1.0,1.0);
+//    voxel_grid_filter.setInputCloud(map_ptr);
+//    voxel_grid_filter.filter(*filtered_scan);
+
+//    std::cout<<"filter map  size is "<<filtered_scan->size()<<std::endl;;
+
+ //   ndt.setInputTarget(map_ptr);
+//    pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud(new pcl::PointCloud<pcl::PointXYZ>);
+//    ndt.align(*output_cloud, Eigen::Matrix4f::Identity());
+
+
+    gpset = iv::modulecomm::RegisterRecv("ndtposeset",ListenPoseSet);
+    gppose = iv::modulecomm::RegisterSend("ndtpose",10000,3);
+
+    if(map_ptr->size() == 0)
+    {
+        gbNDTRun = false;
+        return;
+    }
+
+
+
+    pcl::PointCloud<pcl::PointXYZ>::Ptr dummy_scan_ptr(new pcl::PointCloud<pcl::PointXYZ>());
+    pcl::PointXYZ dummy_point;
+    dummy_scan_ptr->push_back(dummy_point);
+
+
+
+
+
+    if(map_ptr->size()>0)map_loaded = 1;
+
+ //   gpmapthread = new std::thread(LocalMapUpdate,1);
+
+
+
+
+}
+
+void ndt_match_Init_nomap()
+{
+    _tf_x = 0;
+    _tf_y = 0;
+    _tf_z = 0;
+    _tf_roll = 0;
+    _tf_pitch = 0;
+    _tf_yaw = 0;
+
+    Eigen::Translation3f tl_btol(_tf_x, _tf_y, _tf_z);                 // tl: translation
+    Eigen::AngleAxisf rot_x_btol(_tf_roll, Eigen::Vector3f::UnitX());  // rot: rotation
+    Eigen::AngleAxisf rot_y_btol(_tf_pitch, Eigen::Vector3f::UnitY());
+    Eigen::AngleAxisf rot_z_btol(_tf_yaw, Eigen::Vector3f::UnitZ());
+    tf_btol = (tl_btol * rot_z_btol * rot_y_btol * rot_x_btol).matrix();
+
+
+
+    init_pos_set = 1;
+
+
+
+
+    initial_pose.x = 0;
+    initial_pose.y = 0;
+    initial_pose.z = 0;
+    initial_pose.roll = 0;
+    initial_pose.pitch = 0;
+    initial_pose.yaw = 0;
+
+    localizer_pose.x = initial_pose.x;
+    localizer_pose.y = initial_pose.y;
+    localizer_pose.z = initial_pose.z;
+    localizer_pose.roll = initial_pose.roll;
+    localizer_pose.pitch = initial_pose.pitch;
+    localizer_pose.yaw = initial_pose.yaw;
+
+    previous_pose.x = initial_pose.x;
+    previous_pose.y = initial_pose.y;
+    previous_pose.z = initial_pose.z;
+    previous_pose.roll = initial_pose.roll;
+    previous_pose.pitch = initial_pose.pitch;
+    previous_pose.yaw = initial_pose.yaw;
+
+    current_pose.x = initial_pose.x;
+    current_pose.y = initial_pose.y;
+    current_pose.z = initial_pose.z;
+    current_pose.roll = initial_pose.roll;
+    current_pose.pitch = initial_pose.pitch;
+    current_pose.yaw = initial_pose.yaw;
+
+    current_velocity = 0;
+    current_velocity_x = 0;
+    current_velocity_y = 0;
+    current_velocity_z = 0;
+    angular_velocity = 0;
+
+
+    gpmapthread = new std::thread(LocalMapUpdate,1);
+    gbGFRun = true;
+    gpgpsfixthread = new std::thread(GPSPosMonitor,&gbGFRun);
+    gpsavelastthread = new std::thread(SaveMonitor,&gbGFRun);
+
+
+
+
+}
+
+
+void ndt_match_SetMap(pcl::PointCloud<pcl::PointXYZ>::Ptr mappcd)
+{
+    if(mappcd->size() == 0 )return;
+
+    pcl::PointCloud<pcl::PointXYZ>::Ptr map_ptr(new pcl::PointCloud<pcl::PointXYZ>(*mappcd));
+
+
+
+
+    pcl::PointCloud<pcl::PointXYZ>::Ptr dummy_scan_ptr(new pcl::PointCloud<pcl::PointXYZ>());
+    pcl::PointXYZ dummy_point;
+    dummy_scan_ptr->push_back(dummy_point);
+
+
+
+
+
+
+
+    if(map_ptr->size()>0)map_loaded = 1;
+}
+
+
+
+static double wrapToPm(double a_num, const double a_max)
+{
+  if (a_num >= a_max)
+  {
+    a_num -= 2.0 * a_max;
+  }
+  return a_num;
+}
+
+static double wrapToPmPi(const double a_angle_rad)
+{
+  return wrapToPm(a_angle_rad, M_PI);
+}
+
+static double calcDiffForRadian(const double lhs_rad, const double rhs_rad)
+{
+  double diff_rad = lhs_rad - rhs_rad;
+  if (diff_rad >= M_PI)
+    diff_rad = diff_rad - 2 * M_PI;
+  else if (diff_rad < -M_PI)
+    diff_rad = diff_rad + 2 * M_PI;
+  return diff_rad;
+}
+
+
+static unsigned int g_seq_old = 0;
+
+#include <QTime>
+
+
+
+bool isfirst = true;
+
+#include <QMutex>
+std::vector<iv::imudata> gvectorimu;
+QMutex gMuteximu;
+
+static void lidar_imu_calc(qint64 current_lidar_time)
+{
+    int nsize;
+
+    int i;
+    gMuteximu.lock();
+    nsize = gvectorimu.size();
+    for(i=0;i<nsize;i++)
+    {
+        iv::imudata ximudata = gvectorimu[i];
+        double ximu_angular_velocity_x,ximu_angular_velocity_y,ximu_angular_velocity_z;
+        double ximu_linear_acceleration_x,ximu_linear_acceleration_y,ximu_linear_acceleration_z;
+        if(current_lidar_time >  ximudata.imutime)
+        {
+            ximu_angular_velocity_x = ximudata.imu_angular_velocity_x;
+            ximu_angular_velocity_y = ximudata.imu_angular_velocity_y;
+            ximu_angular_velocity_z = ximudata.imu_angular_velocity_z;
+            ximu_linear_acceleration_x = ximudata.imu_linear_acceleration_x;
+            ximu_linear_acceleration_y = ximudata.imu_linear_acceleration_y;
+            ximu_linear_acceleration_z = ximudata.imu_linear_acceleration_z;
+            qint64 current_time_imu = ximudata.imutime;
+//            givlog->verbose("NDT","imu time is %ld",current_time_imu);
+            static qint64 previous_time_imu = current_time_imu;
+            double diff_time = (current_time_imu - previous_time_imu);
+            diff_time = diff_time/1000.0;
+
+            if(diff_time < 0)diff_time = 0.000001;
+            if(diff_time > 0.1)diff_time = 0.1;
+
+            if(current_time_imu < previous_time_imu)
+            {
+
+                std::cout<<"current time is old "<<current_time_imu<<std::endl;
+                previous_time_imu = current_time_imu;
+                continue;
+            }
+
+            double diff_imu_roll = ximu_angular_velocity_x * diff_time;
+            double diff_imu_pitch = ximu_angular_velocity_y * diff_time;
+            double diff_imu_yaw = ximu_angular_velocity_z * diff_time;
+
+            current_pose_imu.roll += diff_imu_roll;
+            current_pose_imu.pitch += diff_imu_pitch;
+            current_pose_imu.yaw += diff_imu_yaw;
+
+            double accX1 = ximu_linear_acceleration_x;
+            double accY1 = std::cos(current_pose_imu.roll) * ximu_linear_acceleration_y -
+                           std::sin(current_pose_imu.roll) * ximu_linear_acceleration_z;
+            double accZ1 = std::sin(current_pose_imu.roll) * ximu_linear_acceleration_y +
+                           std::cos(current_pose_imu.roll) * ximu_linear_acceleration_z;
+
+            double accX2 = std::sin(current_pose_imu.pitch) * accZ1 + std::cos(current_pose_imu.pitch) * accX1;
+            double accY2 = accY1;
+            double accZ2 = std::cos(current_pose_imu.pitch) * accZ1 - std::sin(current_pose_imu.pitch) * accX1;
+
+            double accX = std::cos(current_pose_imu.yaw) * accX2 - std::sin(current_pose_imu.yaw) * accY2;
+            double accY = std::sin(current_pose_imu.yaw) * accX2 + std::cos(current_pose_imu.yaw) * accY2;
+            double accZ = accZ2;
+
+            offset_imu_x += current_velocity_imu_x * diff_time + accX * diff_time * diff_time / 2.0;
+            offset_imu_y += current_velocity_imu_y * diff_time + accY * diff_time * diff_time / 2.0;
+            offset_imu_z += current_velocity_imu_z * diff_time + accZ * diff_time * diff_time / 2.0;
+
+            current_velocity_imu_x += accX * diff_time;
+            current_velocity_imu_y += accY * diff_time;
+            current_velocity_imu_z += accZ * diff_time;
+
+            offset_imu_roll += diff_imu_roll;
+            offset_imu_pitch += diff_imu_pitch;
+            offset_imu_yaw += diff_imu_yaw;
+
+          //  guess_pose_imu.x = previous_pose.x + offset_imu_x;
+          //  guess_pose_imu.y = previous_pose.y + offset_imu_y;
+          //  guess_pose_imu.z = previous_pose.z + offset_imu_z;
+          //  guess_pose_imu.roll = previous_pose.roll + offset_imu_roll;
+          //  guess_pose_imu.pitch = previous_pose.pitch + offset_imu_pitch;
+          //  guess_pose_imu.yaw = previous_pose.yaw + offset_imu_yaw;
+
+            predict_pose_imu.x = previous_pose.x + offset_imu_x;
+            predict_pose_imu.y = previous_pose.y + offset_imu_y;
+            predict_pose_imu.z = previous_pose.z + offset_imu_z;
+            predict_pose_imu.roll = previous_pose.roll + offset_imu_roll;
+            predict_pose_imu.pitch = previous_pose.pitch + offset_imu_pitch;
+            predict_pose_imu.yaw = previous_pose.yaw + offset_imu_yaw;
+
+            givlog->verbose("NDT","imu x:%f y:%f z:%f yaw:%f",offset_imu_x,
+                            offset_imu_y,offset_imu_z,offset_imu_yaw);
+
+            previous_time_imu = current_time_imu;
+
+
+        }
+        else
+        {
+            break;;
+        }
+
+    }
+
+    if(i>0)
+    {
+        gvectorimu.erase(gvectorimu.begin(),gvectorimu.begin()+i);
+    }
+
+    gMuteximu.unlock();
+
+//    for(i=0;i<gvectorimu.size();i++)
+//    {
+//        iv::imudata ximudata = gvectorimu[i];
+//        double ximu_angular_velocity_x,ximu_angular_velocity_y,ximu_angular_velocity_z;
+//        double ximu_linear_acceleration_x,ximu_linear_acceleration_y,ximu_linear_acceleration_z;
+//        if(current_lidar_time >  ximudata.imutime)
+//        {
+
+//            gvectorimu.erase(gvectorimu.begin())
+//        }
+//    }
+}
+
+
+static void imu_calc(qint64 current_time_imu)
+{
+
+  static qint64 previous_time_imu = current_time_imu;
+  double diff_time = (current_time_imu - previous_time_imu);
+  diff_time = diff_time/1000.0;
+
+  if(current_time_imu < previous_time_imu)
+  {
+      std::cout<<"current time is old "<<current_time_imu<<std::endl;
+      return;
+  }
+
+  double diff_imu_roll = imu_angular_velocity_x * diff_time;
+  double diff_imu_pitch = imu_angular_velocity_y * diff_time;
+  double diff_imu_yaw = imu_angular_velocity_z * diff_time;
+
+  current_pose_imu.roll += diff_imu_roll;
+  current_pose_imu.pitch += diff_imu_pitch;
+  current_pose_imu.yaw += diff_imu_yaw;
+
+  double accX1 = imu_linear_acceleration_x;
+  double accY1 = std::cos(current_pose_imu.roll) * imu_linear_acceleration_y -
+                 std::sin(current_pose_imu.roll) * imu_linear_acceleration_z;
+  double accZ1 = std::sin(current_pose_imu.roll) * imu_linear_acceleration_y +
+                 std::cos(current_pose_imu.roll) * imu_linear_acceleration_z;
+
+  double accX2 = std::sin(current_pose_imu.pitch) * accZ1 + std::cos(current_pose_imu.pitch) * accX1;
+  double accY2 = accY1;
+  double accZ2 = std::cos(current_pose_imu.pitch) * accZ1 - std::sin(current_pose_imu.pitch) * accX1;
+
+  double accX = std::cos(current_pose_imu.yaw) * accX2 - std::sin(current_pose_imu.yaw) * accY2;
+  double accY = std::sin(current_pose_imu.yaw) * accX2 + std::cos(current_pose_imu.yaw) * accY2;
+  double accZ = accZ2;
+
+  offset_imu_x += current_velocity_imu_x * diff_time + accX * diff_time * diff_time / 2.0;
+  offset_imu_y += current_velocity_imu_y * diff_time + accY * diff_time * diff_time / 2.0;
+  offset_imu_z += current_velocity_imu_z * diff_time + accZ * diff_time * diff_time / 2.0;
+
+  current_velocity_imu_x += accX * diff_time;
+  current_velocity_imu_y += accY * diff_time;
+  current_velocity_imu_z += accZ * diff_time;
+
+  offset_imu_roll += diff_imu_roll;
+  offset_imu_pitch += diff_imu_pitch;
+  offset_imu_yaw += diff_imu_yaw;
+
+//  guess_pose_imu.x = previous_pose.x + offset_imu_x;
+//  guess_pose_imu.y = previous_pose.y + offset_imu_y;
+//  guess_pose_imu.z = previous_pose.z + offset_imu_z;
+//  guess_pose_imu.roll = previous_pose.roll + offset_imu_roll;
+//  guess_pose_imu.pitch = previous_pose.pitch + offset_imu_pitch;
+//  guess_pose_imu.yaw = previous_pose.yaw + offset_imu_yaw;
+
+  predict_pose_imu.x = previous_pose.x + offset_imu_x;
+  predict_pose_imu.y = previous_pose.y + offset_imu_y;
+  predict_pose_imu.z = previous_pose.z + offset_imu_z;
+  predict_pose_imu.roll = previous_pose.roll + offset_imu_roll;
+  predict_pose_imu.pitch = previous_pose.pitch + offset_imu_pitch;
+  predict_pose_imu.yaw = previous_pose.yaw + offset_imu_yaw;
+
+  givlog->verbose("NDT","imu x:%f y:%f z:%f yaw:%f",offset_imu_x,
+                  offset_imu_y,offset_imu_z,offset_imu_yaw);
+
+  previous_time_imu = current_time_imu;
+}
+
+
+void imu_update(qint64 current_time_imu,double imu_roll,double imu_pitch,double imu_yaw,
+                       double acceleration_x,double acceleration_y,
+                       double acceleration_z)
+{
+  // std::cout << __func__ << std::endl;
+
+// double imu_angular_velocity_x,imu_angular_velocity_y,imu_angular_velocity_z;
+
+  static double previous_time_imu = current_time_imu;
+  double diff_time = (current_time_imu - previous_time_imu);
+  diff_time = diff_time/1000.0;
+
+
+  imu_roll = imu_roll * M_PI/180.0;
+  imu_pitch = imu_pitch * M_PI/180.0;
+  imu_yaw = (-1.0)*imu_yaw * M_PI/180.0;
+
+  imu_yaw = imu_yaw + 2.0*M_PI;
+
+//  imu_pitch = 0;
+//  imu_roll = 0;
+
+  imu_roll = wrapToPmPi(imu_roll);
+  imu_pitch = wrapToPmPi(imu_pitch);
+  imu_yaw = wrapToPmPi(imu_yaw);
+
+  static double previous_imu_roll = imu_roll, previous_imu_pitch = imu_pitch, previous_imu_yaw = imu_yaw;
+  const double diff_imu_roll = imu_roll - previous_imu_roll;
+
+  const double diff_imu_pitch = imu_pitch - previous_imu_pitch;
+
+  double diff_imu_yaw;
+  if (fabs(imu_yaw - previous_imu_yaw) > M_PI)
+  {
+    if (imu_yaw > 0)
+      diff_imu_yaw = (imu_yaw - previous_imu_yaw) - M_PI * 2;
+    else
+      diff_imu_yaw = -M_PI * 2 - (imu_yaw - previous_imu_yaw);
+  }
+  else
+    diff_imu_yaw = imu_yaw - previous_imu_yaw;
+
+
+  imu_linear_acceleration_x = acceleration_x;
+//  imu_linear_acceleration_y = acceleration_y;
+//  imu_linear_acceleration_z = acceleration_z;
+  imu_linear_acceleration_y = 0;
+  imu_linear_acceleration_z = 0;
+
+  if (diff_time != 0)
+  {
+    imu_angular_velocity_x = diff_imu_roll / diff_time;
+    imu_angular_velocity_y = diff_imu_pitch / diff_time;
+    imu_angular_velocity_z = diff_imu_yaw / diff_time;
+  }
+  else
+  {
+    imu_angular_velocity_x = 0;
+    imu_angular_velocity_y = 0;
+    imu_angular_velocity_z = 0;
+  }
+
+  iv::imudata ximudata;
+  ximudata.imutime = current_time_imu;
+  ximudata.imu_linear_acceleration_x = imu_linear_acceleration_x;
+  ximudata.imu_linear_acceleration_y = imu_linear_acceleration_y;
+  ximudata.imu_linear_acceleration_z = imu_linear_acceleration_z;
+  ximudata.imu_angular_velocity_x = imu_angular_velocity_x;
+  ximudata.imu_angular_velocity_y = imu_angular_velocity_y;
+  ximudata.imu_angular_velocity_z = imu_angular_velocity_z;
+
+  gMuteximu.lock();
+  gvectorimu.push_back(ximudata);
+  gMuteximu.unlock();
+//  imu_calc(current_time_imu);
+
+  previous_time_imu = current_time_imu;
+  previous_imu_roll = imu_roll;
+  previous_imu_pitch = imu_pitch;
+  previous_imu_yaw = imu_yaw;
+}
+
+
+void restartndtfailcount()
+{
+    gndtcalcfailcount = 0;
+}
+
+void setrelocpose(pose xPose)
+{
+    gPoseReloc = xPose;
+    gbPoseReloc = true;
+}
+
+
+#ifdef TEST_CALCTIME
+int ncalc = 0;
+int gncalctotal = 0;
+#endif
+void point_ndt(pcl::PointCloud<pcl::PointXYZ>::Ptr raw_scan)
+{
+
+    static bool bNeedUpdateVel = false; //if gps update ndt, need update velocity
+    qint64 current_scan_time = raw_scan->header.stamp;
+    static qint64 old_scan_time = current_scan_time;
+    if(gbNDTRun == false)return;
+
+    bool bNotChangev = false;
+
+
+
+    if(gbgpsupdatendt == true)
+    {
+
+        std::cout<<std::chrono::system_clock::now().time_since_epoch().count()/1000000<<" update ndt pos use gps pos. "<<std::endl;
+        gcurndtgpspos = gcurgpsgpspos;
+//        gcurndtgpspos.pitch = 0; //not use gps pitch and roll
+//        gcurndtgpspos.roll = 0;
+        gbgpsupdatendt = false;
+        gbNeedGPSUpdateNDT = false;
+        current_velocity_x = 0;
+        current_velocity_y = 0;
+        current_velocity_z = 0;
+        angular_velocity = 0;
+        bNeedUpdateVel = true;
+
+//
+//
+//        gcurndtgpspos = gcurgpsgpspos;
+//        current_pose = CalcPose(gvector_trace[gusemapindex].mndtorigin,gcurndtgpspos);
+//       predict_pose_for_ndt = current_pose;
+        current_velocity_imu_x = 0;
+        current_velocity_imu_y = 0;
+        current_velocity_imu_z = 0;
+        gMuteximu.lock();
+        gvectorimu.erase(gvectorimu.begin(),gvectorimu.begin() + gvectorimu.size());
+        gMuteximu.unlock();
+        bNotChangev = true;
+        return;
+    }
+//    gbgpsupdatendt = false;
+
+    if(g_hasmap == false)
+    {
+        return;
+    }
+    if(gbNeedUpdate)
+    {
+        if(gbuseanh)
+        {
+            pthread_mutex_lock(&mutex);
+            anh_ndt = glocalanh_ndt;
+            pthread_mutex_unlock(&mutex);
+        }
+        else
+        {
+            pthread_mutex_lock(&mutex);
+            ndtraw = glocalndtraw;
+            pthread_mutex_unlock(&mutex);
+        }
+
+        gusemapindex = gcurmapindex;
+        gbNeedUpdate = false;
+
+    }
+
+
+
+
+
+    previous_pose = CalcPose(gvector_trace[gusemapindex].mndtorigin,gcurndtgpspos);
+
+    if(gbPoseReloc)
+    {
+        previous_pose = gPoseReloc;
+        gbPoseReloc = false;
+    }
+
+ //   std::cout<<" previos head: "<<previous_pose.yaw<<std::endl;
+    if(bNeedUpdateVel == true)
+    {
+        bNeedUpdateVel = false;
+        current_velocity_x = previous_pose.vx;
+        current_velocity_y = previous_pose.vy;
+        current_velocity_imu_x = current_velocity_x;
+        current_velocity_imu_y = current_velocity_y;
+    }
+    givlog->verbose("previos pose is %f %f",previous_pose.x,previous_pose.y);
+
+//    if(map_loaded == 0)
+//    {
+//        std::cout<<BOLDRED<<" point_ndt not set map."<<RESET<<std::endl;
+//        return;
+//    }
+
+
+
+    pthread_mutex_lock(&mutex_pose);
+
+    QTime xTime;
+    xTime.start();
+//    std::cout<<"time is "<<xTime.elapsed()<<std::endl;
+    double voxel_leaf_size = 2.0;
+    double measurement_range = 200.0;
+
+    pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_scan(new pcl::PointCloud<pcl::PointXYZ>());
+
+    pcl::VoxelGrid<pcl::PointXYZ> voxel_grid_filter;
+    voxel_grid_filter.setLeafSize(voxel_leaf_size, voxel_leaf_size, voxel_leaf_size);
+    voxel_grid_filter.setInputCloud(raw_scan);
+    voxel_grid_filter.filter(*filtered_scan);
+
+//    std::cout<<"voxed time is "<<xTime.elapsed()<<std::endl;
+
+//    std::cout<<" seq is "<<raw_scan->header.seq<<std::endl;
+  //  qDebug("seq = %d stamp = %d ",raw_scan->header.seq,raw_scan->header.stamp);
+  //  std::cout<<"raw scan size is "<<raw_scan->size()<<"  filtered scan size is "<<filtered_scan->size()<<std::endl;
+
+ //   return;
+    matching_start = std::chrono::system_clock::now();
+
+    pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_scan_ptr(new pcl::PointCloud<pcl::PointXYZ>(*filtered_scan));
+
+    int scan_points_num = filtered_scan_ptr->size();
+
+    Eigen::Matrix4f t(Eigen::Matrix4f::Identity());   // base_link
+    Eigen::Matrix4f t2(Eigen::Matrix4f::Identity());  // localizer
+
+    std::chrono::time_point<std::chrono::system_clock> align_start, align_end, getFitnessScore_start,
+        getFitnessScore_end;
+    static double align_time, getFitnessScore_time = 0.0;
+
+//    std::cout<<"run hear"<<std::endl;
+    pthread_mutex_lock(&mutex);
+
+//    if (_method_type == MethodType::PCL_GENERIC)
+ //   ndt.setInputSource(filtered_scan_ptr);
+
+
+    if(gbuseanh)
+    {
+#ifdef  TEST_CALCTIME
+        std::cout<<"use anh."<<std::endl;
+#endif
+        anh_ndt.setInputSource(filtered_scan_ptr);
+    }
+    else
+    {
+#ifdef  TEST_CALCTIME
+        std::cout<<"use ndt."<<std::endl;
+#endif
+        ndtraw->setInputSource(filtered_scan_ptr);
+    }
+
+    // Guess the initial gross estimation of the transformation
+//    double diff_time = (current_scan_time - previous_scan_time).toSec();
+
+
+    double diff_time = 0.0;
+
+    if(g_seq_old != 0)
+    {
+        if((raw_scan->header.seq - g_seq_old)>0)
+        {
+            diff_time = raw_scan->header.seq - g_seq_old;
+            diff_time = diff_time * 0.1;
+        }
+    }
+
+    g_seq_old = raw_scan->header.seq;
+
+    diff_time = current_scan_time -old_scan_time;
+    diff_time = diff_time/1000.0;
+    old_scan_time = current_scan_time;
+
+    if(diff_time<=0)diff_time = 0.1;
+    if(diff_time>1.0)diff_time = 0.1;
+
+//    std::cout<<"diff time is "<<diff_time<<std::endl;
+
+//    std::cout<<" diff time is "<<diff_time<<std::endl;
+
+//    diff_time = 0.0;
+
+//    if (_offset == "linear")
+//    {
+
+      if(diff_time<0.1)diff_time = 0.1;
+      offset_x = current_velocity_x * diff_time;
+      offset_y = current_velocity_y * diff_time;
+      offset_z = current_velocity_z * diff_time;
+      offset_yaw = angular_velocity * diff_time;
+//    }
+      predict_pose.x = previous_pose.x + offset_x;
+      predict_pose.y = previous_pose.y + offset_y;
+      predict_pose.z = previous_pose.z + offset_z;
+      predict_pose.roll = previous_pose.roll;
+      predict_pose.pitch = previous_pose.pitch;
+      predict_pose.yaw = previous_pose.yaw + offset_yaw;
+
+      pose predict_pose_for_ndt;
+
+
+      givlog->verbose("NDT","previous x:%f y:%f z:%f yaw:%f",previous_pose.x,
+                      previous_pose.y,previous_pose.z,previous_pose.yaw);
+//      if (_use_imu == true && _use_odom == true)
+//         imu_odom_calc(current_scan_time);
+       if (_use_imu == true && _use_odom == false)
+       {
+         lidar_imu_calc((current_scan_time-0));
+       }
+//       if (_use_imu == false && _use_odom == true)
+//         odom_calc(current_scan_time);
+
+//       if (_use_imu == true && _use_odom == true)
+//         predict_pose_for_ndt = predict_pose_imu_odom;
+//       else
+//           if (_use_imu == true && _use_odom == false)
+//         predict_pose_for_ndt = predict_pose_imu;
+//       else if (_use_imu == false && _use_odom == true)
+//         predict_pose_for_ndt = predict_pose_odom;
+//       else
+//         predict_pose_for_ndt = predict_pose;
+
+      if (_use_imu == true && _use_odom == false)
+      {
+         predict_pose_for_ndt = predict_pose_imu;
+ //         predict_pose_for_ndt = predict_pose;
+ //         predict_pose_for_ndt.yaw = predict_pose_imu.yaw;
+      }
+      else
+      {
+          predict_pose_for_ndt = predict_pose;
+      }
+
+      predict_pose_for_ndt = predict_pose; //not use predict_pose_imu in shelan.
+
+      if(fabs(predict_pose_for_ndt.x -previous_pose.x)>10)
+      {
+          predict_pose_for_ndt = previous_pose;
+      }
+
+      if(fabs(predict_pose_for_ndt.y -previous_pose.y)>10)
+      {
+          predict_pose_for_ndt = previous_pose;
+      }
+
+ //     predict_pose_for_ndt = predict_pose;
+
+
+
+//      predict_pose_for_ndt.pitch = 0;
+//      predict_pose_for_ndt.roll = 0;
+      givlog->verbose("NDT","pre x:%f y:%f z:%f yaw:%f",predict_pose_for_ndt.x,
+                      predict_pose_for_ndt.y,predict_pose_for_ndt.z,predict_pose_for_ndt.yaw);
+//      qDebug("pre x:%f y:%f z:%f yaw:%f pitch: %f roll: %f",predict_pose_for_ndt.x,
+//             predict_pose_for_ndt.y,predict_pose_for_ndt.z,predict_pose_for_ndt.yaw,predict_pose_for_ndt.pitch,predict_pose_for_ndt.roll);
+//      predict_pose_for_ndt = predict_pose;
+
+      Eigen::Translation3f init_translation(predict_pose_for_ndt.x, predict_pose_for_ndt.y, predict_pose_for_ndt.z);
+      Eigen::AngleAxisf init_rotation_x(predict_pose_for_ndt.roll, Eigen::Vector3f::UnitX());
+      Eigen::AngleAxisf init_rotation_y(predict_pose_for_ndt.pitch, Eigen::Vector3f::UnitY());
+      Eigen::AngleAxisf init_rotation_z(predict_pose_for_ndt.yaw, Eigen::Vector3f::UnitZ());
+      Eigen::Matrix4f init_guess = (init_translation * init_rotation_z * init_rotation_y * init_rotation_x) * tf_btol;
+
+      pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud(new pcl::PointCloud<pcl::PointXYZ>);
+
+ //     std::cout<<"before ndt time is "<<xTime.elapsed()<<std::endl;
+//      std::cout<<"time is "<<xTime.elapsed()<<std::endl;
+
+
+  //    std::cout<<" gues pose yaw: "<<predict_pose_for_ndt.yaw<<std::endl;
+      pcl::PointCloud<pcl::PointXYZ>::Ptr aligned(new pcl::PointCloud<pcl::PointXYZ>());
+      align_start = std::chrono::system_clock::now();
+      if(gbuseanh)
+      {
+        anh_ndt.align(*aligned,init_guess);
+      }
+      else
+        ndtraw->align(*aligned,init_guess);
+      align_end = std::chrono::system_clock::now();
+
+      if(xTime.elapsed()<90)
+      std::cout<<GREEN<<"after ndt time is "<<xTime.elapsed()<<RESET<<std::endl;
+      else
+          std::cout<<RED<<"after ndt time is "<<xTime.elapsed()<<RESET<<std::endl;
+
+
+#ifdef TEST_CALCTIME
+
+    gncalctotal = gncalctotal + xTime.elapsed();
+    ncalc++;
+    if(ncalc>0)
+    {
+        std::cout<<" average calc time: "<<gncalctotal/ncalc<<std::endl;
+
+    }
+#endif
+
+      if(gbuseanh)
+      {
+          has_converged = anh_ndt.hasConverged();
+          t = anh_ndt.getFinalTransformation();
+          iteration = anh_ndt.getFinalNumIteration();
+          getFitnessScore_start = std::chrono::system_clock::now();
+          fitness_score = anh_ndt.getFitnessScore();
+          getFitnessScore_end = std::chrono::system_clock::now();
+          trans_probability = anh_ndt.getTransformationProbability();
+      }
+      else
+      {
+          has_converged = ndtraw->hasConverged();
+          t = ndtraw->getFinalTransformation();
+          iteration = ndtraw->getFinalNumIteration();
+          getFitnessScore_start = std::chrono::system_clock::now();
+          fitness_score = ndtraw->getFitnessScore();
+          getFitnessScore_end = std::chrono::system_clock::now();
+          trans_probability = ndtraw->getTransformationProbability();
+      }
+
+
+      if(trans_probability >= 1.0)
+      {
+          gndtcalcfailcount = 0;
+      }
+      else
+      {
+          gndtcalcfailcount++;
+      }
+
+      if(gndtcalcfailcount >= 30)
+      {
+          gbNeedReloc = true;
+      }
+
+
+      std::cout<<"score: "<<fitness_score<<" trans_probability:"<<trans_probability<<std::endl;
+  //    std::cout<<" scoure is "<<fitness_score<<std::endl;
+
+//      std::cout<<" iter is "<<iteration<<std::endl;
+
+      t2 = t * tf_btol.inverse();
+
+      getFitnessScore_time =
+          std::chrono::duration_cast<std::chrono::microseconds>(getFitnessScore_end - getFitnessScore_start).count() /
+          1000.0;
+
+      pthread_mutex_unlock(&mutex);
+
+      tf::Matrix3x3 mat_l;  // localizer
+      mat_l.setValue(static_cast<double>(t(0, 0)), static_cast<double>(t(0, 1)), static_cast<double>(t(0, 2)),
+                     static_cast<double>(t(1, 0)), static_cast<double>(t(1, 1)), static_cast<double>(t(1, 2)),
+                     static_cast<double>(t(2, 0)), static_cast<double>(t(2, 1)), static_cast<double>(t(2, 2)));
+
+      // Update localizer_pose
+      localizer_pose.x = t(0, 3);
+      localizer_pose.y = t(1, 3);
+      localizer_pose.z = t(2, 3);
+      mat_l.getRPY(localizer_pose.roll, localizer_pose.pitch, localizer_pose.yaw, 1);
+
+
+//     std::cout<<" local pose x is "<<localizer_pose.x<<std::endl;
+
+      tf::Matrix3x3 mat_b;  // base_link
+      mat_b.setValue(static_cast<double>(t2(0, 0)), static_cast<double>(t2(0, 1)), static_cast<double>(t2(0, 2)),
+                     static_cast<double>(t2(1, 0)), static_cast<double>(t2(1, 1)), static_cast<double>(t2(1, 2)),
+                     static_cast<double>(t2(2, 0)), static_cast<double>(t2(2, 1)), static_cast<double>(t2(2, 2)));
+
+      // Update ndt_pose
+      ndt_pose.x = t2(0, 3);
+      ndt_pose.y = t2(1, 3);
+      ndt_pose.z = t2(2, 3);
+
+//      ndt_pose.x = localizer_pose.x;
+//      ndt_pose.y = localizer_pose.y;
+//      ndt_pose.z = localizer_pose.z;
+      mat_b.getRPY(ndt_pose.roll, ndt_pose.pitch, ndt_pose.yaw, 1);
+
+
+      givlog->verbose("NDT","calc x:%f y:%f z:%f yaw:%f",ndt_pose.x,
+                      ndt_pose.y,ndt_pose.z,ndt_pose.yaw);
+
+
+      // Calculate the difference between ndt_pose and predict_pose
+      predict_pose_error = sqrt((ndt_pose.x - predict_pose_for_ndt.x) * (ndt_pose.x - predict_pose_for_ndt.x) +
+                                (ndt_pose.y - predict_pose_for_ndt.y) * (ndt_pose.y - predict_pose_for_ndt.y) +
+                                (ndt_pose.z - predict_pose_for_ndt.z) * (ndt_pose.z - predict_pose_for_ndt.z));
+
+
+ //     std::cout<<" pose error is "<<predict_pose_error<<std::endl;
+ //     std::cout<<"ndt pose x is "<<ndt_pose.x<<std::endl;
+      if (predict_pose_error <= (PREDICT_POSE_THRESHOLD*10*diff_time))
+      {
+        use_predict_pose = 0;
+      }
+      else
+      {
+        use_predict_pose = 1;
+      }
+      use_predict_pose = 0;
+
+      if (use_predict_pose == 0)
+      {
+        current_pose.x = ndt_pose.x;
+        current_pose.y = ndt_pose.y;
+        current_pose.z = ndt_pose.z;
+        current_pose.roll = ndt_pose.roll;
+        current_pose.pitch = ndt_pose.pitch;
+        current_pose.yaw = ndt_pose.yaw;
+
+      }
+      else
+      {
+          givlog->verbose("NDT","USE PREDICT POSE");
+        current_pose.x = predict_pose_for_ndt.x;
+        current_pose.y = predict_pose_for_ndt.y;
+        current_pose.z = predict_pose_for_ndt.z;
+        current_pose.roll = predict_pose_for_ndt.roll;
+        current_pose.pitch = predict_pose_for_ndt.pitch;
+        current_pose.yaw = predict_pose_for_ndt.yaw;
+      }
+
+ //     std::cout<<" current pose x is "<<current_pose.x<<std::endl;
+
+      // Compute the velocity and acceleration
+      diff_x = current_pose.x - previous_pose.x;
+      diff_y = current_pose.y - previous_pose.y;
+      diff_z = current_pose.z - previous_pose.z;
+      diff_yaw = calcDiffForRadian(current_pose.yaw, previous_pose.yaw);
+      diff = sqrt(diff_x * diff_x + diff_y * diff_y + diff_z * diff_z);
+
+      current_velocity = (diff_time > 0) ? (diff / diff_time) : 0;
+      current_velocity_x = (diff_time > 0) ? (diff_x / diff_time) : 0;
+      current_velocity_y = (diff_time > 0) ? (diff_y / diff_time) : 0;
+      current_velocity_z = (diff_time > 0) ? (diff_z / diff_time) : 0;
+      angular_velocity = (diff_time > 0) ? (diff_yaw / diff_time) : 0;
+
+      current_pose.vx = current_velocity_x;
+      current_pose.vy = current_velocity_y;
+
+      current_velocity_smooth = (current_velocity + previous_velocity + previous_previous_velocity) / 3.0;
+      if (current_velocity_smooth < 0.2)
+      {
+        current_velocity_smooth = 0.0;
+      }
+
+ //     bNotChangev = true;
+      if((diff_time >  0)&&(bNotChangev == false))
+      {
+          double aa = (current_velocity -previous_velocity)/diff_time;
+          if(fabs(aa)>5.0)
+          {
+              givlog->verbose("NDT","aa is %f",aa);
+              aa = fabs(5.0/aa);
+              current_velocity = previous_velocity + aa*(current_velocity -previous_velocity);
+              current_velocity_x = previous_velocity_x + aa *((current_velocity_x -previous_velocity_x));
+              current_velocity_y = previous_velocity_y + aa *((current_velocity_y -previous_velocity_y));
+              current_velocity_z = previous_velocity_z + aa *((current_velocity_z -previous_velocity_z));
+
+          }
+      }
+
+      current_accel = (diff_time > 0) ? ((current_velocity - previous_velocity) / diff_time) : 0;
+      current_accel_x = (diff_time > 0) ? ((current_velocity_x - previous_velocity_x) / diff_time) : 0;
+      current_accel_y = (diff_time > 0) ? ((current_velocity_y - previous_velocity_y) / diff_time) : 0;
+      current_accel_z = (diff_time > 0) ? ((current_velocity_z - previous_velocity_z) / diff_time) : 0;
+
+ //   std::cout<<"fit time is "<<getFitnessScore_time<<std::endl;
+
+      gcurndtgpspos = PoseToGPS(gvector_trace[gcurmapindex].mndtorigin,current_pose);
+//      std::cout<<" pre x:"<<previous_pose.x<<" pre y:"<<previous_pose.y<<std::endl;
+    std::cout<<" x: "<<current_pose.x<<" y: "<<current_pose.y
+            <<" z: "<<current_pose.z<<" yaw:"<<current_pose.yaw
+           <<" vel: "<<current_velocity<<std::endl;
+
+
+    std::cout<<"NDT ve:"<<gcurndtgpspos.ve<<" vn:"<<gcurndtgpspos.vn
+            <<" GPS ve:"<<gcurgpsgpspos.ve<<" vn:"<<gcurgpsgpspos.vn<<std::endl;
+
+    gbFileNDTUpdate = true;
+
+//    gw->Updatexyyaw(current_pose.x,current_pose.y,current_pose.yaw);
+
+    current_pose_imu.x = current_pose.x;
+    current_pose_imu.y = current_pose.y;
+    current_pose_imu.z = current_pose.z;
+    current_pose_imu.roll = current_pose.roll;
+    current_pose_imu.pitch = current_pose.pitch;
+    current_pose_imu.yaw = current_pose.yaw;
+
+    current_velocity_imu_x = current_velocity_x;
+    current_velocity_imu_y = current_velocity_y;
+    current_velocity_imu_z = current_velocity_z;
+
+    current_velocity_imu_z = 0;
+
+    offset_imu_x = 0.0;
+    offset_imu_y = 0.0;
+    offset_imu_z = 0.0;
+    offset_imu_roll = 0.0;
+    offset_imu_pitch = 0.0;
+    offset_imu_yaw = 0.0;
+
+//    std::cout<<" vx is "<<current_velocity_x<<" vy is "<<current_velocity_y<<std::endl;
+    previous_pose.x = current_pose.x;
+    previous_pose.y = current_pose.y;
+    previous_pose.z = current_pose.z;
+    previous_pose.roll = current_pose.roll;
+    previous_pose.pitch = current_pose.pitch;
+    previous_pose.yaw = current_pose.yaw;
+
+//    previous_scan_time = current_scan_time;
+
+    previous_previous_velocity = previous_velocity;
+    previous_velocity = current_velocity;
+    previous_velocity_x = current_velocity_x;
+    previous_velocity_y = current_velocity_y;
+    previous_velocity_z = current_velocity_z;
+    previous_accel = current_accel;
+
+    pthread_mutex_lock(&mutex_read);
+    gindex++;
+    glidar_pose.accel = current_accel;
+    glidar_pose.accel_x = current_accel_x;
+    glidar_pose.accel_y = current_accel_y;
+    glidar_pose.accel_z = current_accel_z;
+    glidar_pose.vel = current_velocity;
+    glidar_pose.vel_x = current_velocity_x;
+    glidar_pose.vel_y = current_velocity_y;
+    glidar_pose.vel_z = current_velocity_z;
+    glidar_pose.mpose = current_pose;
+    pthread_mutex_unlock(&mutex_read);
+
+    pthread_mutex_unlock(&mutex_pose);
+
+
+
+
+    double hdg_o = (90 - gvector_trace[gcurmapindex].mndtorigin.heading)*M_PI/180.0;
+    double ndt_vn = current_velocity_x * cos(hdg_o) - current_velocity_y * sin(hdg_o);
+    double ndt_ve = current_velocity_x * sin(hdg_o) + current_velocity_y * cos(hdg_o);
+    double ndt_vd = current_accel_z;
+
+
+
+    std::cout<<std::setprecision(9)<<"gps lat:"<<gcurndtgpspos.lat<<" lon:"
+            <<gcurndtgpspos.lon<<" z:"<<gcurndtgpspos.height<<" heading:"
+           <<gcurndtgpspos.heading<<std::endl;
+    std::cout<<std::setprecision(6)<<std::endl;
+
+
+
+
+    double x0,y0;
+    double x,y;
+    GaussProjCal(gcurndtgpspos.lon,gcurndtgpspos.lat,&x0,&y0);
+    GaussProjCal(gcurgpsgpspos.lon,gcurgpsgpspos.lat,&x,&y);
+    double dis = sqrt(pow(x-x0,2)+ pow(y-y0,2));
+    double headdiff = fabs(gcurgpsgpspos.heading - gcurndtgpspos.heading);
+    double zdiff = fabs(gcurgpsgpspos.height - gcurndtgpspos.height);
+    std::cout<<" dis:"<<dis<<" headdiff:"<<headdiff<<" zdiff:"<<zdiff<<std::endl;
+
+    iv::lidar::ndtpos ndtpos;
+    ndtpos.set_lidartime(raw_scan->header.stamp/1000);
+    ndtpos.set_ndt_time(QDateTime::currentMSecsSinceEpoch());
+    ndtpos.set_accel(current_accel);
+    ndtpos.set_accel_x(current_accel_x);
+    ndtpos.set_accel_y(current_accel_y);
+    ndtpos.set_accel_z(current_accel_z);
+    ndtpos.set_vel(current_velocity);
+    ndtpos.set_vel_x(current_velocity_x);
+    ndtpos.set_vel_y(current_velocity_y);
+    ndtpos.set_vel_z(current_velocity_z);
+    ndtpos.set_fitness_score(fitness_score);
+    ndtpos.set_has_converged(has_converged);
+    ndtpos.set_id(0);
+    ndtpos.set_iteration(iteration);
+    ndtpos.set_trans_probability(trans_probability);
+    ndtpos.set_pose_pitch(current_pose.pitch);
+    ndtpos.set_pose_roll(current_pose.roll);
+    ndtpos.set_pose_yaw(current_pose.yaw);
+    ndtpos.set_pose_x(current_pose.x);
+    ndtpos.set_pose_y(current_pose.y);
+    ndtpos.set_pose_z(current_pose.z);
+    ndtpos.set_comp_time(xTime.elapsed());
+
+
+    int ndatasize = ndtpos.ByteSize();
+    char * strser = new char[ndatasize];
+    bool bSer = ndtpos.SerializeToArray(strser,ndatasize);
+    if(bSer)
+    {
+        iv::modulecomm::ModuleSendMsg(gpc,strser,ndatasize);
+    }
+    else
+    {
+        std::cout<<BOLDRED<<"ndtpos serialize error."<<RESET<<std::endl;
+    }
+    delete strser;
+
+    iv::lidar::ndtgpspos xndtgpspos;
+    xndtgpspos.set_lat(gcurndtgpspos.lat);
+    xndtgpspos.set_lon(gcurndtgpspos.lon);
+    xndtgpspos.set_heading(gcurndtgpspos.heading);
+    xndtgpspos.set_height(gcurndtgpspos.height);
+    xndtgpspos.set_pitch(gcurndtgpspos.pitch);
+    xndtgpspos.set_roll(gcurndtgpspos.roll);
+    xndtgpspos.set_tras_prob(trans_probability);
+    xndtgpspos.set_score(fitness_score);
+    xndtgpspos.set_vn(ndt_vn);
+    xndtgpspos.set_ve(ndt_ve);
+    xndtgpspos.set_vd(ndt_vd);
+
+    givlog->debug("ndtgpspos","lat:%11.7f lon:%11.7f height:%11.3f heading:%6.3f pitch:%6.3f roll:%6.3f vn:%6.3f ve:%6.3f vd:%6.3f trans_prob:%6.3f score:%6.3f",
+                  xndtgpspos.lat(),xndtgpspos.lon(),xndtgpspos.height(),
+                  xndtgpspos.heading(),xndtgpspos.pitch(),xndtgpspos.roll(),
+                  xndtgpspos.vn(),xndtgpspos.ve(),xndtgpspos.vd(),
+                  xndtgpspos.tras_prob(),xndtgpspos.score());
+
+    ndatasize = xndtgpspos.ByteSize();
+    strser = new char[ndatasize];
+    bSer = xndtgpspos.SerializeToArray(strser,ndatasize);
+    if(bSer)
+    {
+        std::cout<<"share msg."<<std::endl;
+        iv::modulecomm::ModuleSendMsg(gpd,strser,ndatasize);
+    }
+    else
+    {
+        std::cout<<BOLDRED<<"ndtgpspos serialize error."<<RESET<<std::endl;
+    }
+    delete strser;
+
+    if(trans_probability < 0.5)gbNeedGPSUpdateNDT = true;
+    else gbNeedGPSUpdateNDT = false;
+
+
+}
+
+
+int GetPose(int & curindex,iv::lidar_pose & xlidar_pose)
+{
+     pthread_mutex_lock(&mutex_read);
+    if(curindex == gindex)
+    {
+        pthread_mutex_unlock(&mutex_read);
+        return 0;
+    }
+    curindex = gindex;
+    xlidar_pose = glidar_pose;
+    pthread_mutex_unlock(&mutex_read);
+    return 1;
+}
+
+void SetCurPose(double x0, double y0, double yaw0, double z0, double pitch0, double roll0)
+{
+
+    std::cout<<"set pose"<<std::endl;
+    pthread_mutex_lock(&mutex_pose);
+
+    initial_pose.x = x0;
+    initial_pose.y = y0;
+    initial_pose.z = z0;
+    initial_pose.roll = roll0;
+    initial_pose.pitch = pitch0;
+    initial_pose.yaw = yaw0;
+
+    localizer_pose.x = initial_pose.x;
+    localizer_pose.y = initial_pose.y;
+    localizer_pose.z = initial_pose.z;
+    localizer_pose.roll = initial_pose.roll;
+    localizer_pose.pitch = initial_pose.pitch;
+    localizer_pose.yaw = initial_pose.yaw;
+
+    previous_pose.x = initial_pose.x;
+    previous_pose.y = initial_pose.y;
+    previous_pose.z = initial_pose.z;
+    previous_pose.roll = initial_pose.roll;
+    previous_pose.pitch = initial_pose.pitch;
+    previous_pose.yaw = initial_pose.yaw;
+
+    current_pose.x = initial_pose.x;
+    current_pose.y = initial_pose.y;
+    current_pose.z = initial_pose.z;
+    current_pose.roll = initial_pose.roll;
+    current_pose.pitch = initial_pose.pitch;
+    current_pose.yaw = initial_pose.yaw;
+
+    current_velocity = 0;
+    current_velocity_x = 0;
+    current_velocity_y = 0;
+    current_velocity_z = 0;
+    angular_velocity = 0;
+
+    pthread_mutex_unlock(&mutex_pose);
+
+
+}
+
+void SetRunState(bool bRun)
+{
+    std::cout<<"set state."<<std::endl;
+    gbNDTRun = bRun;
+}
+
+void setuseimu(bool bUse)
+{
+    _use_imu = bUse;
+}
+

src/detection/detection_ndt_matching/ndt_matching.h

@@ -0,0 +1,116 @@
+#ifndef NDT_MATCHING_H
+#define NDT_MATCHING_H
+#include<QDateTime>
+#include <QMutex>
+
+struct pose
+{
+  double x;
+  double y;
+  double z;
+  double roll;
+  double pitch;
+  double yaw;
+  double vx;
+  double vy;
+};
+
+
+namespace  iv {
+struct lidar_pose
+{
+    lidar_pose() {}
+
+    double vel_x;
+    double vel_y;
+    double vel_z;
+    double vel;
+    double accel_x;
+    double accel_y;
+    double accel_z;
+    double accel;
+    struct pose mpose;
+};
+
+}
+
+namespace iv {
+struct imudata
+{
+  qint64 imutime;
+  double imu_linear_acceleration_x;
+  double imu_linear_acceleration_y;
+  double imu_linear_acceleration_z;
+  double imu_angular_velocity_x;
+  double imu_angular_velocity_y;
+  double imu_angular_velocity_z;
+};
+}
+
+
+void ndt_match_Init_nomap();
+
+void ndt_match_Init(pcl::PointCloud<pcl::PointXYZ>::Ptr mappcd);
+
+void point_ndt(pcl::PointCloud<pcl::PointXYZ>::Ptr raw_scan);
+
+void point_ndt_test(pcl::PointCloud<pcl::PointXYZ>::Ptr raw_scan);
+
+int GetPose(int & curindex,iv::lidar_pose & xlidar_pose);
+
+void SetCurPose(double x0,double y0,double yaw0,double z0 = 0,double pitch0 = 0,double roll0 = 0);
+
+void ndt_match_SetMap(pcl::PointCloud<pcl::PointXYZ>::Ptr mappcd);
+
+void SetRunState(bool bRun);
+
+void restartndtfailcount();
+
+void setrelocpose(pose xPose);
+
+namespace iv {
+struct ndttracepoint
+{
+    double x;
+    double y;
+    double z;
+    double pitch;
+    double roll;
+    double yaw;
+};
+}
+
+namespace iv {
+struct gpspos
+{
+    double lat;
+    double lon;
+    double height;
+    double heading;
+    double pitch;
+    double roll;
+    double ve;
+    double vn;
+};
+}
+
+iv::gpspos PoseToGPS(iv::gpspos xori,pose xpose);
+pose CalcPose(iv::gpspos xori,iv::gpspos xcur);
+
+namespace iv {
+struct ndtmaptrace
+{
+    ndtmaptrace() {}
+    std::vector<ndttracepoint> mvector_trace;
+    std::string mstrpcdpath;
+    iv::gpspos mndtorigin;
+};
+
+}
+
+void setuseimu(bool bUse);
+void imu_update(qint64 current_time_imu,double imu_roll,double imu_pitch,double imu_yaw,
+                       double acceleration_x,double acceleration_y,
+                       double acceleration_z);
+
+#endif // NDT_MATCHING_H

src/detection/detection_ndt_matching_gpu_multi/detection_ndt_matching_gpu_multi.pro

@@ -43,12 +43,15 @@ INCLUDEPATH += /opt/ros/kinetic/include
 INCLUDEPATH += /usr/include/pcl-1.8
 INCLUDEPATH += /usr/include/pcl-1.7
 INCLUDEPATH += /usr/include/eigen3
+unix:INCLUDEPATH += /usr/include/pcl-1.10
+INCLUDEPATH += /opt/ros/noetic/include
 
 INCLUDEPATH += $$PWD/../../include/msgtype/
 
 INCLUDEPATH += /usr/local/cuda-10.0/targets/aarch64-linux/include
 INCLUDEPATH += /usr/local/cuda-10.2/targets/aarch64-linux/include
 INCLUDEPATH += /usr/local/cuda-10.2/targets/x86_64-linux/include
+INCLUDEPATH += /usr/local/cuda/targets/aarch64-linux/include
 
 DEFINES += CUDA_FOUND
 

src/detection/detection_ndt_matching_gpu_multi/main.cpp

@@ -37,6 +37,7 @@ iv::gpspos glastndtgpspos;    //last ndt pos(convert to gps)
 iv::gpspos gcurndtgpspos;     //cur ndt pos(conver to gps)
 iv::gpspos gcurgpsgpspos;    //cur gps pos
 bool gbGPSFix = false;   //GPS is Fix. If 300 times report rtk 6.
+int64_t gnLastGPSTime = 0; //ms
 
 std::thread * gpthread;   //State thread pointer.
 
@@ -430,6 +431,8 @@ void ListenRaw(const char * strdata,const unsigned int nSize,const unsigned int
     if(nFixCount < 300)gbGPSFix = false;
     else gbGPSFix = true;
 
+    gnLastGPSTime = std::chrono::system_clock::now().time_since_epoch().count()/1000000;
+
 }
 
 
@@ -514,7 +517,7 @@ int main(int argc, char *argv[])
    iv::xmlparam::Xmlparam xparam(strpath.toStdString());
 
    gstr_lidarmsgname = xparam.GetParam("lidarmsg","lidar_pc");
-   gstr_gpsmsgname = xparam.GetParam("gpsmsg","ins550d_gpsimu");
+   gstr_gpsmsgname = xparam.GetParam("gpsmsg","hcp2_gpsimu");
    gstr_ndtposmsgname = xparam.GetParam("ndtposmsg","ndtpos");
    gstr_ndtgpsposmsgname = xparam.GetParam("ndtgpsposmsg","ndtgpspos");
    gstr_ndtmainsensormsgname = xparam.GetParam("mainsensor","gps");

src/detection/detection_ndt_matching_gpu_multi/ndt_matching.cpp

@@ -297,6 +297,7 @@ extern iv::gpspos glastndtgpspos;
 extern iv::gpspos gcurndtgpspos;
 extern iv::gpspos gcurgpsgpspos;
 extern bool gbGPSFix;
+extern int64_t gnLastGPSTime;
 
 static bool gbgpsupdatendt = false;
 
@@ -327,8 +328,8 @@ iv::gpspos PoseToGPS(iv::gpspos xori,pose xpose)
     xgpspos.lat = lat;
     xgpspos.height = xpose.z + xori.height;
     xgpspos.heading = heading;
-    xgpspos.pitch = xpose.pitch + xori.pitch;
-    xgpspos.roll  = xpose.roll + xori.roll;
+    xgpspos.pitch = xpose.pitch *180.0/M_PI + xori.pitch;
+    xgpspos.roll  = xpose.roll *180.0/M_PI + xori.roll;
     xgpspos.ve = xpose.vx * cos(hdg_o) - xpose.vy * sin(hdg_o);
     xgpspos.vn = xpose.vx * sin(hdg_o) + xpose.vy * cos(hdg_o);
 
@@ -382,8 +383,8 @@ pose CalcPose(iv::gpspos xori, iv::gpspos xcur)
     posex.x = rel_x;
     posex.y = rel_y;
     posex.z = xcur.height - xori.height;
-    posex.pitch = xcur.pitch - xori.pitch;
-    posex.roll = xcur.roll - xori.roll;
+    posex.pitch = (xcur.pitch - xori.pitch)*M_PI/180.0;
+    posex.roll = (xcur.roll - xori.roll)*M_PI/180.0;
     posex.yaw = rel_hdg;
 
     posex.vx = xcur.ve * cos(-hdg_o) - xcur.vn * sin(-hdg_o);
@@ -449,6 +450,7 @@ static void getmindistrace(int & index, double & ftracedis)
     else
     {
         index = -1;
+        ftracedis = 100000.0;
     }
 }
 
@@ -530,7 +532,7 @@ void LocalMapUpdate(int n)
     {
 
         getmindistrace(index,ftracedis);
-//        std::cout<<"trace dis is "<<ftracedis<<std::endl;
+
         if(g_hasmap == false)
         {
 
@@ -561,10 +563,12 @@ void LocalMapUpdate(int n)
 
         if(ftracedis > 50)
         {
+            std::cout<<std::chrono::system_clock::now().time_since_epoch().count()/1000000<<" trace dis is "<<ftracedis<<std::endl;
             std::cout<<" Out Range."<<std::endl;
             g_hasmap = false;
         }
 
+
         std::this_thread::sleep_for(std::chrono::milliseconds(100));
     }
 }
@@ -607,6 +611,12 @@ void GPSPosMonitor(bool * pbRun)
     }
     while(*pbRun)
     {
+        int64_t nmsnow = std::chrono::system_clock::now().time_since_epoch().count()/1000000;
+        if(abs(nmsnow - gnLastGPSTime)>1000)
+        {
+            std::this_thread::sleep_for(std::chrono::milliseconds(10));
+            continue;
+        }
         if(gbGPSFix == true)
         {
             double x0,y0;
@@ -1217,6 +1227,7 @@ void point_ndt(pcl::PointCloud<pcl::PointXYZ>::Ptr raw_scan)
     if(gbgpsupdatendt == true)
     {
 
+        std::cout<<std::chrono::system_clock::now().time_since_epoch().count()/1000000<<" update ndt pos use gps pos. "<<std::endl;
         gcurndtgpspos = gcurgpsgpspos;
         gbgpsupdatendt = false;
         gbNeedGPSUpdateNDT = false;
@@ -1345,7 +1356,7 @@ void point_ndt(pcl::PointCloud<pcl::PointXYZ>::Ptr raw_scan)
     old_scan_time = current_scan_time;
 
     if(diff_time<=0)diff_time = 0.1;
-    if(diff_time>1.0)diff_time = 1.0;
+    if(diff_time>1.0)diff_time = 0.1;
 
 //    std::cout<<"diff time is "<<diff_time<<std::endl;
 
@@ -1404,6 +1415,8 @@ void point_ndt(pcl::PointCloud<pcl::PointXYZ>::Ptr raw_scan)
           predict_pose_for_ndt = predict_pose;
       }
 
+      predict_pose_for_ndt = predict_pose; //not use predict_pose_imu in shelan.
+
       if(fabs(predict_pose_for_ndt.x -previous_pose.x)>10)
       {
           predict_pose_for_ndt = previous_pose;

src/detection/detection_ndt_pclomp/detection_ndt_matching_gpu_multi.xml

@@ -0,0 +1,13 @@
+<xml>	
+	<node name="detection_ndt_matching_gpu_multi.xml">
+		<param name="lidarmsg" value="lidarpc_center" />
+		<param name="gpsmsg" value="ins550d_gpsimu" />
+		<param name="ndtposmsg" value="ndtpos" />
+		<param name="ndtgpsposmsg" value="ndtgpspos" />
+		<param name="AngleCorrect" value="0.0" />
+		<param name="HeightCorrect" value="0.0" />
+		<param name="SaveLastPos" value="true" />
+		<param name="Arm_LidarGPSBase_x" value="0.9" />
+		<param name="Arm_LidarGPSBase_y" value="0.0" />
+	</node>
+</xml>

src/detection/detection_ndt_pclomp/detection_ndt_pclomp.pro

@@ -0,0 +1,97 @@
+QT -= gui
+QT += network
+CONFIG += console c++17
+CONFIG -= app_bundle
+
+QMAKE_CXXFLAGS += -std=gnu++17
+
+QMAKE_LFLAGS += -no-pie
+
+#QMAKE_CXXFLAGS +=  -g
+
+# The following define makes your compiler emit warnings if you use
+# any feature of Qt which as been marked deprecated (the exact warnings
+# depend on your compiler). Please consult the documentation of the
+# deprecated API in order to know how to port your code away from it.
+DEFINES += QT_DEPRECATED_WARNINGS
+
+# You can also make your code fail to compile if you use deprecated APIs.
+# In order to do so, uncomment the following line.
+# You can also select to disable deprecated APIs only up to a certain version of Qt.
+#DEFINES += QT_DISABLE_DEPRECATED_BEFORE=0x060000    # disables all the APIs deprecated before Qt 6.0.0
+
+SOURCES += main.cpp \
+    ndt_matching.cpp \
+    ../../include/msgtype/ndtpos.pb.cc \
+    gnss_coordinate_convert.cpp \
+    ../../include/msgtype/imu.pb.cc \
+    ../../include/msgtype/gpsimu.pb.cc \
+    ../../include/msgtype/gps.pb.cc \
+    ../../include/msgtype/ndtgpspos.pb.cc \
+    pclomp/ndt_omp.cpp \
+    pclomp/voxel_grid_covariance_omp.cpp
+
+HEADERS += \
+    ndt_matching.h \
+    ../../include/msgtype/ndtpos.pb.h \
+    gnss_coordinate_convert.h \
+    ../../include/msgtype/imu.pb.h \
+    ../../include/msgtype/gpsimu.pb.h \
+    ../../include/msgtype/gps.pb.h \
+    ../../include/msgtype/ndtgpspos.pb.h \
+    pclomp/ndt_omp_impl.hpp \
+    pclomp/voxel_grid_covariance_omp_impl.hpp \
+    pclomp/pclomp/ndt_omp.h \
+    pclomp/pclomp/voxel_grid_covariance_omp.h
+
+INCLUDEPATH += /opt/ros/melodic/include
+INCLUDEPATH += /opt/ros/kinetic/include
+INCLUDEPATH += /usr/include/pcl-1.8
+INCLUDEPATH += /usr/include/pcl-1.7
+INCLUDEPATH += /usr/include/eigen3
+unix:INCLUDEPATH += /usr/include/pcl-1.10
+INCLUDEPATH += /opt/ros/noetic/include
+
+INCLUDEPATH += $$PWD/../../include/msgtype/
+
+INCLUDEPATH += $$PWD/pclomp
+
+DEFINES += _OPENMP
+#DEFINES += USE_PCL_OPENMP
+
+unix:LIBS += -lboost_thread -lboost_system -lprotobuf
+
+unix:LIBS +=  -lpcl_common\
+        -lpcl_features\
+        -lpcl_filters\
+        -lpcl_io\
+        -lpcl_io_ply\
+        -lpcl_kdtree\
+        -lpcl_keypoints\
+        -lpcl_octree\
+        -lpcl_outofcore\
+        -lpcl_people\
+        -lpcl_recognition\
+        -lpcl_registration\
+        -lpcl_sample_consensus\
+        -lpcl_search\
+        -lpcl_segmentation\
+        -lpcl_surface\
+        -lpcl_tracking\
+        -lpcl_visualization
+
+
+#INCLUDEPATH += $$PWD/../../common/ndt_gpu/
+
+
+!include(../../../include/common.pri ) {
+    error( "Couldn't find the common.pri file!" )
+}
+
+#DEFINES+= TEST_CALCTIME
+
+INCLUDEPATH += $$PWD/../../../include/
+LIBS += -L$$PWD/../../../bin/ -lxmlparam -lmodulecomm -livlog -livfault -lndt_gpu  -livexit -livbacktrace
+
+LIBS += -lmpi -lopen-pal -lgomp
+

src/detection/detection_ndt_pclomp/detection_ndt_pclomp.xml

@@ -0,0 +1,13 @@
+<xml>	
+	<node name="detection_ndt_pclomp.xml">
+		<param name="lidarmsg" value="lidarpc_center" />
+		<param name="gpsmsg" value="ins550d_gpsimu" />
+		<param name="ndtposmsg" value="ndtpos" />
+		<param name="ndtgpsposmsg" value="ndtgpspos" />
+		<param name="AngleCorrect" value="0.0" />
+		<param name="HeightCorrect" value="0.0" />
+		<param name="SaveLastPos" value="true" />
+		<param name="Arm_LidarGPSBase_x" value="0.0" />
+		<param name="Arm_LidarGPSBase_y" value="0.0" />
+	</node>
+</xml>

src/detection/detection_ndt_pclomp/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/detection/detection_ndt_pclomp/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/detection/detection_ndt_pclomp/main.cpp

@@ -0,0 +1,590 @@
+#include <QCoreApplication>
+
+#include "modulecomm.h"
+
+#include <pcl/io/pcd_io.h>
+#include <pcl/point_types.h>
+
+#include <QTime>
+#include <QDir>
+#include <QFile>
+
+#include <thread>
+
+#include "ivfault.h"
+#include "ivlog.h"
+#include "ndt_matching.h"
+#include "gpsimu.pb.h"
+
+#include "ivexit.h"
+#include "ivbacktrace.h"
+#include "xmlparam.h"
+#include "ivversion.h"
+
+
+QTime gTime;    //a Time function. use calculate elapsed.
+
+int gndttime;    //ndt calculate time
+
+void * gpa,* gpb ,* gpc, * gpd;   //Modulecomm pointer
+
+iv::Ivfault *gfault = nullptr;   //fault diag
+iv::Ivlog *givlog = nullptr;   //log
+
+std::vector<iv::ndtmaptrace> gvector_trace;  //trace and map origin. use shift map
+
+iv::gpspos glastndtgpspos;    //last ndt pos(convert to gps)
+iv::gpspos gcurndtgpspos;     //cur ndt pos(conver to gps)
+iv::gpspos gcurgpsgpspos;    //cur gps pos
+bool gbGPSFix = false;   //GPS is Fix. If 300 times report rtk 6.
+int64_t gnLastGPSTime = 0; //ms
+
+std::thread * gpthread;   //State thread pointer.
+
+std::string gstr_lidarmsgname;   //lidar message name
+std::string gstr_gpsmsgname;   // gps message name
+std::string gstr_ndtposmsgname;  //ndtpos message name
+std::string gstr_ndtgpsposmsgname;   //gpspos message name
+std::string gstr_ndtmainsensormsgname;
+std::string gstr_arm_x;
+std::string gstr_arm_y;
+
+std::string gstr_yawcorrect;   //lidar yaw correct.
+std::string gstr_heightcorrect;  //gps height correct.
+double gyawcorrect = 0.0;        //lidar yaw correct.
+double gheightcorrect = 0.0;      //gps height correct.
+
+std::string gstr_savelastpos; //set save last pos. default true
+bool gbSaveLastPos = true;
+
+double garm_x = 0.0;
+double garm_y = 0.0;
+
+QFile * gpFileLastPos = 0;//Save Last Positin
+
+/**
+ * @brief readtrace read trace
+ * @param pFile
+ * @return
+ */
+std::vector<iv::ndttracepoint> readtrace(QFile * pFile)
+{
+    std::vector<iv::ndttracepoint> ntp;
+    QByteArray ba;
+    ba = pFile->readLine();
+
+    do
+    {
+        QString str;
+        str = ba;
+        QStringList strlist;
+        strlist = str.split("\t");
+        if(strlist.size() == 6)
+        {
+            iv::ndttracepoint tp;
+            QString strx;
+            int j;
+            double fv[6];
+            for(j=0;j<6;j++)
+            {
+                strx = strlist.at(j);
+                fv[j] = strx.toDouble();
+            }
+            tp.x = fv[0];
+            tp.y = fv[1];
+            tp.z = fv[2];
+            tp.pitch = fv[3];
+            tp.roll = fv[4];
+            tp.yaw = fv[5];
+            ntp.push_back(tp);
+        }
+        else
+        {
+            qDebug("len is %d, %d",ba.length(),strlist.size());
+        }
+        ba = pFile->readLine();
+    }while(ba.length()>0);
+
+    return ntp;
+}
+
+/**
+ * @brief readndtorigin read .pcd file's origin gps position.
+ * @param pFile
+ * @param pnori
+ * @return
+ */
+int readndtorigin(QFile * pFile,iv::gpspos * pnori)
+{
+    int nrtn = -1;
+    QByteArray ba;
+    ba = pFile->readLine();
+    QString str;
+    str = ba;
+    QStringList strlist;
+    strlist = str.split("\t");
+    if(strlist.size() == 6)
+    {
+        QString xstr[6];
+        int i;
+        for(i=0;i<6;i++)xstr[i] = strlist.at(i);
+        pnori->lon = xstr[0].toDouble();
+        pnori->lat = xstr[1].toDouble();
+        pnori->height = xstr[2].toDouble();
+        pnori->heading = xstr[3].toDouble();
+        pnori->pitch = xstr[4].toDouble();
+        pnori->roll = xstr[5].toDouble();
+        nrtn = 0;
+    }
+    return nrtn;
+
+}
+
+/**
+ * @brief LoadLastPos Load last position
+ * @param strlastposfilepath
+ */
+static void LoadLastPos(const char * strlastposfilepath)
+{
+    QFile * xFile = new QFile();
+    xFile->setFileName(strlastposfilepath);
+    if(xFile->open(QIODevice::ReadWrite))
+    {
+        int nrtn = readndtorigin(xFile,&glastndtgpspos);
+        if(nrtn < 0)
+        {
+            givlog->warn("load last pos fail.");
+        }
+        if(gbSaveLastPos)gpFileLastPos = xFile;
+        else
+        {
+            gpFileLastPos = 0;
+            xFile->close();
+        }
+    }
+}
+
+/**
+ * @brief LoadTrace
+ */
+static void LoadTrace()
+{
+    std::string strpath = getenv("HOME");
+    strpath = strpath + "/map/";
+
+    std::string strlastposfile = strpath;
+    strlastposfile = strlastposfile + "lastpos.txt";
+    LoadLastPos(strlastposfile.data());
+
+    QString strpathdir = strpath.data();
+    QDir dir(strpath.data());
+    QStringList xfilter;
+    xfilter<<"*.pcd";
+    foreach(QString strfilename,dir.entryList(xfilter,QDir::Files))
+    {
+        qDebug(strfilename.toLatin1().data());
+        QString stroripath;
+        QString strtracepath;
+        QString strpcdpath;
+        stroripath = strfilename;
+        stroripath = stroripath.left(stroripath.length() - 4);
+        stroripath = strpathdir +  stroripath + ".ori";
+        strtracepath = strfilename;
+        strtracepath = strtracepath.left(strtracepath.length() - 4);
+        strtracepath = strpathdir +  strtracepath + ".txt";
+        strpcdpath = strfilename;
+        strpcdpath = strpathdir + strpcdpath;
+        QFile xFileori,xFile,xFilepcd;
+
+        xFileori.setFileName(stroripath);
+        xFilepcd.setFileName(strpcdpath);
+        xFile.setFileName(strtracepath);
+        if(xFileori.exists() && xFile.exists())
+        {
+            qDebug("file name ok.");
+            if(xFile.open(QIODevice::ReadOnly) && xFileori.open(QIODevice::ReadOnly))
+            {
+                std::vector<iv::ndttracepoint> xv =  readtrace(&xFile);
+                iv::gpspos xnori;
+                int nrtn = readndtorigin(&xFileori,&xnori);
+                if((xv.size() > 0)&&(nrtn == 0))
+                {
+                    iv::ndtmaptrace nmt;
+                    nmt.mvector_trace = xv;
+                    nmt.mstrpcdpath = strpcdpath.toLatin1().data();
+                    nmt.mndtorigin = xnori;
+                    gvector_trace.push_back(nmt);
+                    qDebug("this is ok.");
+                }
+
+            }
+            xFile.close();
+            xFileori.close();
+
+        }
+        else
+        {
+            qDebug(" file not ok.");
+        }
+    }
+    return;
+}
+
+int gnothavedatatime = 0;
+/**
+ * @brief ListenPointCloud
+ * @param strdata
+ * @param nSize
+ * @param index
+ * @param dt
+ * @param strmemname
+ */
+void ListenPointCloud(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
+{
+
+    QTime xTime;
+    xTime.start();
+
+    gnothavedatatime = 0;
+    if(nSize <=16)return;
+    unsigned int * pHeadSize = (unsigned int *)strdata;
+    if(*pHeadSize > nSize)
+    {
+        std::cout<<"ListenPointCloud data is small headsize ="<<*pHeadSize<<"  data size is"<<nSize<<std::endl;
+    }
+
+
+    pcl::PointCloud<pcl::PointXYZ>::Ptr point_cloud(
+                new pcl::PointCloud<pcl::PointXYZ>());
+    int nNameSize;
+    nNameSize = *pHeadSize - 4-4-8;
+    char * strName = new char[nNameSize+1];strName[nNameSize] = 0;
+    memcpy(strName,(char *)((char *)strdata +4),nNameSize);
+    point_cloud->header.frame_id = strName;
+    memcpy(&point_cloud->header.seq,(char *)strdata+4+nNameSize,4);
+    memcpy(&point_cloud->header.stamp,(char *)strdata+4+nNameSize+4,8);
+    int nPCount = (nSize - *pHeadSize)/sizeof(pcl::PointXYZI);
+    int i;
+    pcl::PointXYZI * p;
+    p = (pcl::PointXYZI *)((char *)strdata + *pHeadSize);
+    for(i=0;i<nPCount;i++)
+    {
+        pcl::PointXYZ xp;
+        xp.x = p->y;
+        xp.y = p->x * (-1.0);
+        xp.z = p->z;
+
+        if(gyawcorrect != 0.0)
+        {
+            double yaw =  gyawcorrect;
+            double x1,y1;
+            x1 = xp.x;
+            y1 = xp.y;
+            xp.x = x1*cos(yaw) -y1*sin(yaw);
+            xp.y = x1*sin(yaw) + y1*cos(yaw);
+
+        }
+
+ //       xp.intensity = p->intensity;
+        point_cloud->push_back(xp);
+        p++;
+    }
+
+    givlog->verbose("point cloud size is %d",
+                    point_cloud->size());
+
+
+
+    point_ndt(point_cloud);
+    gndttime = xTime.elapsed();
+    givlog->verbose("ndt time is %d, gtime is %d", xTime.elapsed(), gTime.elapsed());
+ //   return;
+
+
+ //   gw->UpdatePointCloud(point_cloud);
+ //   DBSCAN_PC(point_cloud);
+
+    gfault->SetFaultState(0, 0, "ok");
+}
+
+/**
+ * @brief ListenMapUpdate
+ * @param strdata
+ * @param nSize
+ * @param index
+ * @param dt
+ * @param strmemname
+ */
+void ListenMapUpdate(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
+{
+    char * strpath = new char[nSize+1];
+    memcpy(strpath,strdata,nSize);
+    strpath[nSize] = 0;
+
+    pcl::PointCloud<pcl::PointXYZ>::Ptr mapx = boost::shared_ptr<pcl::PointCloud<pcl::PointXYZ>>(new pcl::PointCloud<pcl::PointXYZ>);;
+    if(0 == pcl::io::loadPCDFile(strpath,*mapx))
+    {
+        int size = mapx->size();
+        givlog->verbose("mapx size = %d", size);
+        ndt_match_SetMap(mapx);
+
+    }
+
+    gfault->SetFaultState(0, 0, "ok");
+    delete strpath;
+}
+
+/**
+ * @brief ListenNDTRunstate
+ * @param strdata
+ * @param nSize
+ * @param index
+ * @param dt
+ * @param strmemname
+ */
+void ListenNDTRunstate(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
+{
+    bool bState;
+    if(nSize < sizeof(bool))
+    {
+        gfault->SetFaultState(1, 0, "ListenNDTRunstate data size is small");
+        givlog->error("ListenNDTRunstate data size is small");
+        return;
+    }
+    memcpy(&bState,strdata,sizeof(bool));
+
+    SetRunState(bState);
+    gfault->SetFaultState(0, 0, "ok");
+}
+
+/**
+ * @brief pausecomm
+ */
+void pausecomm()
+{
+    iv::modulecomm::PauseComm(gpa);
+    iv::modulecomm::PauseComm(gpb);
+    iv::modulecomm::PauseComm(gpc);
+    iv::modulecomm::PauseComm(gpd);
+}
+
+/**
+ * @brief continuecomm
+ */
+void continuecomm()
+{
+    iv::modulecomm::ContintuComm(gpa);
+    iv::modulecomm::ContintuComm(gpb);
+    iv::modulecomm::ContintuComm(gpc);
+    iv::modulecomm::ContintuComm(gpd);
+}
+
+/**
+ * @brief ListenRaw
+ * @param strdata
+ * @param nSize
+ * @param index
+ * @param dt
+ * @param strmemname
+ */
+void ListenRaw(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
+{
+    static unsigned int nFixCount = 0;
+    iv::gps::gpsimu xgpsimu;
+    if(!xgpsimu.ParseFromArray(strdata,nSize))
+    {
+        std::cout<<"ListenRaw Parse error."<<std::endl;
+    }
+
+    gcurgpsgpspos.lat = xgpsimu.lat();
+    gcurgpsgpspos.lon = xgpsimu.lon();
+    gcurgpsgpspos.height = xgpsimu.height() + gheightcorrect;
+    gcurgpsgpspos.heading = xgpsimu.heading();
+    gcurgpsgpspos.pitch = xgpsimu.pitch();
+    gcurgpsgpspos.roll = xgpsimu.roll();
+    gcurgpsgpspos.ve = xgpsimu.ve();
+    gcurgpsgpspos.vn = xgpsimu.vn();
+
+    givlog->verbose("gps lat:%11.7f lon:%11.7f heading:%11.7f height:%6.3f rtk:%d",
+                    xgpsimu.lat(),xgpsimu.lon(),xgpsimu.heading(),
+                    xgpsimu.height(),xgpsimu.rtk_state());
+
+    if(xgpsimu.has_acce_x())
+    {
+        setuseimu(true);
+        imu_update(xgpsimu.time(),
+                   xgpsimu.roll(),xgpsimu.pitch(),
+                   xgpsimu.heading(),xgpsimu.acce_x(),
+                   xgpsimu.acce_y(),xgpsimu.acce_z());
+    }
+
+
+    if(xgpsimu.rtk_state() == 6)
+    {
+        nFixCount++;
+    }
+    else
+    {
+        nFixCount = 0;
+    }
+
+    if(nFixCount < 300)gbGPSFix = false;
+    else gbGPSFix = true;
+
+    gnLastGPSTime = std::chrono::system_clock::now().time_since_epoch().count()/1000000;
+
+}
+
+
+bool gbstate = true;
+
+/**
+ * @brief statethread Monitor ndt state thread.
+ */
+void statethread()
+{
+    int nstate = 0;
+    int nlaststate = 0;
+    while(gbstate)
+    {
+        std::this_thread::sleep_for(std::chrono::milliseconds(10));
+        if(gnothavedatatime < 100000) gnothavedatatime++;
+
+        if(gnothavedatatime < 100)
+        {
+            nstate = 0;
+        }
+        if(gnothavedatatime > 1000)
+        {
+            nstate = 1;
+        }
+        if(gnothavedatatime > 6000)
+        {
+            nstate = 2;
+        }
+        if(nstate != nlaststate)
+        {
+            nlaststate = nstate;
+            switch (nstate) {
+            case 0:
+                givlog->info("ndt matching  is ok");
+                gfault->SetFaultState(0,0,"data is ok.");
+                break;
+            case 1:
+                givlog->warn("more than 10 seconds not have lidar pointcloud.");
+                gfault->SetFaultState(1,1,"more than 10 seconds not have lidar pointcloud.");
+                break;
+            case 2:
+                givlog->error("more than 60 seconds not have lidar pointcloud.");
+                gfault->SetFaultState(2,2,"more than 60 seconds not have lidar pointcloud.");
+                break;
+            default:
+                break;
+            }
+        }
+    }
+}
+
+/**
+ * @brief exitfunc when receive exit command.
+ */
+void exitfunc()
+{
+    std::cout<<"enter exitfunc."<<std::endl;
+    gbstate = false;
+    gpthread->join();
+    iv::modulecomm::Unregister(gpa);
+    iv::modulecomm::Unregister(gpb);
+    iv::modulecomm::Unregister(gpc);
+    iv::modulecomm::Unregister(gpd);
+    if(gpFileLastPos != 0)gpFileLastPos->close();
+    std::cout<<"Complete exitfunc."<<std::endl;
+}
+
+int main(int argc, char *argv[])
+{
+    showversion("detection_ndt_pclomp");
+    QCoreApplication a(argc, argv);
+
+    QString strpath = QCoreApplication::applicationDirPath();
+
+   if(argc < 2)
+       strpath = strpath + "/detection_ndt_pclomp.xml";
+   else
+       strpath = argv[1];
+   std::cout<<strpath.toStdString()<<std::endl;
+
+   iv::xmlparam::Xmlparam xparam(strpath.toStdString());
+
+   gstr_lidarmsgname = xparam.GetParam("lidarmsg","lidar_pc");
+   gstr_gpsmsgname = xparam.GetParam("gpsmsg","hcp2_gpsimu");
+   gstr_ndtposmsgname = xparam.GetParam("ndtposmsg","ndtpos");
+   gstr_ndtgpsposmsgname = xparam.GetParam("ndtgpsposmsg","ndtgpspos");
+   gstr_ndtmainsensormsgname = xparam.GetParam("mainsensor","gps");
+
+   gstr_yawcorrect = xparam.GetParam("AngleCorrect","0.0");
+   gstr_heightcorrect = xparam.GetParam("HeightCorrect","0.0");
+   gstr_savelastpos = xparam.GetParam("SaveLastPos","true");
+
+   gstr_arm_x = xparam.GetParam("Arm_LidarGPSBase_x","0.0");
+   gstr_arm_y = xparam.GetParam("Arm_LidarGPSBase_y","0.0");
+
+   gyawcorrect = atof(gstr_yawcorrect.data()) * M_PI/180.0;
+   gheightcorrect = atof(gstr_heightcorrect.data());
+
+   if(gstr_savelastpos == "true")gbSaveLastPos = true;
+   else gbSaveLastPos = false;
+
+   garm_x = atof(gstr_arm_x.data());
+   garm_y = atof(gstr_arm_y.data());
+
+
+   std::cout<<"lidar message is "<<gstr_lidarmsgname<<std::endl;
+   std::cout<<"gps message is "<<gstr_gpsmsgname<<std::endl;
+   std::cout<<"ndtpos message is "<<gstr_ndtposmsgname<<std::endl;
+   std::cout<<"ndtgpspos message is "<<gstr_ndtgpsposmsgname<<std::endl;
+   std::cout<<"AngleCorrect  is "<<gstr_yawcorrect<<std::endl;
+   std::cout<<"HeightCorrect is "<<gstr_heightcorrect<<std::endl;
+   std::cout<<"SaveLastPos is "<<gstr_savelastpos<<std::endl;
+   std::cout<<"Arm_LidarGPSBase_x is "<<gstr_arm_x<<std::endl;
+   std::cout<<"Arm_LidarGPSBase_x is "<<gstr_arm_y<<std::endl;
+
+
+    gfault = new iv::Ivfault("detection_ndt_matching_gpu");
+    givlog = new iv::Ivlog("detection_ndt_matching_gpu");
+
+    gfault->SetFaultState(0,0,"Initialize.");
+
+    glastndtgpspos.lat = 39;
+    glastndtgpspos.lon = 119;
+    glastndtgpspos.heading = 0;
+    glastndtgpspos.height = 0;
+    glastndtgpspos.pitch = 0;
+    glastndtgpspos.roll = 0;
+    gcurndtgpspos = glastndtgpspos;
+
+    iv::gpspos xpos = glastndtgpspos;
+    xpos.lat = 39.1;
+    xpos.heading = 90;
+    pose xp = CalcPose(glastndtgpspos,xpos);
+    iv::gpspos xx = PoseToGPS(glastndtgpspos,xp);
+    LoadTrace();
+    std::cout<<"trace size  is "<<gvector_trace.size()<<std::endl;
+
+    gpa = iv::modulecomm::RegisterRecv(gstr_lidarmsgname.data(),ListenPointCloud);
+    gpb = iv::modulecomm::RegisterRecv(gstr_gpsmsgname.data(),ListenRaw);
+    gpc = iv::modulecomm::RegisterSend(gstr_ndtposmsgname.data(),10000,1);
+    gpd = iv::modulecomm::RegisterSend(gstr_ndtgpsposmsgname.data(),10000,1);
+    ndt_match_Init_nomap();
+
+    iv::ivexit::RegIVExitCall(exitfunc);
+    RegisterIVBackTrace();
+
+    gpthread = new std::thread(statethread);
+//    gpb = iv::modulecomm::RegisterRecv("ndt_mappath",ListenMapUpdate);
+ //   gpc = iv::modulecomm::RegisterRecv("ndt_runstate",ListenNDTRunstate);
+
+
+
+    return a.exec();
+}

src/detection/detection_ndt_pclomp/ndt_matching.cpp

@@ -0,0 +1,1849 @@
+/*
+ * Copyright 2015-2019 Autoware Foundation. All rights reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+/*
+ Localization program using Normal Distributions Transform
+
+ Yuki KITSUKAWA
+ */
+
+#include <pthread.h>
+#include <chrono>
+#include <fstream>
+#include <iostream>
+#include <memory>
+#include <sstream>
+#include <string>
+
+#include <thread>
+
+#include <boost/filesystem.hpp>
+
+
+#include <tf/tf.h>
+#include <tf/transform_broadcaster.h>
+#include <tf/transform_datatypes.h>
+#include <tf/transform_listener.h>
+
+#include <pcl/io/io.h>
+#include <pcl/io/pcd_io.h>
+#include <pcl/point_types.h>
+#include <pcl_conversions/pcl_conversions.h>
+
+//#include <ndt_cpu/NormalDistributionsTransform.h>
+#include <pcl/registration/ndt.h>
+
+
+#include <pcl/io/pcd_io.h>
+#include <pcl/point_types.h>
+#include <pcl/point_cloud.h>
+#include <pcl/registration/ndt.h>
+#include <pcl/registration/gicp.h>
+#include <pcl/filters/voxel_grid.h>
+//#include <pcl/visualization/pcl_visualizer.h>
+
+#include <pclomp/ndt_omp.h>
+
+#include <QFile>
+
+//#include <ndt_gpu/NormalDistributionsTransform.h>
+#include "ndtpos.pb.h"
+#include "ndtgpspos.pb.h"
+
+#ifdef USE_PCL_OPENMP
+#include <pcl_omp_registration/ndt.h>
+#endif
+
+#include <pcl_ros/point_cloud.h>
+#include <pcl_ros/transforms.h>
+
+
+
+#include "ndt_matching.h"
+
+#include "modulecomm.h"
+
+
+
+#include "gnss_coordinate_convert.h"
+
+//the following are UBUNTU/LINUX ONLY terminal color codes.
+#define RESET   "\033[0m"
+#define BLACK   "\033[30m"      /* Black */
+#define RED     "\033[31m"      /* Red */
+#define GREEN   "\033[32m"      /* Green */
+#define YELLOW  "\033[33m"      /* Yellow */
+#define BLUE    "\033[34m"      /* Blue */
+#define MAGENTA "\033[35m"      /* Magenta */
+#define CYAN    "\033[36m"      /* Cyan */
+#define WHITE   "\033[37m"      /* White */
+#define BOLDBLACK   "\033[1m\033[30m"      /* Bold Black */
+#define BOLDRED     "\033[1m\033[31m"      /* Bold Red */
+#define BOLDGREEN   "\033[1m\033[32m"      /* Bold Green */
+#define BOLDYELLOW  "\033[1m\033[33m"      /* Bold Yellow */
+#define BOLDBLUE    "\033[1m\033[34m"      /* Bold Blue */
+#define BOLDMAGENTA "\033[1m\033[35m"      /* Bold Magenta */
+#define BOLDCYAN    "\033[1m\033[36m"      /* Bold Cyan */
+#define BOLDWHITE   "\033[1m\033[37m"      /* Bold White */
+
+
+#define PREDICT_POSE_THRESHOLD 0.5
+
+#define Wa 0.4
+#define Wb 0.3
+#define Wc 0.3
+
+#include "ivfault.h"
+#include "ivlog.h"
+
+extern iv::Ivfault *gfault ;
+extern iv::Ivlog *givlog;
+
+static int gindex = 0;
+iv::lidar_pose glidar_pose;
+
+void * gpset;
+void * gppose;
+
+static bool g_hasmap = false;
+
+enum class MethodType
+{
+  PCL_GENERIC = 0,
+  PCL_ANH = 1,
+  PCL_ANH_GPU = 2,
+  PCL_OPENMP = 3,
+};
+static MethodType _method_type = MethodType::PCL_GENERIC;
+
+static pose initial_pose, predict_pose, predict_pose_imu, predict_pose_odom, predict_pose_imu_odom, previous_pose,
+    ndt_pose, current_pose, current_pose_imu, current_pose_odom, current_pose_imu_odom, localizer_pose;
+
+static double offset_x, offset_y, offset_z, offset_yaw;  // current_pos - previous_pose
+static double offset_imu_x, offset_imu_y, offset_imu_z, offset_imu_roll, offset_imu_pitch, offset_imu_yaw;
+static double offset_odom_x, offset_odom_y, offset_odom_z, offset_odom_roll, offset_odom_pitch, offset_odom_yaw;
+static double offset_imu_odom_x, offset_imu_odom_y, offset_imu_odom_z, offset_imu_odom_roll, offset_imu_odom_pitch,
+    offset_imu_odom_yaw;
+
+// Can't load if typed "pcl::PointCloud<pcl::PointXYZRGB> map, add;"
+static pcl::PointCloud<pcl::PointXYZ> map, add;
+
+// If the map is loaded, map_loaded will be 1.
+static int map_loaded = 0;
+static int _use_gnss = 1;
+static int init_pos_set = 0;
+
+static pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> ndt;
+
+
+pclomp::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>::Ptr ndt_omp(new pclomp::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>());
+
+pclomp::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>::Ptr glocalndt_omp(new pclomp::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>());
+
+
+// Default values
+static int max_iter = 30;        // Maximum iterations
+static float ndt_res = 1.0;      // Resolution
+static double step_size = 0.1;   // Step size
+static double trans_eps = 0.01;  // Transformation epsilon
+
+
+static double exe_time = 0.0;
+static bool has_converged;
+static int iteration = 0;
+static double fitness_score = 0.0;
+static double trans_probability = 0.0;
+
+static double diff = 0.0;
+static double diff_x = 0.0, diff_y = 0.0, diff_z = 0.0, diff_yaw;
+
+static double current_velocity = 0.0, previous_velocity = 0.0, previous_previous_velocity = 0.0;  // [m/s]
+static double current_velocity_x = 0.0, previous_velocity_x = 0.0;
+static double current_velocity_y = 0.0, previous_velocity_y = 0.0;
+static double current_velocity_z = 0.0, previous_velocity_z = 0.0;
+// static double current_velocity_yaw = 0.0, previous_velocity_yaw = 0.0;
+static double current_velocity_smooth = 0.0;
+
+static double current_velocity_imu_x = 0.0;
+static double current_velocity_imu_y = 0.0;
+static double current_velocity_imu_z = 0.0;
+
+static double current_accel = 0.0, previous_accel = 0.0;  // [m/s^2]
+static double current_accel_x = 0.0;
+static double current_accel_y = 0.0;
+static double current_accel_z = 0.0;
+// static double current_accel_yaw = 0.0;
+
+static double angular_velocity = 0.0;
+
+static int use_predict_pose = 0;
+
+
+static std::chrono::time_point<std::chrono::system_clock> matching_start, matching_end;
+
+
+static int _queue_size = 1000;
+
+
+static double predict_pose_error = 0.0;
+
+static double _tf_x, _tf_y, _tf_z, _tf_roll, _tf_pitch, _tf_yaw;
+static Eigen::Matrix4f tf_btol;
+
+static std::string _localizer = "velodyne";
+static std::string _offset = "linear";  // linear, zero, quadratic
+
+
+
+static bool _get_height = false;
+static bool _use_local_transform = false;
+static bool _use_imu = false;
+static bool _use_odom = false;
+static bool _imu_upside_down = false;
+static bool _output_log_data = false;
+
+static std::string _imu_topic = "/imu_raw";
+
+static std::ofstream ofs;
+static std::string filename;
+
+//static sensor_msgs::Imu imu;
+//static nav_msgs::Odometry odom;
+
+// static tf::TransformListener local_transform_listener;
+static tf::StampedTransform local_transform;
+
+static unsigned int points_map_num = 0;
+
+pthread_mutex_t mutex;
+
+pthread_mutex_t mutex_read;
+
+pthread_mutex_t mutex_pose;
+
+bool gbNeedGPSUpdateNDT = false;
+
+pcl::PointCloud<pcl::PointXYZ>::Ptr local_map_ptr;
+pcl::PointCloud<pcl::PointXYZ>::Ptr global_map_ptr;
+
+pose glastmappose;
+
+static double imu_angular_velocity_x=0;
+static double imu_angular_velocity_y=0;
+static double imu_angular_velocity_z=0;
+static double imu_linear_acceleration_x=0;
+static double imu_linear_acceleration_y=0;
+static double imu_linear_acceleration_z =0;
+
+extern QFile * gpFileLastPos;//Save Last Positin
+static bool gbFileNDTUpdate;
+
+extern double garm_x ;
+extern double garm_y ;
+
+#include <QDateTime>
+
+//cv::Mat gmatimage;
+void ListenPoseSet(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
+{
+    iv::lidar::ndtpos pos;
+
+    if(false == pos.ParseFromArray(strdata,nSize))
+    {
+        std::cout<<" ndtpos parse error."<<std::endl;
+        return;
+    }
+
+    SetCurPose(pos.pose_x(),pos.pose_y(),pos.pose_yaw(),pos.pose_z(),pos.pose_pitch(),pos.pose_roll());
+
+
+}
+
+static void SetLocalMap()
+{
+    pcl::PointCloud<pcl::PointXYZ>::Ptr local_ptr(new pcl::PointCloud<pcl::PointXYZ>());
+
+    int nSize = global_map_ptr->size();
+
+    int i;
+    for(i=0;i<nSize;i++)
+    {
+        pcl::PointXYZ xp = global_map_ptr->at(i);
+        if(sqrt(pow(xp.x - current_pose.x,2)+pow(xp.y-current_pose.y,2)+pow(xp.z-current_pose.z,2))<30)
+        {
+            local_ptr->push_back(xp);
+        }
+    }
+
+    glastmappose = current_pose;
+
+    local_map_ptr = local_ptr;
+    std::cout<<"current map size is "<<local_map_ptr->size()<<std::endl;
+}
+
+static bool gbNDTRun = true;
+
+static bool gbGFRun = true;
+static bool gbLMRun= true;
+static std::thread * gpmapthread, * gpgpsfixthread,*gpsavelastthread;
+static bool gbNeedUpdate = false;
+
+extern iv::gpspos glastndtgpspos;
+extern iv::gpspos gcurndtgpspos;
+extern iv::gpspos gcurgpsgpspos;
+extern bool gbGPSFix;
+extern int64_t gnLastGPSTime;
+
+static bool gbgpsupdatendt = false;
+
+static int gusemapindex = -1;
+static int gcurmapindex = -1;
+
+extern std::vector<iv::ndtmaptrace> gvector_trace;
+
+extern void * gpa,* gpb ,* gpc, * gpd;
+
+iv::gpspos PoseToGPS(iv::gpspos xori,pose xpose)
+{
+    double x_o,y_o;
+    GaussProjCal(xori.lon,xori.lat,&x_o,&y_o);
+    double lon,lat;
+    double hdg_o = (90 - xori.heading)*M_PI/180.0;
+    double rel_x = xpose.x * cos(hdg_o) - xpose.y * sin(hdg_o);
+    double rel_y = xpose.x * sin(hdg_o) + xpose.y * cos(hdg_o);
+    GaussProjInvCal(x_o + rel_x,y_o + rel_y,&lon,&lat);
+    double hdg_c = hdg_o + xpose.yaw;
+
+    hdg_c = M_PI/2.0 - hdg_c;
+    double heading = hdg_c * 180.0/M_PI;
+    while(heading < 0)heading = heading + 360;
+    while(heading >360)heading = heading - 360;
+    iv::gpspos xgpspos;
+    xgpspos.lon = lon;
+    xgpspos.lat = lat;
+    xgpspos.height = xpose.z + xori.height;
+    xgpspos.heading = heading;
+    xgpspos.pitch = xpose.pitch + xori.pitch;
+    xgpspos.roll  = xpose.roll + xori.roll;
+    xgpspos.ve = xpose.vx * cos(hdg_o) - xpose.vy * sin(hdg_o);
+    xgpspos.vn = xpose.vx * sin(hdg_o) + xpose.vy * cos(hdg_o);
+
+    if((garm_x != 0)||(garm_y != 0)) //arm use to Convert pos to GPS Posision
+    {
+        GaussProjCal(xgpspos.lon,xgpspos.lat,&x_o,&y_o);
+        hdg_o = (90 - xgpspos.heading)*M_PI/180.0;
+        rel_x = garm_x *(-1) * cos(hdg_o) - garm_y *(-1) * sin(hdg_o);
+        rel_y = garm_x *(-1) * sin(hdg_o) + garm_y *(-1) * cos(hdg_o);
+        GaussProjInvCal(x_o + rel_x,y_o + rel_y,&lon,&lat);
+        xgpspos.lon = lon;
+        xgpspos.lat = lat;
+    }
+    return xgpspos;
+
+}
+
+pose CalcPose(iv::gpspos xori, iv::gpspos xcur)
+{
+    double lon,lat;
+    if((garm_x != 0)||(garm_y != 0)) //arm use to Convert pos to GPS Posision
+    {
+        double x_o,y_o;
+        GaussProjCal(xcur.lon,xcur.lat,&x_o,&y_o);
+        double hdg_o = (90 - xcur.heading)*M_PI/180.0;
+        double rel_x = garm_x *(1) * cos(hdg_o) - garm_y *(1) * sin(hdg_o);
+        double rel_y = garm_x *(1) * sin(hdg_o) + garm_y *(1) * cos(hdg_o);
+        GaussProjInvCal(x_o + rel_x,y_o + rel_y,&lon,&lat);
+        xcur.lon = lon;
+        xcur.lat = lat;
+    }
+    double x_o,y_o,hdg_o;
+    double x_c,y_c,hdg_c;
+    GaussProjCal(xori.lon,xori.lat,&x_o,&y_o);
+    GaussProjCal(xcur.lon,xcur.lat,&x_c,&y_c);
+    double dis = sqrt(pow(x_c-x_o,2)+ pow(y_c-y_o,2));
+    double rel_x0,rel_y0;
+    rel_x0 = x_c -x_o;
+    rel_y0 = y_c -y_o;
+    double rel_x,rel_y;
+
+    hdg_o = (90 - xori.heading)*M_PI/180.0;
+    hdg_c = (90 - xcur.heading)*M_PI/180.0;
+    rel_x = rel_x0 * cos(-hdg_o) - rel_y0 * sin(-hdg_o);
+    rel_y = rel_x0 * sin(-hdg_o) + rel_y0 * cos(-hdg_o);
+
+
+    double rel_hdg;
+    rel_hdg = hdg_c - hdg_o;
+    pose posex;
+    posex.x = rel_x;
+    posex.y = rel_y;
+    posex.z = xcur.height - xori.height;
+    posex.pitch = xcur.pitch - xori.pitch;
+    posex.roll = xcur.roll - xori.roll;
+    posex.yaw = rel_hdg;
+
+    posex.vx = xcur.ve * cos(-hdg_o) - xcur.vn * sin(-hdg_o);
+    posex.vy = xcur.ve * sin(-hdg_o) + xcur.vn * cos(-hdg_o);
+
+    return posex;
+
+}
+
+static double gettracedis(int index,pose posex)
+{
+    double fdismin = 1000000.;
+    double zdiff = 0;
+    int neari;
+    if(index < 0)return 1000000.0;
+    if(index>= gvector_trace.size())
+    {
+        return 1000000.0;
+    }
+    int i;
+    std::vector<iv::ndttracepoint> vt = gvector_trace.at(index).mvector_trace;
+    int nsize = vt.size();
+//    std::cout<<"size is "<<nsize<<std::endl;
+    for(i=0;i<nsize;i++)
+    {
+        double fdis = sqrt(pow(posex.x - vt.at(i).x,2) + pow(posex.y - vt.at(i).y,2));
+//        std::cout<<"fdis is "<<fdis<<std::endl;
+        if((fdis < fdismin) &&(fabs(posex.z - vt.at(i).z)<5))
+        {
+            fdismin = fdis;
+            neari = i;
+            zdiff = fabs(posex.z - vt.at(i).z);
+        }
+    }
+
+    return fdismin;
+}
+
+static void getmindistrace(int & index, double & ftracedis)
+{
+    double fdismin = 1000000.0;
+    int xindexmin = -1;
+    int i;
+    int nsize = gvector_trace.size();
+
+
+    for(i=0;i<nsize;i++)
+    {
+        pose posex = CalcPose(gvector_trace[i].mndtorigin,gcurndtgpspos);
+        double fdis = gettracedis(i,posex);
+        if(fdis<fdismin)
+        {
+            fdismin = fdis;
+            xindexmin = i;
+        }
+    }
+
+    if(xindexmin != -1)
+    {
+        ftracedis = fdismin;
+        index = xindexmin;
+    }
+    else
+    {
+        index = -1;
+        ftracedis = 100000.0;
+    }
+}
+
+#include <QTime>
+
+extern void pausecomm();
+extern void continuecomm();
+static void UseMap(int index)
+{
+    pcl::PointCloud<pcl::PointXYZ>::Ptr xmap;
+
+    xmap = boost::shared_ptr<pcl::PointCloud<pcl::PointXYZ>>(new pcl::PointCloud<pcl::PointXYZ>);
+
+
+    QTime xTime;
+    xTime.start();
+    if(0 == pcl::io::loadPCDFile(gvector_trace.at(index).mstrpcdpath.data(),*xmap))
+    {
+
+    }
+    else
+    {
+        std::cout<<"map size is 0"<<std::endl;
+        return;
+    }
+
+    qDebug("load map time is %d",xTime.elapsed());
+
+    pcl::PointCloud<pcl::PointXYZ>::Ptr map_ptr(new pcl::PointCloud<pcl::PointXYZ>(*xmap));
+//    gvectoranh.push_back(new_anh_gpu_ndt_ptr);
+
+    qDebug("ndt load finish time is %d",xTime.elapsed());
+
+    gcurmapindex = index;
+
+    std::cout<<" max threads. "<<omp_get_max_threads()<<std::endl;
+
+    int nthread = omp_get_max_threads();
+    nthread = nthread - 2;
+    if(nthread <1)nthread = 1;
+
+    std::cout<<" use threads: "<<nthread<<std::endl;
+
+    pclomp::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>::Ptr localndt_omp(new pclomp::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>());
+
+
+
+    localndt_omp->setNumThreads(nthread);
+    localndt_omp->setNeighborhoodSearchMethod(pclomp::DIRECT7);
+
+
+    localndt_omp->setResolution(ndt_res);
+    localndt_omp->setInputTarget(map_ptr);
+    localndt_omp->setMaximumIterations(max_iter);
+    localndt_omp->setStepSize(step_size);
+    localndt_omp->setTransformationEpsilon(trans_eps);
+
+    pthread_mutex_lock(&mutex);
+//        ndt = glocal_ndt;
+    glocalndt_omp = localndt_omp;
+    pthread_mutex_unlock(&mutex);
+
+    gbNeedUpdate = true;
+
+
+    std::cout<<" ndt_omp init successfully. "<<std::endl;
+
+
+    std::cout<<"change map, index is "<<index<<std::endl;
+}
+
+void LocalMapUpdate(int n)
+{
+    std::cout<<"LocalMap n is "<<n<<std::endl;
+
+    int index;
+    double ftracedis;
+    int ncurindex = -1;
+
+    int i;
+    for(i=0;i<gvector_trace.size();i++)
+    {
+//        UseMap(i);
+//        std::this_thread::sleep_for(std::chrono::milliseconds(1000));
+    }
+    while(gbLMRun)
+    {
+
+        getmindistrace(index,ftracedis);
+
+        if(g_hasmap == false)
+        {
+
+            if(index >= 0)
+            {
+                if(ftracedis < 30)
+                {
+                    UseMap(index);
+                    ncurindex = index;
+                    g_hasmap = true;
+                }
+            }
+        }
+        else
+        {
+           if(index != ncurindex)
+           {
+                pose posex = CalcPose(gvector_trace[ncurindex].mndtorigin,gcurndtgpspos);
+               double fnowdis = gettracedis(ncurindex,posex);
+               if((fnowdis - ftracedis)>5)
+               {
+                   UseMap(index);
+                   ncurindex = index;
+                   g_hasmap = true;
+               }
+           }
+        }
+
+        if(ftracedis > 50)
+        {
+            std::cout<<std::chrono::system_clock::now().time_since_epoch().count()/1000000<<" trace dis is "<<ftracedis<<std::endl;
+            std::cout<<" Out Range."<<std::endl;
+            g_hasmap = false;
+        }
+
+
+        std::this_thread::sleep_for(std::chrono::milliseconds(100));
+    }
+}
+
+
+void SaveMonitor(bool * pbRun)
+{
+    iv::gpspos xoldgpspos;
+    xoldgpspos.lat = 39;
+    xoldgpspos.lon = 117;
+    while(*pbRun)
+    {
+       if(gpFileLastPos != 0)
+       {
+           if(gbFileNDTUpdate)
+           {
+               if((fabs(xoldgpspos.lat - gcurndtgpspos.lat)>0.0000005)||((fabs(xoldgpspos.lon - gcurndtgpspos.lon)>0.0000005)))
+               {
+                   xoldgpspos = gcurndtgpspos;
+                   char str[1000];
+                   snprintf(str,1000,"%11.7f\t%11.7f\t%9.3f\t%6.2f\t%6.2f\t%6.2f\n                ",
+                            xoldgpspos.lon,xoldgpspos.lat,xoldgpspos.height,
+                            xoldgpspos.heading,xoldgpspos.pitch,xoldgpspos.roll);
+                   gpFileLastPos->seek(0);
+                   gpFileLastPos->write(str,strnlen(str,1000));
+                   gpFileLastPos->flush();
+               }
+               gbFileNDTUpdate = false;
+           }
+       }
+       std::this_thread::sleep_for(std::chrono::milliseconds(100));
+    }
+}
+
+void GPSPosMonitor(bool * pbRun)
+{
+    if(gbGPSFix == false)
+    {
+        gcurndtgpspos =  glastndtgpspos;
+    }
+    while(*pbRun)
+    {
+        int64_t nmsnow = std::chrono::system_clock::now().time_since_epoch().count()/1000000;
+        if(abs(nmsnow - gnLastGPSTime)>1000)
+        {
+            std::this_thread::sleep_for(std::chrono::milliseconds(10));
+            continue;
+        }
+        if(gbGPSFix == true)
+        {
+            double x0,y0;
+            double x,y;
+            GaussProjCal(gcurndtgpspos.lon,gcurndtgpspos.lat,&x0,&y0);
+            GaussProjCal(gcurgpsgpspos.lon,gcurgpsgpspos.lat,&x,&y);
+            double dis = sqrt(pow(x-x0,2)+ pow(y-y0,2));
+            double headdiff = fabs(gcurgpsgpspos.heading - gcurndtgpspos.heading);
+            double zdiff = fabs(gcurgpsgpspos.height - gcurndtgpspos.height);
+            if((dis> 10)||((headdiff>10)&&(headdiff<350))||(zdiff>5)||(g_hasmap == false)||(gbNeedGPSUpdateNDT))
+            {
+                givlog->info("NDTFIX","gps fix ndt pos. dis:%f headdiff:%f zdiff:%f map %d ndt %d",
+                             dis,headdiff,zdiff,g_hasmap,gbNeedGPSUpdateNDT);
+//                gcurndtgpspos = gcurgpsgpspos;
+                  gbgpsupdatendt = true;
+//                current_velocity_x = 0;
+//                current_velocity_y = 0;
+//                current_velocity_z = 0;
+//                angular_velocity = 0;
+//                gbNeedGPSUpdateNDT = false;
+                if(g_hasmap == true)
+                {
+                    int index = gcurmapindex;
+                    if((index >=0)&&(index < gvector_trace.size()))
+                    {
+
+                    }
+                }
+            }
+//            else
+//            {
+//                if(gbgpsupdatendt)
+//                {
+//                    gcurndtgpspos = gcurgpsgpspos;
+//                    gbgpsupdatendt = true;
+//                    current_velocity_x = 0;
+//                    current_velocity_y = 0;
+//                    current_velocity_z = 0;
+//                    angular_velocity = 0;
+//                }
+//            }
+
+        }
+
+        std::this_thread::sleep_for(std::chrono::milliseconds(10));
+    }
+}
+
+
+void ndt_match_Init(pcl::PointCloud<pcl::PointXYZ>::Ptr mappcd)
+{
+    _tf_x = 0;
+    _tf_y = 0;
+    _tf_z = 0;
+    _tf_roll = 0;
+    _tf_pitch = 0;
+    _tf_yaw = 0;
+
+    Eigen::Translation3f tl_btol(_tf_x, _tf_y, _tf_z);                 // tl: translation
+    Eigen::AngleAxisf rot_x_btol(_tf_roll, Eigen::Vector3f::UnitX());  // rot: rotation
+    Eigen::AngleAxisf rot_y_btol(_tf_pitch, Eigen::Vector3f::UnitY());
+    Eigen::AngleAxisf rot_z_btol(_tf_yaw, Eigen::Vector3f::UnitZ());
+    tf_btol = (tl_btol * rot_z_btol * rot_y_btol * rot_x_btol).matrix();
+
+
+
+    init_pos_set = 1;
+
+    initial_pose.x = 0;
+    initial_pose.y = 0;
+    initial_pose.z = 0;
+    initial_pose.roll = 0;
+    initial_pose.pitch = 0;
+    initial_pose.yaw = 0;
+
+    localizer_pose.x = initial_pose.x;
+    localizer_pose.y = initial_pose.y;
+    localizer_pose.z = initial_pose.z;
+    localizer_pose.roll = initial_pose.roll;
+    localizer_pose.pitch = initial_pose.pitch;
+    localizer_pose.yaw = initial_pose.yaw;
+
+    previous_pose.x = initial_pose.x;
+    previous_pose.y = initial_pose.y;
+    previous_pose.z = initial_pose.z;
+    previous_pose.roll = initial_pose.roll;
+    previous_pose.pitch = initial_pose.pitch;
+    previous_pose.yaw = initial_pose.yaw;
+
+    current_pose.x = initial_pose.x;
+    current_pose.y = initial_pose.y;
+    current_pose.z = initial_pose.z;
+    current_pose.roll = initial_pose.roll;
+    current_pose.pitch = initial_pose.pitch;
+    current_pose.yaw = initial_pose.yaw;
+
+    current_velocity = 0;
+    current_velocity_x = 0;
+    current_velocity_y = 0;
+    current_velocity_z = 0;
+    angular_velocity = 0;
+
+
+
+
+//    ndt.align(*output_cloud, Eigen::Matrix4f::Identity());
+
+
+    pcl::PointCloud<pcl::PointXYZ>::Ptr map_ptr(new pcl::PointCloud<pcl::PointXYZ>(*mappcd));
+
+
+    std::cout<<"map size is"<<mappcd->size()<<std::endl;
+    std::cout<<"ptr size is "<<map_ptr->size()<<std::endl;;
+
+    global_map_ptr = map_ptr;
+
+ //   SetLocalMap();
+
+
+//    pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_scan(new pcl::PointCloud<pcl::PointXYZ>());
+
+//    pcl::VoxelGrid<pcl::PointXYZ> voxel_grid_filter;
+//    voxel_grid_filter.setLeafSize(1.0,1.0,1.0);
+//    voxel_grid_filter.setInputCloud(map_ptr);
+//    voxel_grid_filter.filter(*filtered_scan);
+
+//    std::cout<<"filter map  size is "<<filtered_scan->size()<<std::endl;;
+
+ //   ndt.setInputTarget(map_ptr);
+//    pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud(new pcl::PointCloud<pcl::PointXYZ>);
+//    ndt.align(*output_cloud, Eigen::Matrix4f::Identity());
+
+
+    gpset = iv::modulecomm::RegisterRecv("ndtposeset",ListenPoseSet);
+    gppose = iv::modulecomm::RegisterSend("ndtpose",10000,3);
+
+    if(map_ptr->size() == 0)
+    {
+        gbNDTRun = false;
+        return;
+    }
+
+
+
+    pcl::PointCloud<pcl::PointXYZ>::Ptr dummy_scan_ptr(new pcl::PointCloud<pcl::PointXYZ>());
+    pcl::PointXYZ dummy_point;
+    dummy_scan_ptr->push_back(dummy_point);
+
+
+    ndt_omp->setResolution(ndt_res);
+    ndt_omp->setInputTarget(map_ptr);
+    ndt_omp->setMaximumIterations(max_iter);
+    ndt_omp->setStepSize(step_size);
+    ndt_omp->setTransformationEpsilon(trans_eps);
+
+    ndt_omp->setNumThreads(6);
+    ndt_omp->setNeighborhoodSearchMethod(pclomp::DIRECT7);
+
+    std::cout<<" ndt_omp init success. "<<std::endl;
+
+
+
+    if(map_ptr->size()>0)map_loaded = 1;
+
+ //   gpmapthread = new std::thread(LocalMapUpdate,1);
+
+
+
+
+}
+
+void ndt_match_Init_nomap()
+{
+    _tf_x = 0;
+    _tf_y = 0;
+    _tf_z = 0;
+    _tf_roll = 0;
+    _tf_pitch = 0;
+    _tf_yaw = 0;
+
+    Eigen::Translation3f tl_btol(_tf_x, _tf_y, _tf_z);                 // tl: translation
+    Eigen::AngleAxisf rot_x_btol(_tf_roll, Eigen::Vector3f::UnitX());  // rot: rotation
+    Eigen::AngleAxisf rot_y_btol(_tf_pitch, Eigen::Vector3f::UnitY());
+    Eigen::AngleAxisf rot_z_btol(_tf_yaw, Eigen::Vector3f::UnitZ());
+    tf_btol = (tl_btol * rot_z_btol * rot_y_btol * rot_x_btol).matrix();
+
+
+
+    init_pos_set = 1;
+
+
+
+
+    initial_pose.x = 0;
+    initial_pose.y = 0;
+    initial_pose.z = 0;
+    initial_pose.roll = 0;
+    initial_pose.pitch = 0;
+    initial_pose.yaw = 0;
+
+    localizer_pose.x = initial_pose.x;
+    localizer_pose.y = initial_pose.y;
+    localizer_pose.z = initial_pose.z;
+    localizer_pose.roll = initial_pose.roll;
+    localizer_pose.pitch = initial_pose.pitch;
+    localizer_pose.yaw = initial_pose.yaw;
+
+    previous_pose.x = initial_pose.x;
+    previous_pose.y = initial_pose.y;
+    previous_pose.z = initial_pose.z;
+    previous_pose.roll = initial_pose.roll;
+    previous_pose.pitch = initial_pose.pitch;
+    previous_pose.yaw = initial_pose.yaw;
+
+    current_pose.x = initial_pose.x;
+    current_pose.y = initial_pose.y;
+    current_pose.z = initial_pose.z;
+    current_pose.roll = initial_pose.roll;
+    current_pose.pitch = initial_pose.pitch;
+    current_pose.yaw = initial_pose.yaw;
+
+    current_velocity = 0;
+    current_velocity_x = 0;
+    current_velocity_y = 0;
+    current_velocity_z = 0;
+    angular_velocity = 0;
+
+
+    gpmapthread = new std::thread(LocalMapUpdate,1);
+    gbGFRun = true;
+    gpgpsfixthread = new std::thread(GPSPosMonitor,&gbGFRun);
+    gpsavelastthread = new std::thread(SaveMonitor,&gbGFRun);
+
+
+
+
+}
+
+
+void ndt_match_SetMap(pcl::PointCloud<pcl::PointXYZ>::Ptr mappcd)
+{
+    if(mappcd->size() == 0 )return;
+
+    pcl::PointCloud<pcl::PointXYZ>::Ptr map_ptr(new pcl::PointCloud<pcl::PointXYZ>(*mappcd));
+
+
+
+
+    pcl::PointCloud<pcl::PointXYZ>::Ptr dummy_scan_ptr(new pcl::PointCloud<pcl::PointXYZ>());
+    pcl::PointXYZ dummy_point;
+    dummy_scan_ptr->push_back(dummy_point);
+
+
+
+
+
+
+
+    if(map_ptr->size()>0)map_loaded = 1;
+}
+
+
+
+static double wrapToPm(double a_num, const double a_max)
+{
+  if (a_num >= a_max)
+  {
+    a_num -= 2.0 * a_max;
+  }
+  return a_num;
+}
+
+static double wrapToPmPi(const double a_angle_rad)
+{
+  return wrapToPm(a_angle_rad, M_PI);
+}
+
+static double calcDiffForRadian(const double lhs_rad, const double rhs_rad)
+{
+  double diff_rad = lhs_rad - rhs_rad;
+  if (diff_rad >= M_PI)
+    diff_rad = diff_rad - 2 * M_PI;
+  else if (diff_rad < -M_PI)
+    diff_rad = diff_rad + 2 * M_PI;
+  return diff_rad;
+}
+
+
+static unsigned int g_seq_old = 0;
+
+#include <QTime>
+
+
+
+bool isfirst = true;
+
+#include <QMutex>
+std::vector<iv::imudata> gvectorimu;
+QMutex gMuteximu;
+
+static void lidar_imu_calc(qint64 current_lidar_time)
+{
+    int nsize;
+
+    int i;
+    gMuteximu.lock();
+    nsize = gvectorimu.size();
+    for(i=0;i<nsize;i++)
+    {
+        iv::imudata ximudata = gvectorimu[i];
+        double ximu_angular_velocity_x,ximu_angular_velocity_y,ximu_angular_velocity_z;
+        double ximu_linear_acceleration_x,ximu_linear_acceleration_y,ximu_linear_acceleration_z;
+        if(current_lidar_time >  ximudata.imutime)
+        {
+            ximu_angular_velocity_x = ximudata.imu_angular_velocity_x;
+            ximu_angular_velocity_y = ximudata.imu_angular_velocity_y;
+            ximu_angular_velocity_z = ximudata.imu_angular_velocity_z;
+            ximu_linear_acceleration_x = ximudata.imu_linear_acceleration_x;
+            ximu_linear_acceleration_y = ximudata.imu_linear_acceleration_y;
+            ximu_linear_acceleration_z = ximudata.imu_linear_acceleration_z;
+            qint64 current_time_imu = ximudata.imutime;
+//            givlog->verbose("NDT","imu time is %ld",current_time_imu);
+            static qint64 previous_time_imu = current_time_imu;
+            double diff_time = (current_time_imu - previous_time_imu);
+            diff_time = diff_time/1000.0;
+
+            if(diff_time < 0)diff_time = 0.000001;
+            if(diff_time > 0.1)diff_time = 0.1;
+
+            if(current_time_imu < previous_time_imu)
+            {
+
+                std::cout<<"current time is old "<<current_time_imu<<std::endl;
+                previous_time_imu = current_time_imu;
+                continue;
+            }
+
+            double diff_imu_roll = ximu_angular_velocity_x * diff_time;
+            double diff_imu_pitch = ximu_angular_velocity_y * diff_time;
+            double diff_imu_yaw = ximu_angular_velocity_z * diff_time;
+
+            current_pose_imu.roll += diff_imu_roll;
+            current_pose_imu.pitch += diff_imu_pitch;
+            current_pose_imu.yaw += diff_imu_yaw;
+
+            double accX1 = ximu_linear_acceleration_x;
+            double accY1 = std::cos(current_pose_imu.roll) * ximu_linear_acceleration_y -
+                           std::sin(current_pose_imu.roll) * ximu_linear_acceleration_z;
+            double accZ1 = std::sin(current_pose_imu.roll) * ximu_linear_acceleration_y +
+                           std::cos(current_pose_imu.roll) * ximu_linear_acceleration_z;
+
+            double accX2 = std::sin(current_pose_imu.pitch) * accZ1 + std::cos(current_pose_imu.pitch) * accX1;
+            double accY2 = accY1;
+            double accZ2 = std::cos(current_pose_imu.pitch) * accZ1 - std::sin(current_pose_imu.pitch) * accX1;
+
+            double accX = std::cos(current_pose_imu.yaw) * accX2 - std::sin(current_pose_imu.yaw) * accY2;
+            double accY = std::sin(current_pose_imu.yaw) * accX2 + std::cos(current_pose_imu.yaw) * accY2;
+            double accZ = accZ2;
+
+            offset_imu_x += current_velocity_imu_x * diff_time + accX * diff_time * diff_time / 2.0;
+            offset_imu_y += current_velocity_imu_y * diff_time + accY * diff_time * diff_time / 2.0;
+            offset_imu_z += current_velocity_imu_z * diff_time + accZ * diff_time * diff_time / 2.0;
+
+            current_velocity_imu_x += accX * diff_time;
+            current_velocity_imu_y += accY * diff_time;
+            current_velocity_imu_z += accZ * diff_time;
+
+            offset_imu_roll += diff_imu_roll;
+            offset_imu_pitch += diff_imu_pitch;
+            offset_imu_yaw += diff_imu_yaw;
+
+          //  guess_pose_imu.x = previous_pose.x + offset_imu_x;
+          //  guess_pose_imu.y = previous_pose.y + offset_imu_y;
+          //  guess_pose_imu.z = previous_pose.z + offset_imu_z;
+          //  guess_pose_imu.roll = previous_pose.roll + offset_imu_roll;
+          //  guess_pose_imu.pitch = previous_pose.pitch + offset_imu_pitch;
+          //  guess_pose_imu.yaw = previous_pose.yaw + offset_imu_yaw;
+
+            predict_pose_imu.x = previous_pose.x + offset_imu_x;
+            predict_pose_imu.y = previous_pose.y + offset_imu_y;
+            predict_pose_imu.z = previous_pose.z + offset_imu_z;
+            predict_pose_imu.roll = previous_pose.roll + offset_imu_roll;
+            predict_pose_imu.pitch = previous_pose.pitch + offset_imu_pitch;
+            predict_pose_imu.yaw = previous_pose.yaw + offset_imu_yaw;
+
+            givlog->verbose("NDT","imu x:%f y:%f z:%f yaw:%f",offset_imu_x,
+                            offset_imu_y,offset_imu_z,offset_imu_yaw);
+
+            previous_time_imu = current_time_imu;
+
+
+        }
+        else
+        {
+            break;;
+        }
+
+    }
+
+    if(i>0)
+    {
+        gvectorimu.erase(gvectorimu.begin(),gvectorimu.begin()+i);
+    }
+
+    gMuteximu.unlock();
+
+//    for(i=0;i<gvectorimu.size();i++)
+//    {
+//        iv::imudata ximudata = gvectorimu[i];
+//        double ximu_angular_velocity_x,ximu_angular_velocity_y,ximu_angular_velocity_z;
+//        double ximu_linear_acceleration_x,ximu_linear_acceleration_y,ximu_linear_acceleration_z;
+//        if(current_lidar_time >  ximudata.imutime)
+//        {
+
+//            gvectorimu.erase(gvectorimu.begin())
+//        }
+//    }
+}
+
+
+static void imu_calc(qint64 current_time_imu)
+{
+
+  static qint64 previous_time_imu = current_time_imu;
+  double diff_time = (current_time_imu - previous_time_imu);
+  diff_time = diff_time/1000.0;
+
+  if(current_time_imu < previous_time_imu)
+  {
+      std::cout<<"current time is old "<<current_time_imu<<std::endl;
+      return;
+  }
+
+  double diff_imu_roll = imu_angular_velocity_x * diff_time;
+  double diff_imu_pitch = imu_angular_velocity_y * diff_time;
+  double diff_imu_yaw = imu_angular_velocity_z * diff_time;
+
+  current_pose_imu.roll += diff_imu_roll;
+  current_pose_imu.pitch += diff_imu_pitch;
+  current_pose_imu.yaw += diff_imu_yaw;
+
+  double accX1 = imu_linear_acceleration_x;
+  double accY1 = std::cos(current_pose_imu.roll) * imu_linear_acceleration_y -
+                 std::sin(current_pose_imu.roll) * imu_linear_acceleration_z;
+  double accZ1 = std::sin(current_pose_imu.roll) * imu_linear_acceleration_y +
+                 std::cos(current_pose_imu.roll) * imu_linear_acceleration_z;
+
+  double accX2 = std::sin(current_pose_imu.pitch) * accZ1 + std::cos(current_pose_imu.pitch) * accX1;
+  double accY2 = accY1;
+  double accZ2 = std::cos(current_pose_imu.pitch) * accZ1 - std::sin(current_pose_imu.pitch) * accX1;
+
+  double accX = std::cos(current_pose_imu.yaw) * accX2 - std::sin(current_pose_imu.yaw) * accY2;
+  double accY = std::sin(current_pose_imu.yaw) * accX2 + std::cos(current_pose_imu.yaw) * accY2;
+  double accZ = accZ2;
+
+  offset_imu_x += current_velocity_imu_x * diff_time + accX * diff_time * diff_time / 2.0;
+  offset_imu_y += current_velocity_imu_y * diff_time + accY * diff_time * diff_time / 2.0;
+  offset_imu_z += current_velocity_imu_z * diff_time + accZ * diff_time * diff_time / 2.0;
+
+  current_velocity_imu_x += accX * diff_time;
+  current_velocity_imu_y += accY * diff_time;
+  current_velocity_imu_z += accZ * diff_time;
+
+  offset_imu_roll += diff_imu_roll;
+  offset_imu_pitch += diff_imu_pitch;
+  offset_imu_yaw += diff_imu_yaw;
+
+//  guess_pose_imu.x = previous_pose.x + offset_imu_x;
+//  guess_pose_imu.y = previous_pose.y + offset_imu_y;
+//  guess_pose_imu.z = previous_pose.z + offset_imu_z;
+//  guess_pose_imu.roll = previous_pose.roll + offset_imu_roll;
+//  guess_pose_imu.pitch = previous_pose.pitch + offset_imu_pitch;
+//  guess_pose_imu.yaw = previous_pose.yaw + offset_imu_yaw;
+
+  predict_pose_imu.x = previous_pose.x + offset_imu_x;
+  predict_pose_imu.y = previous_pose.y + offset_imu_y;
+  predict_pose_imu.z = previous_pose.z + offset_imu_z;
+  predict_pose_imu.roll = previous_pose.roll + offset_imu_roll;
+  predict_pose_imu.pitch = previous_pose.pitch + offset_imu_pitch;
+  predict_pose_imu.yaw = previous_pose.yaw + offset_imu_yaw;
+
+  givlog->verbose("NDT","imu x:%f y:%f z:%f yaw:%f",offset_imu_x,
+                  offset_imu_y,offset_imu_z,offset_imu_yaw);
+
+  previous_time_imu = current_time_imu;
+}
+
+
+void imu_update(qint64 current_time_imu,double imu_roll,double imu_pitch,double imu_yaw,
+                       double acceleration_x,double acceleration_y,
+                       double acceleration_z)
+{
+  // std::cout << __func__ << std::endl;
+
+// double imu_angular_velocity_x,imu_angular_velocity_y,imu_angular_velocity_z;
+
+  static double previous_time_imu = current_time_imu;
+  double diff_time = (current_time_imu - previous_time_imu);
+  diff_time = diff_time/1000.0;
+
+
+  imu_roll = imu_roll * M_PI/180.0;
+  imu_pitch = imu_pitch * M_PI/180.0;
+  imu_yaw = (-1.0)*imu_yaw * M_PI/180.0;
+
+  imu_yaw = imu_yaw + 2.0*M_PI;
+
+//  imu_pitch = 0;
+//  imu_roll = 0;
+
+  imu_roll = wrapToPmPi(imu_roll);
+  imu_pitch = wrapToPmPi(imu_pitch);
+  imu_yaw = wrapToPmPi(imu_yaw);
+
+  static double previous_imu_roll = imu_roll, previous_imu_pitch = imu_pitch, previous_imu_yaw = imu_yaw;
+  const double diff_imu_roll = imu_roll - previous_imu_roll;
+
+  const double diff_imu_pitch = imu_pitch - previous_imu_pitch;
+
+  double diff_imu_yaw;
+  if (fabs(imu_yaw - previous_imu_yaw) > M_PI)
+  {
+    if (imu_yaw > 0)
+      diff_imu_yaw = (imu_yaw - previous_imu_yaw) - M_PI * 2;
+    else
+      diff_imu_yaw = -M_PI * 2 - (imu_yaw - previous_imu_yaw);
+  }
+  else
+    diff_imu_yaw = imu_yaw - previous_imu_yaw;
+
+
+  imu_linear_acceleration_x = acceleration_x;
+//  imu_linear_acceleration_y = acceleration_y;
+//  imu_linear_acceleration_z = acceleration_z;
+  imu_linear_acceleration_y = 0;
+  imu_linear_acceleration_z = 0;
+
+  if (diff_time != 0)
+  {
+    imu_angular_velocity_x = diff_imu_roll / diff_time;
+    imu_angular_velocity_y = diff_imu_pitch / diff_time;
+    imu_angular_velocity_z = diff_imu_yaw / diff_time;
+  }
+  else
+  {
+    imu_angular_velocity_x = 0;
+    imu_angular_velocity_y = 0;
+    imu_angular_velocity_z = 0;
+  }
+
+  iv::imudata ximudata;
+  ximudata.imutime = current_time_imu;
+  ximudata.imu_linear_acceleration_x = imu_linear_acceleration_x;
+  ximudata.imu_linear_acceleration_y = imu_linear_acceleration_y;
+  ximudata.imu_linear_acceleration_z = imu_linear_acceleration_z;
+  ximudata.imu_angular_velocity_x = imu_angular_velocity_x;
+  ximudata.imu_angular_velocity_y = imu_angular_velocity_y;
+  ximudata.imu_angular_velocity_z = imu_angular_velocity_z;
+
+  gMuteximu.lock();
+  gvectorimu.push_back(ximudata);
+  gMuteximu.unlock();
+//  imu_calc(current_time_imu);
+
+  previous_time_imu = current_time_imu;
+  previous_imu_roll = imu_roll;
+  previous_imu_pitch = imu_pitch;
+  previous_imu_yaw = imu_yaw;
+}
+
+
+#ifdef TEST_CALCTIME
+int ncalc = 0;
+int gncalctotal = 0;
+#endif
+
+void point_ndt(pcl::PointCloud<pcl::PointXYZ>::Ptr raw_scan)
+{
+
+    static bool bNeedUpdateVel = false; //if gps update ndt, need update velocity
+    qint64 current_scan_time = raw_scan->header.stamp;
+    static qint64 old_scan_time = current_scan_time;
+    if(gbNDTRun == false)return;
+
+    bool bNotChangev = false;
+
+    if(gbgpsupdatendt == true)
+    {
+
+        std::cout<<std::chrono::system_clock::now().time_since_epoch().count()/1000000<<" update ndt pos use gps pos. "<<std::endl;
+        gcurndtgpspos = gcurgpsgpspos;
+        gbgpsupdatendt = false;
+        gbNeedGPSUpdateNDT = false;
+        current_velocity_x = 0;
+        current_velocity_y = 0;
+        current_velocity_z = 0;
+        angular_velocity = 0;
+        bNeedUpdateVel = true;
+
+//
+//
+//        gcurndtgpspos = gcurgpsgpspos;
+//        current_pose = CalcPose(gvector_trace[gusemapindex].mndtorigin,gcurndtgpspos);
+//       predict_pose_for_ndt = current_pose;
+        current_velocity_imu_x = 0;
+        current_velocity_imu_y = 0;
+        current_velocity_imu_z = 0;
+        gMuteximu.lock();
+        gvectorimu.erase(gvectorimu.begin(),gvectorimu.begin() + gvectorimu.size());
+        gMuteximu.unlock();
+        bNotChangev = true;
+        return;
+    }
+//    gbgpsupdatendt = false;
+
+    if(g_hasmap == false)
+    {
+        return;
+    }
+    if(gbNeedUpdate)
+    {
+        pthread_mutex_lock(&mutex);
+        ndt_omp = glocalndt_omp;
+        pthread_mutex_unlock(&mutex);
+        gusemapindex = gcurmapindex;
+        gbNeedUpdate = false;
+
+    }
+
+
+
+
+
+    previous_pose = CalcPose(gvector_trace[gusemapindex].mndtorigin,gcurndtgpspos);
+    if(bNeedUpdateVel == true)
+    {
+        bNeedUpdateVel = false;
+        current_velocity_x = previous_pose.vx;
+        current_velocity_y = previous_pose.vy;
+        current_velocity_imu_x = current_velocity_x;
+        current_velocity_imu_y = current_velocity_y;
+    }
+    givlog->verbose("previos pose is %f %f",previous_pose.x,previous_pose.y);
+
+//    if(map_loaded == 0)
+//    {
+//        std::cout<<BOLDRED<<" point_ndt not set map."<<RESET<<std::endl;
+//        return;
+//    }
+
+
+
+    pthread_mutex_lock(&mutex_pose);
+
+    QTime xTime;
+    xTime.start();
+//    std::cout<<"time is "<<xTime.elapsed()<<std::endl;
+    double voxel_leaf_size = 2.0;
+    double measurement_range = 200.0;
+
+    pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_scan(new pcl::PointCloud<pcl::PointXYZ>());
+
+    pcl::VoxelGrid<pcl::PointXYZ> voxel_grid_filter;
+    voxel_grid_filter.setLeafSize(voxel_leaf_size, voxel_leaf_size, voxel_leaf_size);
+    voxel_grid_filter.setInputCloud(raw_scan);
+    voxel_grid_filter.filter(*filtered_scan);
+
+//    std::cout<<"voxed time is "<<xTime.elapsed()<<std::endl;
+
+//    std::cout<<" seq is "<<raw_scan->header.seq<<std::endl;
+  //  qDebug("seq = %d stamp = %d ",raw_scan->header.seq,raw_scan->header.stamp);
+  //  std::cout<<"raw scan size is "<<raw_scan->size()<<"  filtered scan size is "<<filtered_scan->size()<<std::endl;
+
+ //   return;
+    matching_start = std::chrono::system_clock::now();
+
+    pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_scan_ptr(new pcl::PointCloud<pcl::PointXYZ>(*filtered_scan));
+
+    int scan_points_num = filtered_scan_ptr->size();
+
+    Eigen::Matrix4f t(Eigen::Matrix4f::Identity());   // base_link
+    Eigen::Matrix4f t2(Eigen::Matrix4f::Identity());  // localizer
+
+    std::chrono::time_point<std::chrono::system_clock> align_start, align_end, getFitnessScore_start,
+        getFitnessScore_end;
+    static double align_time, getFitnessScore_time = 0.0;
+
+//    std::cout<<"run hear"<<std::endl;
+    pthread_mutex_lock(&mutex);
+
+//    if (_method_type == MethodType::PCL_GENERIC)
+ //   ndt.setInputSource(filtered_scan_ptr);
+
+
+    ndt_omp->setInputSource(filtered_scan_ptr);
+
+    // Guess the initial gross estimation of the transformation
+//    double diff_time = (current_scan_time - previous_scan_time).toSec();
+
+
+    double diff_time = 0.0;
+
+    if(g_seq_old != 0)
+    {
+        if((raw_scan->header.seq - g_seq_old)>0)
+        {
+            diff_time = raw_scan->header.seq - g_seq_old;
+            diff_time = diff_time * 0.1;
+        }
+    }
+
+    g_seq_old = raw_scan->header.seq;
+
+    diff_time = current_scan_time -old_scan_time;
+    diff_time = diff_time/1000.0;
+    old_scan_time = current_scan_time;
+
+    if(diff_time<=0)diff_time = 0.1;
+    if(diff_time>1.0)diff_time = 0.1;
+
+//    std::cout<<"diff time is "<<diff_time<<std::endl;
+
+//    std::cout<<" diff time is "<<diff_time<<std::endl;
+
+//    diff_time = 0.0;
+
+//    if (_offset == "linear")
+//    {
+
+      if(diff_time<0.1)diff_time = 0.1;
+      offset_x = current_velocity_x * diff_time;
+      offset_y = current_velocity_y * diff_time;
+      offset_z = current_velocity_z * diff_time;
+      offset_yaw = angular_velocity * diff_time;
+//    }
+      predict_pose.x = previous_pose.x + offset_x;
+      predict_pose.y = previous_pose.y + offset_y;
+      predict_pose.z = previous_pose.z + offset_z;
+      predict_pose.roll = previous_pose.roll;
+      predict_pose.pitch = previous_pose.pitch;
+      predict_pose.yaw = previous_pose.yaw + offset_yaw;
+
+      pose predict_pose_for_ndt;
+
+
+      givlog->verbose("NDT","previous x:%f y:%f z:%f yaw:%f",previous_pose.x,
+                      previous_pose.y,previous_pose.z,previous_pose.yaw);
+//      if (_use_imu == true && _use_odom == true)
+//         imu_odom_calc(current_scan_time);
+       if (_use_imu == true && _use_odom == false)
+       {
+         lidar_imu_calc((current_scan_time-0));
+       }
+//       if (_use_imu == false && _use_odom == true)
+//         odom_calc(current_scan_time);
+
+//       if (_use_imu == true && _use_odom == true)
+//         predict_pose_for_ndt = predict_pose_imu_odom;
+//       else
+//           if (_use_imu == true && _use_odom == false)
+//         predict_pose_for_ndt = predict_pose_imu;
+//       else if (_use_imu == false && _use_odom == true)
+//         predict_pose_for_ndt = predict_pose_odom;
+//       else
+//         predict_pose_for_ndt = predict_pose;
+
+      if (_use_imu == true && _use_odom == false)
+      {
+         predict_pose_for_ndt = predict_pose_imu;
+ //         predict_pose_for_ndt = predict_pose;
+ //         predict_pose_for_ndt.yaw = predict_pose_imu.yaw;
+      }
+      else
+      {
+          predict_pose_for_ndt = predict_pose;
+      }
+
+      predict_pose_for_ndt = predict_pose; //not use predict_pose_imu in shelan.
+
+      if(fabs(predict_pose_for_ndt.x -previous_pose.x)>10)
+      {
+          predict_pose_for_ndt = previous_pose;
+      }
+
+      if(fabs(predict_pose_for_ndt.y -previous_pose.y)>10)
+      {
+          predict_pose_for_ndt = previous_pose;
+      }
+
+ //     predict_pose_for_ndt = predict_pose;
+
+
+
+
+      givlog->verbose("NDT","pre x:%f y:%f z:%f yaw:%f",predict_pose_for_ndt.x,
+                      predict_pose_for_ndt.y,predict_pose_for_ndt.z,predict_pose_for_ndt.yaw);
+//      predict_pose_for_ndt = predict_pose;
+
+      Eigen::Translation3f init_translation(predict_pose_for_ndt.x, predict_pose_for_ndt.y, predict_pose_for_ndt.z);
+      Eigen::AngleAxisf init_rotation_x(predict_pose_for_ndt.roll, Eigen::Vector3f::UnitX());
+      Eigen::AngleAxisf init_rotation_y(predict_pose_for_ndt.pitch, Eigen::Vector3f::UnitY());
+      Eigen::AngleAxisf init_rotation_z(predict_pose_for_ndt.yaw, Eigen::Vector3f::UnitZ());
+      Eigen::Matrix4f init_guess = (init_translation * init_rotation_z * init_rotation_y * init_rotation_x) * tf_btol;
+
+      pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud(new pcl::PointCloud<pcl::PointXYZ>);
+
+ //     std::cout<<"before ndt time is "<<xTime.elapsed()<<std::endl;
+//      std::cout<<"time is "<<xTime.elapsed()<<std::endl;
+      pcl::PointCloud<pcl::PointXYZ>::Ptr aligned(new pcl::PointCloud<pcl::PointXYZ>());
+      align_start = std::chrono::system_clock::now();
+      ndt_omp->align(*aligned,init_guess);
+      align_end = std::chrono::system_clock::now();
+
+      if(xTime.elapsed()<90)
+      std::cout<<GREEN<<"after ndt time is "<<xTime.elapsed()<<RESET<<std::endl;
+      else
+          std::cout<<RED<<"after ndt time is "<<xTime.elapsed()<<RESET<<std::endl;
+
+#ifdef TEST_CALCTIME
+
+    gncalctotal = gncalctotal + xTime.elapsed();
+    ncalc++;
+    if(ncalc>0)
+    {
+        std::cout<<" average calc time: "<<gncalctotal/ncalc<<std::endl;
+
+    }
+#endif
+
+      has_converged = ndt_omp->hasConverged();
+
+      t = ndt_omp->getFinalTransformation();
+      iteration = ndt_omp->getFinalNumIteration();
+
+      getFitnessScore_start = std::chrono::system_clock::now();
+      fitness_score = ndt_omp->getFitnessScore();
+      getFitnessScore_end = std::chrono::system_clock::now();
+
+      trans_probability = ndt_omp->getTransformationProbability();
+
+      std::cout<<"score: "<<fitness_score<<" trans_probability:"<<trans_probability<<std::endl;
+  //    std::cout<<" scoure is "<<fitness_score<<std::endl;
+
+//      std::cout<<" iter is "<<iteration<<std::endl;
+
+      t2 = t * tf_btol.inverse();
+
+      getFitnessScore_time =
+          std::chrono::duration_cast<std::chrono::microseconds>(getFitnessScore_end - getFitnessScore_start).count() /
+          1000.0;
+
+      pthread_mutex_unlock(&mutex);
+
+      tf::Matrix3x3 mat_l;  // localizer
+      mat_l.setValue(static_cast<double>(t(0, 0)), static_cast<double>(t(0, 1)), static_cast<double>(t(0, 2)),
+                     static_cast<double>(t(1, 0)), static_cast<double>(t(1, 1)), static_cast<double>(t(1, 2)),
+                     static_cast<double>(t(2, 0)), static_cast<double>(t(2, 1)), static_cast<double>(t(2, 2)));
+
+      // Update localizer_pose
+      localizer_pose.x = t(0, 3);
+      localizer_pose.y = t(1, 3);
+      localizer_pose.z = t(2, 3);
+      mat_l.getRPY(localizer_pose.roll, localizer_pose.pitch, localizer_pose.yaw, 1);
+
+
+//     std::cout<<" local pose x is "<<localizer_pose.x<<std::endl;
+
+      tf::Matrix3x3 mat_b;  // base_link
+      mat_b.setValue(static_cast<double>(t2(0, 0)), static_cast<double>(t2(0, 1)), static_cast<double>(t2(0, 2)),
+                     static_cast<double>(t2(1, 0)), static_cast<double>(t2(1, 1)), static_cast<double>(t2(1, 2)),
+                     static_cast<double>(t2(2, 0)), static_cast<double>(t2(2, 1)), static_cast<double>(t2(2, 2)));
+
+      // Update ndt_pose
+      ndt_pose.x = t2(0, 3);
+      ndt_pose.y = t2(1, 3);
+      ndt_pose.z = t2(2, 3);
+
+//      ndt_pose.x = localizer_pose.x;
+//      ndt_pose.y = localizer_pose.y;
+//      ndt_pose.z = localizer_pose.z;
+      mat_b.getRPY(ndt_pose.roll, ndt_pose.pitch, ndt_pose.yaw, 1);
+
+
+      givlog->verbose("NDT","calc x:%f y:%f z:%f yaw:%f",ndt_pose.x,
+                      ndt_pose.y,ndt_pose.z,ndt_pose.yaw);
+
+
+      // Calculate the difference between ndt_pose and predict_pose
+      predict_pose_error = sqrt((ndt_pose.x - predict_pose_for_ndt.x) * (ndt_pose.x - predict_pose_for_ndt.x) +
+                                (ndt_pose.y - predict_pose_for_ndt.y) * (ndt_pose.y - predict_pose_for_ndt.y) +
+                                (ndt_pose.z - predict_pose_for_ndt.z) * (ndt_pose.z - predict_pose_for_ndt.z));
+
+
+ //     std::cout<<" pose error is "<<predict_pose_error<<std::endl;
+ //     std::cout<<"ndt pose x is "<<ndt_pose.x<<std::endl;
+      if (predict_pose_error <= (PREDICT_POSE_THRESHOLD*10*diff_time))
+      {
+        use_predict_pose = 0;
+      }
+      else
+      {
+        use_predict_pose = 1;
+      }
+      use_predict_pose = 0;
+
+      if (use_predict_pose == 0)
+      {
+        current_pose.x = ndt_pose.x;
+        current_pose.y = ndt_pose.y;
+        current_pose.z = ndt_pose.z;
+        current_pose.roll = ndt_pose.roll;
+        current_pose.pitch = ndt_pose.pitch;
+        current_pose.yaw = ndt_pose.yaw;
+
+      }
+      else
+      {
+          givlog->verbose("NDT","USE PREDICT POSE");
+        current_pose.x = predict_pose_for_ndt.x;
+        current_pose.y = predict_pose_for_ndt.y;
+        current_pose.z = predict_pose_for_ndt.z;
+        current_pose.roll = predict_pose_for_ndt.roll;
+        current_pose.pitch = predict_pose_for_ndt.pitch;
+        current_pose.yaw = predict_pose_for_ndt.yaw;
+      }
+
+ //     std::cout<<" current pose x is "<<current_pose.x<<std::endl;
+
+      // Compute the velocity and acceleration
+      diff_x = current_pose.x - previous_pose.x;
+      diff_y = current_pose.y - previous_pose.y;
+      diff_z = current_pose.z - previous_pose.z;
+      diff_yaw = calcDiffForRadian(current_pose.yaw, previous_pose.yaw);
+      diff = sqrt(diff_x * diff_x + diff_y * diff_y + diff_z * diff_z);
+
+      current_velocity = (diff_time > 0) ? (diff / diff_time) : 0;
+      current_velocity_x = (diff_time > 0) ? (diff_x / diff_time) : 0;
+      current_velocity_y = (diff_time > 0) ? (diff_y / diff_time) : 0;
+      current_velocity_z = (diff_time > 0) ? (diff_z / diff_time) : 0;
+      angular_velocity = (diff_time > 0) ? (diff_yaw / diff_time) : 0;
+
+      current_pose.vx = current_velocity_x;
+      current_pose.vy = current_velocity_y;
+
+      current_velocity_smooth = (current_velocity + previous_velocity + previous_previous_velocity) / 3.0;
+      if (current_velocity_smooth < 0.2)
+      {
+        current_velocity_smooth = 0.0;
+      }
+
+ //     bNotChangev = true;
+      if((diff_time >  0)&&(bNotChangev == false))
+      {
+          double aa = (current_velocity -previous_velocity)/diff_time;
+          if(fabs(aa)>5.0)
+          {
+              givlog->verbose("NDT","aa is %f",aa);
+              aa = fabs(5.0/aa);
+              current_velocity = previous_velocity + aa*(current_velocity -previous_velocity);
+              current_velocity_x = previous_velocity_x + aa *((current_velocity_x -previous_velocity_x));
+              current_velocity_y = previous_velocity_y + aa *((current_velocity_y -previous_velocity_y));
+              current_velocity_z = previous_velocity_z + aa *((current_velocity_z -previous_velocity_z));
+
+          }
+      }
+
+      current_accel = (diff_time > 0) ? ((current_velocity - previous_velocity) / diff_time) : 0;
+      current_accel_x = (diff_time > 0) ? ((current_velocity_x - previous_velocity_x) / diff_time) : 0;
+      current_accel_y = (diff_time > 0) ? ((current_velocity_y - previous_velocity_y) / diff_time) : 0;
+      current_accel_z = (diff_time > 0) ? ((current_velocity_z - previous_velocity_z) / diff_time) : 0;
+
+ //   std::cout<<"fit time is "<<getFitnessScore_time<<std::endl;
+
+      gcurndtgpspos = PoseToGPS(gvector_trace[gcurmapindex].mndtorigin,current_pose);
+//      std::cout<<" pre x:"<<previous_pose.x<<" pre y:"<<previous_pose.y<<std::endl;
+    std::cout<<" x: "<<current_pose.x<<" y: "<<current_pose.y
+            <<" z: "<<current_pose.z<<" yaw:"<<current_pose.yaw
+           <<" vel: "<<current_velocity<<std::endl;
+
+
+    std::cout<<"NDT ve:"<<gcurndtgpspos.ve<<" vn:"<<gcurndtgpspos.vn
+            <<" GPS ve:"<<gcurgpsgpspos.ve<<" vn:"<<gcurgpsgpspos.vn<<std::endl;
+
+    gbFileNDTUpdate = true;
+
+//    gw->Updatexyyaw(current_pose.x,current_pose.y,current_pose.yaw);
+
+    current_pose_imu.x = current_pose.x;
+    current_pose_imu.y = current_pose.y;
+    current_pose_imu.z = current_pose.z;
+    current_pose_imu.roll = current_pose.roll;
+    current_pose_imu.pitch = current_pose.pitch;
+    current_pose_imu.yaw = current_pose.yaw;
+
+    current_velocity_imu_x = current_velocity_x;
+    current_velocity_imu_y = current_velocity_y;
+    current_velocity_imu_z = current_velocity_z;
+
+    current_velocity_imu_z = 0;
+
+    offset_imu_x = 0.0;
+    offset_imu_y = 0.0;
+    offset_imu_z = 0.0;
+    offset_imu_roll = 0.0;
+    offset_imu_pitch = 0.0;
+    offset_imu_yaw = 0.0;
+
+//    std::cout<<" vx is "<<current_velocity_x<<" vy is "<<current_velocity_y<<std::endl;
+    previous_pose.x = current_pose.x;
+    previous_pose.y = current_pose.y;
+    previous_pose.z = current_pose.z;
+    previous_pose.roll = current_pose.roll;
+    previous_pose.pitch = current_pose.pitch;
+    previous_pose.yaw = current_pose.yaw;
+
+//    previous_scan_time = current_scan_time;
+
+    previous_previous_velocity = previous_velocity;
+    previous_velocity = current_velocity;
+    previous_velocity_x = current_velocity_x;
+    previous_velocity_y = current_velocity_y;
+    previous_velocity_z = current_velocity_z;
+    previous_accel = current_accel;
+
+    pthread_mutex_lock(&mutex_read);
+    gindex++;
+    glidar_pose.accel = current_accel;
+    glidar_pose.accel_x = current_accel_x;
+    glidar_pose.accel_y = current_accel_y;
+    glidar_pose.accel_z = current_accel_z;
+    glidar_pose.vel = current_velocity;
+    glidar_pose.vel_x = current_velocity_x;
+    glidar_pose.vel_y = current_velocity_y;
+    glidar_pose.vel_z = current_velocity_z;
+    glidar_pose.mpose = current_pose;
+    pthread_mutex_unlock(&mutex_read);
+
+    pthread_mutex_unlock(&mutex_pose);
+
+
+
+
+    double hdg_o = (90 - gvector_trace[gcurmapindex].mndtorigin.heading)*M_PI/180.0;
+    double ndt_vn = current_velocity_x * cos(hdg_o) - current_velocity_y * sin(hdg_o);
+    double ndt_ve = current_velocity_x * sin(hdg_o) + current_velocity_y * cos(hdg_o);
+    double ndt_vd = current_accel_z;
+
+
+
+    std::cout<<std::setprecision(9)<<"gps lat:"<<gcurndtgpspos.lat<<" lon:"
+            <<gcurndtgpspos.lon<<" z:"<<gcurndtgpspos.height<<" heading:"
+           <<gcurndtgpspos.heading<<std::endl;
+    std::cout<<std::setprecision(6)<<std::endl;
+
+
+
+
+    double x0,y0;
+    double x,y;
+    GaussProjCal(gcurndtgpspos.lon,gcurndtgpspos.lat,&x0,&y0);
+    GaussProjCal(gcurgpsgpspos.lon,gcurgpsgpspos.lat,&x,&y);
+    double dis = sqrt(pow(x-x0,2)+ pow(y-y0,2));
+    double headdiff = fabs(gcurgpsgpspos.heading - gcurndtgpspos.heading);
+    double zdiff = fabs(gcurgpsgpspos.height - gcurndtgpspos.height);
+    std::cout<<" dis:"<<dis<<" headdiff:"<<headdiff<<" zdiff:"<<zdiff<<std::endl;
+
+    iv::lidar::ndtpos ndtpos;
+    ndtpos.set_lidartime(raw_scan->header.stamp/1000);
+    ndtpos.set_ndt_time(QDateTime::currentMSecsSinceEpoch());
+    ndtpos.set_accel(current_accel);
+    ndtpos.set_accel_x(current_accel_x);
+    ndtpos.set_accel_y(current_accel_y);
+    ndtpos.set_accel_z(current_accel_z);
+    ndtpos.set_vel(current_velocity);
+    ndtpos.set_vel_x(current_velocity_x);
+    ndtpos.set_vel_y(current_velocity_y);
+    ndtpos.set_vel_z(current_velocity_z);
+    ndtpos.set_fitness_score(fitness_score);
+    ndtpos.set_has_converged(has_converged);
+    ndtpos.set_id(0);
+    ndtpos.set_iteration(iteration);
+    ndtpos.set_trans_probability(trans_probability);
+    ndtpos.set_pose_pitch(current_pose.pitch);
+    ndtpos.set_pose_roll(current_pose.roll);
+    ndtpos.set_pose_yaw(current_pose.yaw);
+    ndtpos.set_pose_x(current_pose.x);
+    ndtpos.set_pose_y(current_pose.y);
+    ndtpos.set_pose_z(current_pose.z);
+    ndtpos.set_comp_time(xTime.elapsed());
+
+
+    int ndatasize = ndtpos.ByteSize();
+    char * strser = new char[ndatasize];
+    bool bSer = ndtpos.SerializeToArray(strser,ndatasize);
+    if(bSer)
+    {
+        iv::modulecomm::ModuleSendMsg(gpc,strser,ndatasize);
+    }
+    else
+    {
+        std::cout<<BOLDRED<<"ndtpos serialize error."<<RESET<<std::endl;
+    }
+    delete strser;
+
+    iv::lidar::ndtgpspos xndtgpspos;
+    xndtgpspos.set_lat(gcurndtgpspos.lat);
+    xndtgpspos.set_lon(gcurndtgpspos.lon);
+    xndtgpspos.set_heading(gcurndtgpspos.heading);
+    xndtgpspos.set_height(gcurndtgpspos.height);
+    xndtgpspos.set_pitch(gcurndtgpspos.pitch);
+    xndtgpspos.set_roll(gcurndtgpspos.roll);
+    xndtgpspos.set_tras_prob(trans_probability);
+    xndtgpspos.set_score(fitness_score);
+    xndtgpspos.set_vn(ndt_vn);
+    xndtgpspos.set_ve(ndt_ve);
+    xndtgpspos.set_vd(ndt_vd);
+
+    givlog->debug("ndtgpspos","lat:%11.7f lon:%11.7f height:%11.3f heading:%6.3f pitch:%6.3f roll:%6.3f vn:%6.3f ve:%6.3f vd:%6.3f trans_prob:%6.3f score:%6.3f",
+                  xndtgpspos.lat(),xndtgpspos.lon(),xndtgpspos.height(),
+                  xndtgpspos.heading(),xndtgpspos.pitch(),xndtgpspos.roll(),
+                  xndtgpspos.vn(),xndtgpspos.ve(),xndtgpspos.vd(),
+                  xndtgpspos.tras_prob(),xndtgpspos.score());
+
+    ndatasize = xndtgpspos.ByteSize();
+    strser = new char[ndatasize];
+    bSer = xndtgpspos.SerializeToArray(strser,ndatasize);
+    if(bSer)
+    {
+        std::cout<<"share msg."<<std::endl;
+        iv::modulecomm::ModuleSendMsg(gpd,strser,ndatasize);
+    }
+    else
+    {
+        std::cout<<BOLDRED<<"ndtgpspos serialize error."<<RESET<<std::endl;
+    }
+    delete strser;
+
+    if(trans_probability < 0.5)gbNeedGPSUpdateNDT = true;
+    else gbNeedGPSUpdateNDT = false;
+
+
+}
+
+
+int GetPose(int & curindex,iv::lidar_pose & xlidar_pose)
+{
+     pthread_mutex_lock(&mutex_read);
+    if(curindex == gindex)
+    {
+        pthread_mutex_unlock(&mutex_read);
+        return 0;
+    }
+    curindex = gindex;
+    xlidar_pose = glidar_pose;
+    pthread_mutex_unlock(&mutex_read);
+    return 1;
+}
+
+void SetCurPose(double x0, double y0, double yaw0, double z0, double pitch0, double roll0)
+{
+
+    std::cout<<"set pose"<<std::endl;
+    pthread_mutex_lock(&mutex_pose);
+
+    initial_pose.x = x0;
+    initial_pose.y = y0;
+    initial_pose.z = z0;
+    initial_pose.roll = roll0;
+    initial_pose.pitch = pitch0;
+    initial_pose.yaw = yaw0;
+
+    localizer_pose.x = initial_pose.x;
+    localizer_pose.y = initial_pose.y;
+    localizer_pose.z = initial_pose.z;
+    localizer_pose.roll = initial_pose.roll;
+    localizer_pose.pitch = initial_pose.pitch;
+    localizer_pose.yaw = initial_pose.yaw;
+
+    previous_pose.x = initial_pose.x;
+    previous_pose.y = initial_pose.y;
+    previous_pose.z = initial_pose.z;
+    previous_pose.roll = initial_pose.roll;
+    previous_pose.pitch = initial_pose.pitch;
+    previous_pose.yaw = initial_pose.yaw;
+
+    current_pose.x = initial_pose.x;
+    current_pose.y = initial_pose.y;
+    current_pose.z = initial_pose.z;
+    current_pose.roll = initial_pose.roll;
+    current_pose.pitch = initial_pose.pitch;
+    current_pose.yaw = initial_pose.yaw;
+
+    current_velocity = 0;
+    current_velocity_x = 0;
+    current_velocity_y = 0;
+    current_velocity_z = 0;
+    angular_velocity = 0;
+
+    pthread_mutex_unlock(&mutex_pose);
+
+
+}
+
+void SetRunState(bool bRun)
+{
+    std::cout<<"set state."<<std::endl;
+    gbNDTRun = bRun;
+}
+
+void setuseimu(bool bUse)
+{
+    _use_imu = bUse;
+}
+

src/detection/detection_ndt_pclomp/ndt_matching.h

@@ -0,0 +1,112 @@
+#ifndef NDT_MATCHING_H
+#define NDT_MATCHING_H
+#include<QDateTime>
+#include <QMutex>
+
+struct pose
+{
+  double x;
+  double y;
+  double z;
+  double roll;
+  double pitch;
+  double yaw;
+  double vx;
+  double vy;
+};
+
+
+namespace  iv {
+struct lidar_pose
+{
+    lidar_pose() {}
+
+    double vel_x;
+    double vel_y;
+    double vel_z;
+    double vel;
+    double accel_x;
+    double accel_y;
+    double accel_z;
+    double accel;
+    struct pose mpose;
+};
+
+}
+
+namespace iv {
+struct imudata
+{
+  qint64 imutime;
+  double imu_linear_acceleration_x;
+  double imu_linear_acceleration_y;
+  double imu_linear_acceleration_z;
+  double imu_angular_velocity_x;
+  double imu_angular_velocity_y;
+  double imu_angular_velocity_z;
+};
+}
+
+
+void ndt_match_Init_nomap();
+
+void ndt_match_Init(pcl::PointCloud<pcl::PointXYZ>::Ptr mappcd);
+
+void point_ndt(pcl::PointCloud<pcl::PointXYZ>::Ptr raw_scan);
+
+void point_ndt_test(pcl::PointCloud<pcl::PointXYZ>::Ptr raw_scan);
+
+int GetPose(int & curindex,iv::lidar_pose & xlidar_pose);
+
+void SetCurPose(double x0,double y0,double yaw0,double z0 = 0,double pitch0 = 0,double roll0 = 0);
+
+void ndt_match_SetMap(pcl::PointCloud<pcl::PointXYZ>::Ptr mappcd);
+
+void SetRunState(bool bRun);
+
+namespace iv {
+struct ndttracepoint
+{
+    double x;
+    double y;
+    double z;
+    double pitch;
+    double roll;
+    double yaw;
+};
+}
+
+namespace iv {
+struct gpspos
+{
+    double lat;
+    double lon;
+    double height;
+    double heading;
+    double pitch;
+    double roll;
+    double ve;
+    double vn;
+};
+}
+
+iv::gpspos PoseToGPS(iv::gpspos xori,pose xpose);
+pose CalcPose(iv::gpspos xori,iv::gpspos xcur);
+
+namespace iv {
+struct ndtmaptrace
+{
+    ndtmaptrace() {}
+    std::vector<ndttracepoint> mvector_trace;
+    std::string mstrpcdpath;
+    iv::gpspos mndtorigin;
+};
+
+}
+
+void setuseimu(bool bUse);
+void imu_update(qint64 current_time_imu,double imu_roll,double imu_pitch,double imu_yaw,
+                       double acceleration_x,double acceleration_y,
+                       double acceleration_z);
+
+#endif // NDT_MATCHING_H

src/detection/detection_ndt_pclomp/pclomp/ndt_omp.cpp

@@ -0,0 +1,5 @@
+#include <pclomp/ndt_omp.h>
+#include <ndt_omp_impl.hpp>
+
+template class pclomp::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>;
+template class pclomp::NormalDistributionsTransform<pcl::PointXYZI, pcl::PointXYZI>;

src/detection/detection_ndt_pclomp/pclomp/ndt_omp_impl.hpp

@@ -0,0 +1,985 @@
+#include <pclomp/ndt_omp.h>
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Point Cloud Library (PCL) - www.pointclouds.org
+ *  Copyright (c) 2010-2011, Willow Garage, Inc.
+ *  Copyright (c) 2012-, Open Perception, Inc.
+ *
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the copyright holder(s) nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id$
+ *
+ */
+
+#ifndef PCL_REGISTRATION_NDT_OMP_IMPL_H_
+#define PCL_REGISTRATION_NDT_OMP_IMPL_H_
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+template<typename PointSource, typename PointTarget>
+pclomp::NormalDistributionsTransform<PointSource, PointTarget>::NormalDistributionsTransform ()
+  : target_cells_ ()
+  , resolution_ (1.0f)
+  , step_size_ (0.1)
+  , outlier_ratio_ (0.55)
+  , gauss_d1_ ()
+  , gauss_d2_ ()
+  , gauss_d3_ ()
+  , trans_probability_ ()
+  , j_ang_a_ (), j_ang_b_ (), j_ang_c_ (), j_ang_d_ (), j_ang_e_ (), j_ang_f_ (), j_ang_g_ (), j_ang_h_ ()
+  , h_ang_a2_ (), h_ang_a3_ (), h_ang_b2_ (), h_ang_b3_ (), h_ang_c2_ (), h_ang_c3_ (), h_ang_d1_ (), h_ang_d2_ ()
+  , h_ang_d3_ (), h_ang_e1_ (), h_ang_e2_ (), h_ang_e3_ (), h_ang_f1_ (), h_ang_f2_ (), h_ang_f3_ ()
+{
+  reg_name_ = "NormalDistributionsTransform";
+
+  double gauss_c1, gauss_c2;
+
+  // Initializes the gaussian fitting parameters (eq. 6.8) [Magnusson 2009]
+  gauss_c1 = 10.0 * (1 - outlier_ratio_);
+  gauss_c2 = outlier_ratio_ / pow (resolution_, 3);
+  gauss_d3_ = -log (gauss_c2);
+  gauss_d1_ = -log ( gauss_c1 + gauss_c2 ) - gauss_d3_;
+  gauss_d2_ = -2 * log ((-log ( gauss_c1 * exp ( -0.5 ) + gauss_c2 ) - gauss_d3_) / gauss_d1_);
+
+  transformation_epsilon_ = 0.1;
+  max_iterations_ = 35;
+
+  search_method = DIRECT7;
+  num_threads_ = omp_get_max_threads();
+}
+
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+template<typename PointSource, typename PointTarget> void
+pclomp::NormalDistributionsTransform<PointSource, PointTarget>::computeTransformation (PointCloudSource &output, const Eigen::Matrix4f &guess)
+{
+  nr_iterations_ = 0;
+  converged_ = false;
+
+  double gauss_c1, gauss_c2;
+
+  // Initializes the gaussian fitting parameters (eq. 6.8) [Magnusson 2009]
+  gauss_c1 = 10 * (1 - outlier_ratio_);
+  gauss_c2 = outlier_ratio_ / pow (resolution_, 3);
+  gauss_d3_ = -log (gauss_c2);
+  gauss_d1_ = -log ( gauss_c1 + gauss_c2 ) - gauss_d3_;
+  gauss_d2_ = -2 * log ((-log ( gauss_c1 * exp ( -0.5 ) + gauss_c2 ) - gauss_d3_) / gauss_d1_);
+
+  if (guess != Eigen::Matrix4f::Identity ())
+  {
+    // Initialise final transformation to the guessed one
+    final_transformation_ = guess;
+    // Apply guessed transformation prior to search for neighbours
+    transformPointCloud (output, output, guess);
+  }
+
+  Eigen::Transform<float, 3, Eigen::Affine, Eigen::ColMajor> eig_transformation;
+  eig_transformation.matrix () = final_transformation_;
+
+  // Convert initial guess matrix to 6 element transformation vector
+  Eigen::Matrix<double, 6, 1> p, delta_p, score_gradient;
+  Eigen::Vector3f init_translation = eig_transformation.translation ();
+  Eigen::Vector3f init_rotation = eig_transformation.rotation ().eulerAngles (0, 1, 2);
+  p << init_translation (0), init_translation (1), init_translation (2),
+  init_rotation (0), init_rotation (1), init_rotation (2);
+
+  Eigen::Matrix<double, 6, 6> hessian;
+
+  double score = 0;
+  double delta_p_norm;
+
+  // Calculate derivatives of initial transform vector, subsequent derivative calculations are done in the step length determination.
+  score = computeDerivatives (score_gradient, hessian, output, p);
+
+  while (!converged_)
+  {
+    // Store previous transformation
+    previous_transformation_ = transformation_;
+
+    // Solve for decent direction using newton method, line 23 in Algorithm 2 [Magnusson 2009]
+    Eigen::JacobiSVD<Eigen::Matrix<double, 6, 6> > sv (hessian, Eigen::ComputeFullU | Eigen::ComputeFullV);
+    // Negative for maximization as opposed to minimization
+    delta_p = sv.solve (-score_gradient);
+
+    //Calculate step length with guaranteed sufficient decrease [More, Thuente 1994]
+    delta_p_norm = delta_p.norm ();
+
+    if (delta_p_norm == 0 || delta_p_norm != delta_p_norm)
+    {
+      trans_probability_ = score / static_cast<double> (input_->points.size ());
+      converged_ = delta_p_norm == delta_p_norm;
+      return;
+    }
+
+    delta_p.normalize ();
+    delta_p_norm = computeStepLengthMT (p, delta_p, delta_p_norm, step_size_, transformation_epsilon_ / 2, score, score_gradient, hessian, output);
+    delta_p *= delta_p_norm;
+
+
+    transformation_ = (Eigen::Translation<float, 3> (static_cast<float> (delta_p (0)), static_cast<float> (delta_p (1)), static_cast<float> (delta_p (2))) *
+                       Eigen::AngleAxis<float> (static_cast<float> (delta_p (3)), Eigen::Vector3f::UnitX ()) *
+                       Eigen::AngleAxis<float> (static_cast<float> (delta_p (4)), Eigen::Vector3f::UnitY ()) *
+                       Eigen::AngleAxis<float> (static_cast<float> (delta_p (5)), Eigen::Vector3f::UnitZ ())).matrix ();
+
+
+    p = p + delta_p;
+
+    // Update Visualizer (untested)
+    if (update_visualizer_ != 0)
+      update_visualizer_ (output, std::vector<int>(), *target_, std::vector<int>() );
+
+    if (nr_iterations_ > max_iterations_ ||
+        (nr_iterations_ && (std::fabs (delta_p_norm) < transformation_epsilon_)))
+    {
+      converged_ = true;
+    }
+
+    nr_iterations_++;
+
+  }
+
+  // Store transformation probability. The relative differences within each scan registration are accurate
+  // but the normalization constants need to be modified for it to be globally accurate
+  trans_probability_ = score / static_cast<double> (input_->points.size ());
+}
+
+#ifndef _OPENMP
+int omp_get_max_threads() { return 1; }
+int omp_get_thread_num() { return 0; }
+#endif
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+template<typename PointSource, typename PointTarget> double
+pclomp::NormalDistributionsTransform<PointSource, PointTarget>::computeDerivatives(Eigen::Matrix<double, 6, 1> &score_gradient,
+	Eigen::Matrix<double, 6, 6> &hessian,
+	PointCloudSource &trans_cloud,
+	Eigen::Matrix<double, 6, 1> &p,
+	bool compute_hessian)
+{
+	score_gradient.setZero();
+	hessian.setZero();
+	double score = 0;
+
+  std::vector<double> scores(input_->points.size());
+  std::vector<Eigen::Matrix<double, 6, 1>, Eigen::aligned_allocator<Eigen::Matrix<double, 6, 1>>> score_gradients(input_->points.size());
+  std::vector<Eigen::Matrix<double, 6, 6>, Eigen::aligned_allocator<Eigen::Matrix<double, 6, 6>>> hessians(input_->points.size());
+  for (std::size_t i = 0; i < input_->points.size(); i++) {
+		scores[i] = 0;
+		score_gradients[i].setZero();
+		hessians[i].setZero();
+	}
+
+	// Precompute Angular Derivatives (eq. 6.19 and 6.21)[Magnusson 2009]
+	computeAngleDerivatives(p);
+
+  std::vector<std::vector<TargetGridLeafConstPtr>> neighborhoods(num_threads_);
+  std::vector<std::vector<float>> distancess(num_threads_);
+
+	// Update gradient and hessian for each point, line 17 in Algorithm 2 [Magnusson 2009]
+#pragma omp parallel for num_threads(num_threads_) schedule(guided, 8)
+	for (std::size_t idx = 0; idx < input_->points.size(); idx++)
+	{
+		int thread_n = omp_get_thread_num();
+
+		// Original Point and Transformed Point
+		PointSource x_pt, x_trans_pt;
+		// Original Point and Transformed Point (for math)
+		Eigen::Vector3d x, x_trans;
+		// Occupied Voxel
+		TargetGridLeafConstPtr cell;
+		// Inverse Covariance of Occupied Voxel
+		Eigen::Matrix3d c_inv;
+
+		// Initialize Point Gradient and Hessian
+		Eigen::Matrix<float, 4, 6> point_gradient_;
+		Eigen::Matrix<float, 24, 6> point_hessian_;
+		point_gradient_.setZero();
+		point_gradient_.block<3, 3>(0, 0).setIdentity();
+		point_hessian_.setZero();
+
+		x_trans_pt = trans_cloud.points[idx];
+
+		auto& neighborhood = neighborhoods[thread_n];
+		auto& distances = distancess[thread_n];
+
+		// Find neighbors (Radius search has been experimentally faster than direct neighbor checking.
+		switch (search_method) {
+		case KDTREE:
+			target_cells_.radiusSearch(x_trans_pt, resolution_, neighborhood, distances);
+			break;
+		case DIRECT26:
+			target_cells_.getNeighborhoodAtPoint(x_trans_pt, neighborhood);
+			break;
+		default:
+		case DIRECT7:
+			target_cells_.getNeighborhoodAtPoint7(x_trans_pt, neighborhood);
+			break;
+		case DIRECT1:
+			target_cells_.getNeighborhoodAtPoint1(x_trans_pt, neighborhood);
+			break;
+		}
+
+		double score_pt = 0;
+		Eigen::Matrix<double, 6, 1> score_gradient_pt = Eigen::Matrix<double, 6, 1>::Zero();
+		Eigen::Matrix<double, 6, 6> hessian_pt = Eigen::Matrix<double, 6, 6>::Zero();
+
+		for (typename std::vector<TargetGridLeafConstPtr>::iterator neighborhood_it = neighborhood.begin(); neighborhood_it != neighborhood.end(); neighborhood_it++)
+		{
+			cell = *neighborhood_it;
+			x_pt = input_->points[idx];
+			x = Eigen::Vector3d(x_pt.x, x_pt.y, x_pt.z);
+
+			x_trans = Eigen::Vector3d(x_trans_pt.x, x_trans_pt.y, x_trans_pt.z);
+
+			// Denorm point, x_k' in Equations 6.12 and 6.13 [Magnusson 2009]
+			x_trans -= cell->getMean();
+			// Uses precomputed covariance for speed.
+			c_inv = cell->getInverseCov();
+
+			// Compute derivative of transform function w.r.t. transform vector, J_E and H_E in Equations 6.18 and 6.20 [Magnusson 2009]
+			computePointDerivatives(x, point_gradient_, point_hessian_);
+			// Update score, gradient and hessian, lines 19-21 in Algorithm 2, according to Equations 6.10, 6.12 and 6.13, respectively [Magnusson 2009]
+			score_pt += updateDerivatives(score_gradient_pt, hessian_pt, point_gradient_, point_hessian_, x_trans, c_inv, compute_hessian);
+		}
+
+		scores[idx] = score_pt;
+		score_gradients[idx].noalias() = score_gradient_pt;
+		hessians[idx].noalias() = hessian_pt;
+	}
+
+  // Ensure that the result is invariant against the summing up order
+  for (std::size_t i = 0; i < input_->points.size(); i++) {
+		score += scores[i];
+		score_gradient += score_gradients[i];
+		hessian += hessians[i];
+	}
+
+	return (score);
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+template<typename PointSource, typename PointTarget> void
+pclomp::NormalDistributionsTransform<PointSource, PointTarget>::computeAngleDerivatives(Eigen::Matrix<double, 6, 1> &p, bool compute_hessian)
+{
+	// Simplified math for near 0 angles
+	double cx, cy, cz, sx, sy, sz;
+	if (fabs(p(3)) < 10e-5)
+	{
+		//p(3) = 0;
+		cx = 1.0;
+		sx = 0.0;
+	}
+	else
+	{
+		cx = cos(p(3));
+		sx = sin(p(3));
+	}
+	if (fabs(p(4)) < 10e-5)
+	{
+		//p(4) = 0;
+		cy = 1.0;
+		sy = 0.0;
+	}
+	else
+	{
+		cy = cos(p(4));
+		sy = sin(p(4));
+	}
+
+	if (fabs(p(5)) < 10e-5)
+	{
+		//p(5) = 0;
+		cz = 1.0;
+		sz = 0.0;
+	}
+	else
+	{
+		cz = cos(p(5));
+		sz = sin(p(5));
+	}
+
+	// Precomputed angular gradiant components. Letters correspond to Equation 6.19 [Magnusson 2009]
+	j_ang_a_ << (-sx * sz + cx * sy * cz), (-sx * cz - cx * sy * sz), (-cx * cy);
+	j_ang_b_ << (cx * sz + sx * sy * cz), (cx * cz - sx * sy * sz), (-sx * cy);
+	j_ang_c_ << (-sy * cz), sy * sz, cy;
+	j_ang_d_ << sx * cy * cz, (-sx * cy * sz), sx * sy;
+	j_ang_e_ << (-cx * cy * cz), cx * cy * sz, (-cx * sy);
+	j_ang_f_ << (-cy * sz), (-cy * cz), 0;
+	j_ang_g_ << (cx * cz - sx * sy * sz), (-cx * sz - sx * sy * cz), 0;
+	j_ang_h_ << (sx * cz + cx * sy * sz), (cx * sy * cz - sx * sz), 0;
+
+	j_ang.setZero();
+	j_ang.row(0).noalias() = Eigen::Vector4f((-sx * sz + cx * sy * cz), (-sx * cz - cx * sy * sz), (-cx * cy), 0.0f);
+	j_ang.row(1).noalias() = Eigen::Vector4f((cx * sz + sx * sy * cz), (cx * cz - sx * sy * sz), (-sx * cy), 0.0f);
+	j_ang.row(2).noalias() = Eigen::Vector4f((-sy * cz), sy * sz, cy, 0.0f);
+	j_ang.row(3).noalias() = Eigen::Vector4f(sx * cy * cz, (-sx * cy * sz), sx * sy, 0.0f);
+	j_ang.row(4).noalias() = Eigen::Vector4f((-cx * cy * cz), cx * cy * sz, (-cx * sy), 0.0f);
+	j_ang.row(5).noalias() = Eigen::Vector4f((-cy * sz), (-cy * cz), 0, 0.0f);
+	j_ang.row(6).noalias() = Eigen::Vector4f((cx * cz - sx * sy * sz), (-cx * sz - sx * sy * cz), 0, 0.0f);
+	j_ang.row(7).noalias() = Eigen::Vector4f((sx * cz + cx * sy * sz), (cx * sy * cz - sx * sz), 0, 0.0f);
+
+	if (compute_hessian)
+	{
+		// Precomputed angular hessian components. Letters correspond to Equation 6.21 and numbers correspond to row index [Magnusson 2009]
+		h_ang_a2_ << (-cx * sz - sx * sy * cz), (-cx * cz + sx * sy * sz), sx * cy;
+		h_ang_a3_ << (-sx * sz + cx * sy * cz), (-cx * sy * sz - sx * cz), (-cx * cy);
+
+		h_ang_b2_ << (cx * cy * cz), (-cx * cy * sz), (cx * sy);
+		h_ang_b3_ << (sx * cy * cz), (-sx * cy * sz), (sx * sy);
+
+		// The sign of 'sx * sz' in c2 is incorrect in [Magnusson 2009], and it is fixed here.
+		h_ang_c2_ << (-sx * cz - cx * sy * sz), (sx * sz - cx * sy * cz), 0;
+		h_ang_c3_ << (cx * cz - sx * sy * sz), (-sx * sy * cz - cx * sz), 0;
+
+		h_ang_d1_ << (-cy * cz), (cy * sz), (-sy);
+		h_ang_d2_ << (-sx * sy * cz), (sx * sy * sz), (sx * cy);
+		h_ang_d3_ << (cx * sy * cz), (-cx * sy * sz), (-cx * cy);
+
+		h_ang_e1_ << (sy * sz), (sy * cz), 0;
+		h_ang_e2_ << (-sx * cy * sz), (-sx * cy * cz), 0;
+		h_ang_e3_ << (cx * cy * sz), (cx * cy * cz), 0;
+
+		h_ang_f1_ << (-cy * cz), (cy * sz), 0;
+		h_ang_f2_ << (-cx * sz - sx * sy * cz), (-cx * cz + sx * sy * sz), 0;
+		h_ang_f3_ << (-sx * sz + cx * sy * cz), (-cx * sy * sz - sx * cz), 0;
+
+		h_ang.setZero();
+		h_ang.row(0).noalias() = Eigen::Vector4f((-cx * sz - sx * sy * cz), (-cx * cz + sx * sy * sz), sx * cy, 0.0f);		// a2
+		h_ang.row(1).noalias() = Eigen::Vector4f((-sx * sz + cx * sy * cz), (-cx * sy * sz - sx * cz), (-cx * cy), 0.0f);	// a3
+
+		h_ang.row(2).noalias() = Eigen::Vector4f((cx * cy * cz), (-cx * cy * sz), (cx * sy), 0.0f);							// b2
+		h_ang.row(3).noalias() = Eigen::Vector4f((sx * cy * cz), (-sx * cy * sz), (sx * sy), 0.0f);							// b3
+
+		h_ang.row(4).noalias() = Eigen::Vector4f((-sx * cz - cx * sy * sz), (sx * sz - cx * sy * cz), 0, 0.0f);				// c2
+		h_ang.row(5).noalias() = Eigen::Vector4f((cx * cz - sx * sy * sz), (-sx * sy * cz - cx * sz), 0, 0.0f);				// c3
+
+		h_ang.row(6).noalias() = Eigen::Vector4f((-cy * cz), (cy * sz), (sy), 0.0f);										// d1
+		h_ang.row(7).noalias() = Eigen::Vector4f((-sx * sy * cz), (sx * sy * sz), (sx * cy), 0.0f);							// d2
+		h_ang.row(8).noalias() = Eigen::Vector4f((cx * sy * cz), (-cx * sy * sz), (-cx * cy), 0.0f);						// d3
+
+		h_ang.row(9).noalias() = Eigen::Vector4f((sy * sz), (sy * cz), 0, 0.0f);											// e1
+		h_ang.row(10).noalias() = Eigen::Vector4f ((-sx * cy * sz), (-sx * cy * cz), 0, 0.0f);								// e2
+		h_ang.row(11).noalias() = Eigen::Vector4f ((cx * cy * sz), (cx * cy * cz), 0, 0.0f);								// e3
+
+		h_ang.row(12).noalias() = Eigen::Vector4f ((-cy * cz), (cy * sz), 0, 0.0f);											// f1
+		h_ang.row(13).noalias() = Eigen::Vector4f ((-cx * sz - sx * sy * cz), (-cx * cz + sx * sy * sz), 0, 0.0f);			// f2
+		h_ang.row(14).noalias() = Eigen::Vector4f ((-sx * sz + cx * sy * cz), (-cx * sy * sz - sx * cz), 0, 0.0f);			// f3
+	}
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+template<typename PointSource, typename PointTarget> void
+pclomp::NormalDistributionsTransform<PointSource, PointTarget>::computePointDerivatives(Eigen::Vector3d &x, Eigen::Matrix<float, 4, 6>& point_gradient_, Eigen::Matrix<float, 24, 6>& point_hessian_, bool compute_hessian) const
+{
+	Eigen::Vector4f x4(x[0], x[1], x[2], 0.0f);
+
+	// Calculate first derivative of Transformation Equation 6.17 w.r.t. transform vector p.
+	// Derivative w.r.t. ith element of transform vector corresponds to column i, Equation 6.18 and 6.19 [Magnusson 2009]
+	Eigen::Matrix<float, 8, 1> x_j_ang = j_ang * x4;
+
+	point_gradient_(1, 3) = x_j_ang[0];
+	point_gradient_(2, 3) = x_j_ang[1];
+	point_gradient_(0, 4) = x_j_ang[2];
+	point_gradient_(1, 4) = x_j_ang[3];
+	point_gradient_(2, 4) = x_j_ang[4];
+	point_gradient_(0, 5) = x_j_ang[5];
+	point_gradient_(1, 5) = x_j_ang[6];
+	point_gradient_(2, 5) = x_j_ang[7];
+
+	if (compute_hessian)
+	{
+		Eigen::Matrix<float, 16, 1> x_h_ang = h_ang * x4;
+
+		// Vectors from Equation 6.21 [Magnusson 2009]
+		Eigen::Vector4f a (0, x_h_ang[0], x_h_ang[1], 0.0f);
+		Eigen::Vector4f b (0, x_h_ang[2], x_h_ang[3], 0.0f);
+		Eigen::Vector4f c (0, x_h_ang[4], x_h_ang[5], 0.0f);
+		Eigen::Vector4f d (x_h_ang[6], x_h_ang[7], x_h_ang[8], 0.0f);
+		Eigen::Vector4f e (x_h_ang[9], x_h_ang[10], x_h_ang[11], 0.0f);
+		Eigen::Vector4f f (x_h_ang[12], x_h_ang[13], x_h_ang[14], 0.0f);
+
+		// Calculate second derivative of Transformation Equation 6.17 w.r.t. transform vector p.
+		// Derivative w.r.t. ith and jth elements of transform vector corresponds to the 3x1 block matrix starting at (3i,j), Equation 6.20 and 6.21 [Magnusson 2009]
+		point_hessian_.block<4, 1>((9/3)*4, 3) = a;
+		point_hessian_.block<4, 1>((12/3)*4, 3) = b;
+		point_hessian_.block<4, 1>((15/3)*4, 3) = c;
+		point_hessian_.block<4, 1>((9/3)*4, 4) = b;
+		point_hessian_.block<4, 1>((12/3)*4, 4) = d;
+		point_hessian_.block<4, 1>((15/3)*4, 4) = e;
+		point_hessian_.block<4, 1>((9/3)*4, 5) = c;
+		point_hessian_.block<4, 1>((12/3)*4, 5) = e;
+		point_hessian_.block<4, 1>((15/3)*4, 5) = f;
+	}
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+template<typename PointSource, typename PointTarget> void
+pclomp::NormalDistributionsTransform<PointSource, PointTarget>::computePointDerivatives(Eigen::Vector3d &x, Eigen::Matrix<double, 3, 6>& point_gradient_, Eigen::Matrix<double, 18, 6>& point_hessian_, bool compute_hessian) const
+{
+	// Calculate first derivative of Transformation Equation 6.17 w.r.t. transform vector p.
+	// Derivative w.r.t. ith element of transform vector corresponds to column i, Equation 6.18 and 6.19 [Magnusson 2009]
+	point_gradient_(1, 3) = x.dot(j_ang_a_);
+	point_gradient_(2, 3) = x.dot(j_ang_b_);
+	point_gradient_(0, 4) = x.dot(j_ang_c_);
+	point_gradient_(1, 4) = x.dot(j_ang_d_);
+	point_gradient_(2, 4) = x.dot(j_ang_e_);
+	point_gradient_(0, 5) = x.dot(j_ang_f_);
+	point_gradient_(1, 5) = x.dot(j_ang_g_);
+	point_gradient_(2, 5) = x.dot(j_ang_h_);
+
+	if (compute_hessian)
+	{
+		// Vectors from Equation 6.21 [Magnusson 2009]
+		Eigen::Vector3d a, b, c, d, e, f;
+
+		a << 0, x.dot(h_ang_a2_), x.dot(h_ang_a3_);
+		b << 0, x.dot(h_ang_b2_), x.dot(h_ang_b3_);
+		c << 0, x.dot(h_ang_c2_), x.dot(h_ang_c3_);
+		d << x.dot(h_ang_d1_), x.dot(h_ang_d2_), x.dot(h_ang_d3_);
+		e << x.dot(h_ang_e1_), x.dot(h_ang_e2_), x.dot(h_ang_e3_);
+		f << x.dot(h_ang_f1_), x.dot(h_ang_f2_), x.dot(h_ang_f3_);
+
+		// Calculate second derivative of Transformation Equation 6.17 w.r.t. transform vector p.
+		// Derivative w.r.t. ith and jth elements of transform vector corresponds to the 3x1 block matrix starting at (3i,j), Equation 6.20 and 6.21 [Magnusson 2009]
+		point_hessian_.block<3, 1>(9, 3) = a;
+		point_hessian_.block<3, 1>(12, 3) = b;
+		point_hessian_.block<3, 1>(15, 3) = c;
+		point_hessian_.block<3, 1>(9, 4) = b;
+		point_hessian_.block<3, 1>(12, 4) = d;
+		point_hessian_.block<3, 1>(15, 4) = e;
+		point_hessian_.block<3, 1>(9, 5) = c;
+		point_hessian_.block<3, 1>(12, 5) = e;
+		point_hessian_.block<3, 1>(15, 5) = f;
+	}
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+template<typename PointSource, typename PointTarget> double
+pclomp::NormalDistributionsTransform<PointSource, PointTarget>::updateDerivatives(Eigen::Matrix<double, 6, 1> &score_gradient,
+	Eigen::Matrix<double, 6, 6> &hessian,
+	const Eigen::Matrix<float, 4, 6> &point_gradient4,
+	const Eigen::Matrix<float, 24, 6> &point_hessian_,
+	const Eigen::Vector3d &x_trans, const Eigen::Matrix3d &c_inv,
+	bool compute_hessian) const
+{
+	Eigen::Matrix<float, 1, 4> x_trans4( x_trans[0], x_trans[1], x_trans[2], 0.0f );
+	Eigen::Matrix4f c_inv4 = Eigen::Matrix4f::Zero();
+	c_inv4.topLeftCorner(3, 3) = c_inv.cast<float>();
+
+	float gauss_d2 = gauss_d2_;
+
+	// e^(-d_2/2 * (x_k - mu_k)^T Sigma_k^-1 (x_k - mu_k)) Equation 6.9 [Magnusson 2009]
+	float e_x_cov_x = exp(-gauss_d2 * x_trans4.dot(x_trans4 * c_inv4) * 0.5f);
+	// Calculate probability of transformed points existence, Equation 6.9 [Magnusson 2009]
+	float score_inc = -gauss_d1_ * e_x_cov_x;
+
+	e_x_cov_x = gauss_d2 * e_x_cov_x;
+
+	// Error checking for invalid values.
+	if (e_x_cov_x > 1 || e_x_cov_x < 0 || e_x_cov_x != e_x_cov_x)
+		return (0);
+
+	// Reusable portion of Equation 6.12 and 6.13 [Magnusson 2009]
+	e_x_cov_x *= gauss_d1_;
+
+	Eigen::Matrix<float, 4, 6> c_inv4_x_point_gradient4 = c_inv4 * point_gradient4;
+	Eigen::Matrix<float, 6, 1> x_trans4_dot_c_inv4_x_point_gradient4 = x_trans4 * c_inv4_x_point_gradient4;
+
+	score_gradient.noalias() += (e_x_cov_x * x_trans4_dot_c_inv4_x_point_gradient4).cast<double>();
+
+	if (compute_hessian) {
+		Eigen::Matrix<float, 1, 4> x_trans4_x_c_inv4 = x_trans4 * c_inv4;
+		Eigen::Matrix<float, 6, 6> point_gradient4_colj_dot_c_inv4_x_point_gradient4_col_i = point_gradient4.transpose() * c_inv4_x_point_gradient4;
+		Eigen::Matrix<float, 6, 1> x_trans4_dot_c_inv4_x_ext_point_hessian_4ij;
+
+		for (int i = 0; i < 6; i++) {
+			// Sigma_k^-1 d(T(x,p))/dpi, Reusable portion of Equation 6.12 and 6.13 [Magnusson 2009]
+			// Update gradient, Equation 6.12 [Magnusson 2009]
+			x_trans4_dot_c_inv4_x_ext_point_hessian_4ij.noalias() = x_trans4_x_c_inv4 * point_hessian_.block<4, 6>(i * 4, 0);
+
+			for (int j = 0; j < hessian.cols(); j++) {
+				// Update hessian, Equation 6.13 [Magnusson 2009]
+				hessian(i, j) += e_x_cov_x * (-gauss_d2 * x_trans4_dot_c_inv4_x_point_gradient4(i) * x_trans4_dot_c_inv4_x_point_gradient4(j) +
+					x_trans4_dot_c_inv4_x_ext_point_hessian_4ij(j) +
+					point_gradient4_colj_dot_c_inv4_x_point_gradient4_col_i(j, i));
+			}
+		}
+	}
+
+	return (score_inc);
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+template<typename PointSource, typename PointTarget> void
+pclomp::NormalDistributionsTransform<PointSource, PointTarget>::computeHessian (Eigen::Matrix<double, 6, 6> &hessian,
+                                                                             PointCloudSource &trans_cloud, Eigen::Matrix<double, 6, 1> &)
+{
+  // Original Point and Transformed Point
+  PointSource x_pt, x_trans_pt;
+  // Original Point and Transformed Point (for math)
+  Eigen::Vector3d x, x_trans;
+  // Occupied Voxel
+  TargetGridLeafConstPtr cell;
+  // Inverse Covariance of Occupied Voxel
+  Eigen::Matrix3d c_inv;
+
+  // Initialize Point Gradient and Hessian
+  Eigen::Matrix<double, 3, 6> point_gradient_;
+  Eigen::Matrix<double, 18, 6> point_hessian_;
+  point_gradient_.setZero();
+  point_gradient_.block<3, 3>(0, 0).setIdentity();
+  point_hessian_.setZero();
+
+  hessian.setZero ();
+
+  // Precompute Angular Derivatives unnecessary because only used after regular derivative calculation
+
+  // Update hessian for each point, line 17 in Algorithm 2 [Magnusson 2009]
+  for (size_t idx = 0; idx < input_->points.size (); idx++)
+  {
+    x_trans_pt = trans_cloud.points[idx];
+
+    // Find neighbors (Radius search has been experimentally faster than direct neighbor checking.
+    std::vector<TargetGridLeafConstPtr> neighborhood;
+    std::vector<float> distances;
+		switch (search_method) {
+		case KDTREE:
+			target_cells_.radiusSearch(x_trans_pt, resolution_, neighborhood, distances);
+			break;
+		case DIRECT26:
+			target_cells_.getNeighborhoodAtPoint(x_trans_pt, neighborhood);
+			break;
+		default:
+		case DIRECT7:
+			target_cells_.getNeighborhoodAtPoint7(x_trans_pt, neighborhood);
+			break;
+		case DIRECT1:
+			target_cells_.getNeighborhoodAtPoint1(x_trans_pt, neighborhood);
+			break;
+		}
+
+    for (typename std::vector<TargetGridLeafConstPtr>::iterator neighborhood_it = neighborhood.begin (); neighborhood_it != neighborhood.end (); neighborhood_it++)
+    {
+      cell = *neighborhood_it;
+
+      {
+        x_pt = input_->points[idx];
+        x = Eigen::Vector3d (x_pt.x, x_pt.y, x_pt.z);
+
+        x_trans = Eigen::Vector3d (x_trans_pt.x, x_trans_pt.y, x_trans_pt.z);
+
+        // Denorm point, x_k' in Equations 6.12 and 6.13 [Magnusson 2009]
+        x_trans -= cell->getMean ();
+        // Uses precomputed covariance for speed.
+        c_inv = cell->getInverseCov ();
+
+        // Compute derivative of transform function w.r.t. transform vector, J_E and H_E in Equations 6.18 and 6.20 [Magnusson 2009]
+        computePointDerivatives (x, point_gradient_, point_hessian_);
+        // Update hessian, lines 21 in Algorithm 2, according to Equations 6.10, 6.12 and 6.13, respectively [Magnusson 2009]
+        updateHessian (hessian, point_gradient_, point_hessian_, x_trans, c_inv);
+      }
+    }
+  }
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+template<typename PointSource, typename PointTarget> void
+pclomp::NormalDistributionsTransform<PointSource, PointTarget>::updateHessian (Eigen::Matrix<double, 6, 6> &hessian,
+	const Eigen::Matrix<double, 3, 6> &point_gradient_,
+	const Eigen::Matrix<double, 18, 6> &point_hessian_,
+	const Eigen::Vector3d &x_trans,
+	const Eigen::Matrix3d &c_inv) const
+{
+  Eigen::Vector3d cov_dxd_pi;
+  // e^(-d_2/2 * (x_k - mu_k)^T Sigma_k^-1 (x_k - mu_k)) Equation 6.9 [Magnusson 2009]
+  double e_x_cov_x = gauss_d2_ * exp (-gauss_d2_ * x_trans.dot (c_inv * x_trans) / 2);
+
+  // Error checking for invalid values.
+  if (e_x_cov_x > 1 || e_x_cov_x < 0 || e_x_cov_x != e_x_cov_x)
+    return;
+
+  // Reusable portion of Equation 6.12 and 6.13 [Magnusson 2009]
+  e_x_cov_x *= gauss_d1_;
+
+  for (int i = 0; i < 6; i++)
+  {
+    // Sigma_k^-1 d(T(x,p))/dpi, Reusable portion of Equation 6.12 and 6.13 [Magnusson 2009]
+    cov_dxd_pi = c_inv * point_gradient_.col (i);
+
+    for (int j = 0; j < hessian.cols (); j++)
+    {
+      // Update hessian, Equation 6.13 [Magnusson 2009]
+      hessian (i, j) += e_x_cov_x * (-gauss_d2_ * x_trans.dot (cov_dxd_pi) * x_trans.dot (c_inv * point_gradient_.col (j)) +
+                                  x_trans.dot (c_inv * point_hessian_.block<3, 1>(3 * i, j)) +
+                                  point_gradient_.col (j).dot (cov_dxd_pi) );
+    }
+  }
+
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+template<typename PointSource, typename PointTarget> bool
+pclomp::NormalDistributionsTransform<PointSource, PointTarget>::updateIntervalMT (double &a_l, double &f_l, double &g_l,
+                                                                               double &a_u, double &f_u, double &g_u,
+                                                                               double a_t, double f_t, double g_t)
+{
+  // Case U1 in Update Algorithm and Case a in Modified Update Algorithm [More, Thuente 1994]
+  if (f_t > f_l)
+  {
+    a_u = a_t;
+    f_u = f_t;
+    g_u = g_t;
+    return (false);
+  }
+  // Case U2 in Update Algorithm and Case b in Modified Update Algorithm [More, Thuente 1994]
+  else
+  if (g_t * (a_l - a_t) > 0)
+  {
+    a_l = a_t;
+    f_l = f_t;
+    g_l = g_t;
+    return (false);
+  }
+  // Case U3 in Update Algorithm and Case c in Modified Update Algorithm [More, Thuente 1994]
+  else
+  if (g_t * (a_l - a_t) < 0)
+  {
+    a_u = a_l;
+    f_u = f_l;
+    g_u = g_l;
+
+    a_l = a_t;
+    f_l = f_t;
+    g_l = g_t;
+    return (false);
+  }
+  // Interval Converged
+  else
+    return (true);
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+template<typename PointSource, typename PointTarget> double
+pclomp::NormalDistributionsTransform<PointSource, PointTarget>::trialValueSelectionMT (double a_l, double f_l, double g_l,
+                                                                                    double a_u, double f_u, double g_u,
+                                                                                    double a_t, double f_t, double g_t)
+{
+  // Case 1 in Trial Value Selection [More, Thuente 1994]
+  if (f_t > f_l)
+  {
+    // Calculate the minimizer of the cubic that interpolates f_l, f_t, g_l and g_t
+    // Equation 2.4.52 [Sun, Yuan 2006]
+    double z = 3 * (f_t - f_l) / (a_t - a_l) - g_t - g_l;
+    double w = std::sqrt (z * z - g_t * g_l);
+    // Equation 2.4.56 [Sun, Yuan 2006]
+    double a_c = a_l + (a_t - a_l) * (w - g_l - z) / (g_t - g_l + 2 * w);
+
+    // Calculate the minimizer of the quadratic that interpolates f_l, f_t and g_l
+    // Equation 2.4.2 [Sun, Yuan 2006]
+    double a_q = a_l - 0.5 * (a_l - a_t) * g_l / (g_l - (f_l - f_t) / (a_l - a_t));
+
+    if (std::fabs (a_c - a_l) < std::fabs (a_q - a_l))
+      return (a_c);
+    else
+      return (0.5 * (a_q + a_c));
+  }
+  // Case 2 in Trial Value Selection [More, Thuente 1994]
+  else
+  if (g_t * g_l < 0)
+  {
+    // Calculate the minimizer of the cubic that interpolates f_l, f_t, g_l and g_t
+    // Equation 2.4.52 [Sun, Yuan 2006]
+    double z = 3 * (f_t - f_l) / (a_t - a_l) - g_t - g_l;
+    double w = std::sqrt (z * z - g_t * g_l);
+    // Equation 2.4.56 [Sun, Yuan 2006]
+    double a_c = a_l + (a_t - a_l) * (w - g_l - z) / (g_t - g_l + 2 * w);
+
+    // Calculate the minimizer of the quadratic that interpolates f_l, g_l and g_t
+    // Equation 2.4.5 [Sun, Yuan 2006]
+    double a_s = a_l - (a_l - a_t) / (g_l - g_t) * g_l;
+
+    if (std::fabs (a_c - a_t) >= std::fabs (a_s - a_t))
+      return (a_c);
+    else
+      return (a_s);
+  }
+  // Case 3 in Trial Value Selection [More, Thuente 1994]
+  else
+  if (std::fabs (g_t) <= std::fabs (g_l))
+  {
+    // Calculate the minimizer of the cubic that interpolates f_l, f_t, g_l and g_t
+    // Equation 2.4.52 [Sun, Yuan 2006]
+    double z = 3 * (f_t - f_l) / (a_t - a_l) - g_t - g_l;
+    double w = std::sqrt (z * z - g_t * g_l);
+    double a_c = a_l + (a_t - a_l) * (w - g_l - z) / (g_t - g_l + 2 * w);
+
+    // Calculate the minimizer of the quadratic that interpolates g_l and g_t
+    // Equation 2.4.5 [Sun, Yuan 2006]
+    double a_s = a_l - (a_l - a_t) / (g_l - g_t) * g_l;
+
+    double a_t_next;
+
+    if (std::fabs (a_c - a_t) < std::fabs (a_s - a_t))
+      a_t_next = a_c;
+    else
+      a_t_next = a_s;
+
+    if (a_t > a_l)
+      return (std::min (a_t + 0.66 * (a_u - a_t), a_t_next));
+    else
+      return (std::max (a_t + 0.66 * (a_u - a_t), a_t_next));
+  }
+  // Case 4 in Trial Value Selection [More, Thuente 1994]
+  else
+  {
+    // Calculate the minimizer of the cubic that interpolates f_u, f_t, g_u and g_t
+    // Equation 2.4.52 [Sun, Yuan 2006]
+    double z = 3 * (f_t - f_u) / (a_t - a_u) - g_t - g_u;
+    double w = std::sqrt (z * z - g_t * g_u);
+    // Equation 2.4.56 [Sun, Yuan 2006]
+    return (a_u + (a_t - a_u) * (w - g_u - z) / (g_t - g_u + 2 * w));
+  }
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+template<typename PointSource, typename PointTarget> double
+pclomp::NormalDistributionsTransform<PointSource, PointTarget>::computeStepLengthMT (const Eigen::Matrix<double, 6, 1> &x, Eigen::Matrix<double, 6, 1> &step_dir, double step_init, double step_max,
+                                                                                  double step_min, double &score, Eigen::Matrix<double, 6, 1> &score_gradient, Eigen::Matrix<double, 6, 6> &hessian,
+                                                                                  PointCloudSource &trans_cloud)
+{
+  // Set the value of phi(0), Equation 1.3 [More, Thuente 1994]
+  double phi_0 = -score;
+  // Set the value of phi'(0), Equation 1.3 [More, Thuente 1994]
+  double d_phi_0 = -(score_gradient.dot (step_dir));
+
+  Eigen::Matrix<double, 6, 1>  x_t;
+
+  if (d_phi_0 >= 0)
+  {
+    // Not a decent direction
+    if (d_phi_0 == 0)
+      return 0;
+    else
+    {
+      // Reverse step direction and calculate optimal step.
+      d_phi_0 *= -1;
+      step_dir *= -1;
+
+    }
+  }
+
+  // The Search Algorithm for T(mu) [More, Thuente 1994]
+
+  int max_step_iterations = 10;
+  int step_iterations = 0;
+
+  // Sufficient decrease constant, Equation 1.1 [More, Thuete 1994]
+  double mu = 1.e-4;
+  // Curvature condition constant, Equation 1.2 [More, Thuete 1994]
+  double nu = 0.9;
+
+  // Initial endpoints of Interval I,
+  double a_l = 0, a_u = 0;
+
+  // Auxiliary function psi is used until I is determined ot be a closed interval, Equation 2.1 [More, Thuente 1994]
+  double f_l = auxiliaryFunction_PsiMT (a_l, phi_0, phi_0, d_phi_0, mu);
+  double g_l = auxiliaryFunction_dPsiMT (d_phi_0, d_phi_0, mu);
+
+  double f_u = auxiliaryFunction_PsiMT (a_u, phi_0, phi_0, d_phi_0, mu);
+  double g_u = auxiliaryFunction_dPsiMT (d_phi_0, d_phi_0, mu);
+
+  // Check used to allow More-Thuente step length calculation to be skipped by making step_min == step_max
+  bool interval_converged = (step_max - step_min) < 0, open_interval = true;
+
+  double a_t = step_init;
+  a_t = std::min (a_t, step_max);
+  a_t = std::max (a_t, step_min);
+
+  x_t = x + step_dir * a_t;
+
+  final_transformation_ = (Eigen::Translation<float, 3>(static_cast<float> (x_t (0)), static_cast<float> (x_t (1)), static_cast<float> (x_t (2))) *
+                           Eigen::AngleAxis<float> (static_cast<float> (x_t (3)), Eigen::Vector3f::UnitX ()) *
+                           Eigen::AngleAxis<float> (static_cast<float> (x_t (4)), Eigen::Vector3f::UnitY ()) *
+                           Eigen::AngleAxis<float> (static_cast<float> (x_t (5)), Eigen::Vector3f::UnitZ ())).matrix ();
+
+  // New transformed point cloud
+  transformPointCloud (*input_, trans_cloud, final_transformation_);
+
+  // Updates score, gradient and hessian.  Hessian calculation is unnecessary but testing showed that most step calculations use the
+  // initial step suggestion and recalculation the reusable portions of the hessian would intail more computation time.
+  score = computeDerivatives (score_gradient, hessian, trans_cloud, x_t, true);
+
+  // Calculate phi(alpha_t)
+  double phi_t = -score;
+  // Calculate phi'(alpha_t)
+  double d_phi_t = -(score_gradient.dot (step_dir));
+
+  // Calculate psi(alpha_t)
+  double psi_t = auxiliaryFunction_PsiMT (a_t, phi_t, phi_0, d_phi_0, mu);
+  // Calculate psi'(alpha_t)
+  double d_psi_t = auxiliaryFunction_dPsiMT (d_phi_t, d_phi_0, mu);
+
+  // Iterate until max number of iterations, interval convergence or a value satisfies the sufficient decrease, Equation 1.1, and curvature condition, Equation 1.2 [More, Thuente 1994]
+  while (!interval_converged && step_iterations < max_step_iterations && !(psi_t <= 0 /*Sufficient Decrease*/ && d_phi_t <= -nu * d_phi_0 /*Curvature Condition*/))
+  {
+    // Use auxiliary function if interval I is not closed
+    if (open_interval)
+    {
+      a_t = trialValueSelectionMT (a_l, f_l, g_l,
+                                   a_u, f_u, g_u,
+                                   a_t, psi_t, d_psi_t);
+    }
+    else
+    {
+      a_t = trialValueSelectionMT (a_l, f_l, g_l,
+                                   a_u, f_u, g_u,
+                                   a_t, phi_t, d_phi_t);
+    }
+
+    a_t = std::min (a_t, step_max);
+    a_t = std::max (a_t, step_min);
+
+    x_t = x + step_dir * a_t;
+
+    final_transformation_ = (Eigen::Translation<float, 3> (static_cast<float> (x_t (0)), static_cast<float> (x_t (1)), static_cast<float> (x_t (2))) *
+                             Eigen::AngleAxis<float> (static_cast<float> (x_t (3)), Eigen::Vector3f::UnitX ()) *
+                             Eigen::AngleAxis<float> (static_cast<float> (x_t (4)), Eigen::Vector3f::UnitY ()) *
+                             Eigen::AngleAxis<float> (static_cast<float> (x_t (5)), Eigen::Vector3f::UnitZ ())).matrix ();
+
+    // New transformed point cloud
+    // Done on final cloud to prevent wasted computation
+    transformPointCloud (*input_, trans_cloud, final_transformation_);
+
+    // Updates score, gradient. Values stored to prevent wasted computation.
+    score = computeDerivatives (score_gradient, hessian, trans_cloud, x_t, false);
+
+    // Calculate phi(alpha_t+)
+    phi_t = -score;
+    // Calculate phi'(alpha_t+)
+    d_phi_t = -(score_gradient.dot (step_dir));
+
+    // Calculate psi(alpha_t+)
+    psi_t = auxiliaryFunction_PsiMT (a_t, phi_t, phi_0, d_phi_0, mu);
+    // Calculate psi'(alpha_t+)
+    d_psi_t = auxiliaryFunction_dPsiMT (d_phi_t, d_phi_0, mu);
+
+    // Check if I is now a closed interval
+    if (open_interval && (psi_t <= 0 && d_psi_t >= 0))
+    {
+      open_interval = false;
+
+      // Converts f_l and g_l from psi to phi
+      f_l = f_l + phi_0 - mu * d_phi_0 * a_l;
+      g_l = g_l + mu * d_phi_0;
+
+      // Converts f_u and g_u from psi to phi
+      f_u = f_u + phi_0 - mu * d_phi_0 * a_u;
+      g_u = g_u + mu * d_phi_0;
+    }
+
+    if (open_interval)
+    {
+      // Update interval end points using Updating Algorithm [More, Thuente 1994]
+      interval_converged = updateIntervalMT (a_l, f_l, g_l,
+                                             a_u, f_u, g_u,
+                                             a_t, psi_t, d_psi_t);
+    }
+    else
+    {
+      // Update interval end points using Modified Updating Algorithm [More, Thuente 1994]
+      interval_converged = updateIntervalMT (a_l, f_l, g_l,
+                                             a_u, f_u, g_u,
+                                             a_t, phi_t, d_phi_t);
+    }
+
+    step_iterations++;
+  }
+
+  // If inner loop was run then hessian needs to be calculated.
+  // Hessian is unnecessary for step length determination but gradients are required
+  // so derivative and transform data is stored for the next iteration.
+  if (step_iterations)
+    computeHessian (hessian, trans_cloud, x_t);
+
+  return (a_t);
+}
+
+
+template<typename PointSource, typename PointTarget>
+double pclomp::NormalDistributionsTransform<PointSource, PointTarget>::calculateScore(const PointCloudSource & trans_cloud) const
+{
+	double score = 0;
+
+	for (std::size_t idx = 0; idx < trans_cloud.points.size(); idx++)
+	{
+		PointSource x_trans_pt = trans_cloud.points[idx];
+
+		// Find neighbors (Radius search has been experimentally faster than direct neighbor checking.
+		std::vector<TargetGridLeafConstPtr> neighborhood;
+		std::vector<float> distances;
+		switch (search_method) {
+		case KDTREE:
+			target_cells_.radiusSearch(x_trans_pt, resolution_, neighborhood, distances);
+			break;
+		case DIRECT26:
+			target_cells_.getNeighborhoodAtPoint(x_trans_pt, neighborhood);
+			break;
+		default:
+		case DIRECT7:
+			target_cells_.getNeighborhoodAtPoint7(x_trans_pt, neighborhood);
+			break;
+		case DIRECT1:
+			target_cells_.getNeighborhoodAtPoint1(x_trans_pt, neighborhood);
+			break;
+		}
+
+		for (typename std::vector<TargetGridLeafConstPtr>::iterator neighborhood_it = neighborhood.begin(); neighborhood_it != neighborhood.end(); neighborhood_it++)
+		{
+			TargetGridLeafConstPtr cell = *neighborhood_it;
+
+			Eigen::Vector3d x_trans = Eigen::Vector3d(x_trans_pt.x, x_trans_pt.y, x_trans_pt.z);
+
+			// Denorm point, x_k' in Equations 6.12 and 6.13 [Magnusson 2009]
+			x_trans -= cell->getMean();
+			// Uses precomputed covariance for speed.
+			Eigen::Matrix3d c_inv = cell->getInverseCov();
+
+			// e^(-d_2/2 * (x_k - mu_k)^T Sigma_k^-1 (x_k - mu_k)) Equation 6.9 [Magnusson 2009]
+			double e_x_cov_x = exp(-gauss_d2_ * x_trans.dot(c_inv * x_trans) / 2);
+			// Calculate probability of transformed points existence, Equation 6.9 [Magnusson 2009]
+			double score_inc = -gauss_d1_ * e_x_cov_x - gauss_d3_;
+
+			score += score_inc / neighborhood.size();
+		}
+	}
+	return (score) / static_cast<double> (trans_cloud.size());
+}
+
+#endif // PCL_REGISTRATION_NDT_IMPL_H_

src/detection/detection_ndt_pclomp/pclomp/pclomp/ndt_omp.h

@@ -0,0 +1,506 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Point Cloud Library (PCL) - www.pointclouds.org
+ *  Copyright (c) 2010-2012, Willow Garage, Inc.
+ *  Copyright (c) 2012-, Open Perception, Inc.
+ *
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the copyright holder(s) nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id$
+ *
+ */
+
+#ifndef PCL_REGISTRATION_NDT_OMP_H_
+#define PCL_REGISTRATION_NDT_OMP_H_
+
+#include <pcl/registration/registration.h>
+#include <pcl/search/impl/search.hpp>
+#include <pclomp/voxel_grid_covariance_omp.h>
+
+#include <unsupported/Eigen/NonLinearOptimization>
+
+namespace pclomp
+{
+	enum NeighborSearchMethod {
+		KDTREE,
+		DIRECT26,
+		DIRECT7,
+		DIRECT1
+	};
+
+	/** \brief A 3D Normal Distribution Transform registration implementation for point cloud data.
+	  * \note For more information please see
+	  * <b>Magnusson, M. (2009). The Three-Dimensional Normal-Distributions Transform —
+	  * an Efficient Representation for Registration, Surface Analysis, and Loop Detection.
+	  * PhD thesis, Orebro University. Orebro Studies in Technology 36.</b>,
+	  * <b>More, J., and Thuente, D. (1994). Line Search Algorithm with Guaranteed Sufficient Decrease
+	  * In ACM Transactions on Mathematical Software.</b> and
+	  * Sun, W. and Yuan, Y, (2006) Optimization Theory and Methods: Nonlinear Programming. 89-100
+	  * \note Math refactored by Todor Stoyanov.
+	  * \author Brian Okorn (Space and Naval Warfare Systems Center Pacific)
+	  */
+	template<typename PointSource, typename PointTarget>
+	class NormalDistributionsTransform : public pcl::Registration<PointSource, PointTarget>
+	{
+	protected:
+
+		typedef typename pcl::Registration<PointSource, PointTarget>::PointCloudSource PointCloudSource;
+		typedef typename PointCloudSource::Ptr PointCloudSourcePtr;
+		typedef typename PointCloudSource::ConstPtr PointCloudSourceConstPtr;
+
+		typedef typename pcl::Registration<PointSource, PointTarget>::PointCloudTarget PointCloudTarget;
+		typedef typename PointCloudTarget::Ptr PointCloudTargetPtr;
+		typedef typename PointCloudTarget::ConstPtr PointCloudTargetConstPtr;
+
+		typedef pcl::PointIndices::Ptr PointIndicesPtr;
+		typedef pcl::PointIndices::ConstPtr PointIndicesConstPtr;
+
+		/** \brief Typename of searchable voxel grid containing mean and covariance. */
+		typedef pclomp::VoxelGridCovariance<PointTarget> TargetGrid;
+		/** \brief Typename of pointer to searchable voxel grid. */
+		typedef TargetGrid* TargetGridPtr;
+		/** \brief Typename of const pointer to searchable voxel grid. */
+		typedef const TargetGrid* TargetGridConstPtr;
+		/** \brief Typename of const pointer to searchable voxel grid leaf. */
+		typedef typename TargetGrid::LeafConstPtr TargetGridLeafConstPtr;
+
+
+	public:
+
+#if PCL_VERSION >= PCL_VERSION_CALC(1, 10, 0)
+		typedef pcl::shared_ptr< NormalDistributionsTransform<PointSource, PointTarget> > Ptr;
+		typedef pcl::shared_ptr< const NormalDistributionsTransform<PointSource, PointTarget> > ConstPtr;
+#else
+		typedef boost::shared_ptr< NormalDistributionsTransform<PointSource, PointTarget> > Ptr;
+		typedef boost::shared_ptr< const NormalDistributionsTransform<PointSource, PointTarget> > ConstPtr;
+#endif
+
+
+		/** \brief Constructor.
+		  * Sets \ref outlier_ratio_ to 0.35, \ref step_size_ to 0.05 and \ref resolution_ to 1.0
+		  */
+		NormalDistributionsTransform();
+
+		/** \brief Empty destructor */
+		virtual ~NormalDistributionsTransform() {}
+
+    void setNumThreads(int n) {
+      num_threads_ = n;
+    }
+
+		/** \brief Provide a pointer to the input target (e.g., the point cloud that we want to align the input source to).
+		  * \param[in] cloud the input point cloud target
+		  */
+		inline void
+			setInputTarget(const PointCloudTargetConstPtr &cloud)
+		{
+			pcl::Registration<PointSource, PointTarget>::setInputTarget(cloud);
+			init();
+		}
+
+		/** \brief Set/change the voxel grid resolution.
+		  * \param[in] resolution side length of voxels
+		  */
+		inline void
+			setResolution(float resolution)
+		{
+			// Prevents unnecessary voxel initiations
+			if (resolution_ != resolution)
+			{
+				resolution_ = resolution;
+				if (input_)
+					init();
+			}
+		}
+
+		/** \brief Get voxel grid resolution.
+		  * \return side length of voxels
+		  */
+		inline float
+			getResolution() const
+		{
+			return (resolution_);
+		}
+
+		/** \brief Get the newton line search maximum step length.
+		  * \return maximum step length
+		  */
+		inline double
+			getStepSize() const
+		{
+			return (step_size_);
+		}
+
+		/** \brief Set/change the newton line search maximum step length.
+		  * \param[in] step_size maximum step length
+		  */
+		inline void
+			setStepSize(double step_size)
+		{
+			step_size_ = step_size;
+		}
+
+		/** \brief Get the point cloud outlier ratio.
+		  * \return outlier ratio
+		  */
+		inline double
+			getOutlierRatio() const
+		{
+			return (outlier_ratio_);
+		}
+
+		/** \brief Set/change the point cloud outlier ratio.
+		  * \param[in] outlier_ratio outlier ratio
+		  */
+		inline void
+			setOutlierRatio(double outlier_ratio)
+		{
+			outlier_ratio_ = outlier_ratio;
+		}
+
+		inline void setNeighborhoodSearchMethod(NeighborSearchMethod method) {
+			search_method = method;
+		}
+
+		/** \brief Get the registration alignment probability.
+		  * \return transformation probability
+		  */
+		inline double
+			getTransformationProbability() const
+		{
+			return (trans_probability_);
+		}
+
+		/** \brief Get the number of iterations required to calculate alignment.
+		  * \return final number of iterations
+		  */
+		inline int
+			getFinalNumIteration() const
+		{
+			return (nr_iterations_);
+		}
+
+		/** \brief Convert 6 element transformation vector to affine transformation.
+		  * \param[in] x transformation vector of the form [x, y, z, roll, pitch, yaw]
+		  * \param[out] trans affine transform corresponding to given transformation vector
+		  */
+		static void
+			convertTransform(const Eigen::Matrix<double, 6, 1> &x, Eigen::Affine3f &trans)
+		{
+			trans = Eigen::Translation<float, 3>(float(x(0)), float(x(1)), float(x(2))) *
+				Eigen::AngleAxis<float>(float(x(3)), Eigen::Vector3f::UnitX()) *
+				Eigen::AngleAxis<float>(float(x(4)), Eigen::Vector3f::UnitY()) *
+				Eigen::AngleAxis<float>(float(x(5)), Eigen::Vector3f::UnitZ());
+		}
+
+		/** \brief Convert 6 element transformation vector to transformation matrix.
+		  * \param[in] x transformation vector of the form [x, y, z, roll, pitch, yaw]
+		  * \param[out] trans 4x4 transformation matrix corresponding to given transformation vector
+		  */
+		static void
+			convertTransform(const Eigen::Matrix<double, 6, 1> &x, Eigen::Matrix4f &trans)
+		{
+			Eigen::Affine3f _affine;
+			convertTransform(x, _affine);
+			trans = _affine.matrix();
+		}
+
+		// negative log likelihood function
+		// lower is better
+		double calculateScore(const PointCloudSource& cloud) const;
+
+	protected:
+
+		using pcl::Registration<PointSource, PointTarget>::reg_name_;
+		using pcl::Registration<PointSource, PointTarget>::getClassName;
+		using pcl::Registration<PointSource, PointTarget>::input_;
+		using pcl::Registration<PointSource, PointTarget>::indices_;
+		using pcl::Registration<PointSource, PointTarget>::target_;
+		using pcl::Registration<PointSource, PointTarget>::nr_iterations_;
+		using pcl::Registration<PointSource, PointTarget>::max_iterations_;
+		using pcl::Registration<PointSource, PointTarget>::previous_transformation_;
+		using pcl::Registration<PointSource, PointTarget>::final_transformation_;
+		using pcl::Registration<PointSource, PointTarget>::transformation_;
+		using pcl::Registration<PointSource, PointTarget>::transformation_epsilon_;
+		using pcl::Registration<PointSource, PointTarget>::converged_;
+		using pcl::Registration<PointSource, PointTarget>::corr_dist_threshold_;
+		using pcl::Registration<PointSource, PointTarget>::inlier_threshold_;
+
+		using pcl::Registration<PointSource, PointTarget>::update_visualizer_;
+
+		/** \brief Estimate the transformation and returns the transformed source (input) as output.
+		  * \param[out] output the resultant input transformed point cloud dataset
+		  */
+		virtual void
+			computeTransformation(PointCloudSource &output)
+		{
+			computeTransformation(output, Eigen::Matrix4f::Identity());
+		}
+
+		/** \brief Estimate the transformation and returns the transformed source (input) as output.
+		  * \param[out] output the resultant input transformed point cloud dataset
+		  * \param[in] guess the initial gross estimation of the transformation
+		  */
+		virtual void
+			computeTransformation(PointCloudSource &output, const Eigen::Matrix4f &guess);
+
+		/** \brief Initiate covariance voxel structure. */
+		void inline
+			init()
+		{
+			target_cells_.setLeafSize(resolution_, resolution_, resolution_);
+			target_cells_.setInputCloud(target_);
+			// Initiate voxel structure.
+			target_cells_.filter(true);
+		}
+
+		/** \brief Compute derivatives of probability function w.r.t. the transformation vector.
+		  * \note Equation 6.10, 6.12 and 6.13 [Magnusson 2009].
+		  * \param[out] score_gradient the gradient vector of the probability function w.r.t. the transformation vector
+		  * \param[out] hessian the hessian matrix of the probability function w.r.t. the transformation vector
+		  * \param[in] trans_cloud transformed point cloud
+		  * \param[in] p the current transform vector
+		  * \param[in] compute_hessian flag to calculate hessian, unnecessary for step calculation.
+		  */
+		double
+			computeDerivatives(Eigen::Matrix<double, 6, 1> &score_gradient,
+				Eigen::Matrix<double, 6, 6> &hessian,
+				PointCloudSource &trans_cloud,
+				Eigen::Matrix<double, 6, 1> &p,
+				bool compute_hessian = true);
+
+		/** \brief Compute individual point contributions to derivatives of probability function w.r.t. the transformation vector.
+		  * \note Equation 6.10, 6.12 and 6.13 [Magnusson 2009].
+		  * \param[in,out] score_gradient the gradient vector of the probability function w.r.t. the transformation vector
+		  * \param[in,out] hessian the hessian matrix of the probability function w.r.t. the transformation vector
+		  * \param[in] x_trans transformed point minus mean of occupied covariance voxel
+		  * \param[in] c_inv covariance of occupied covariance voxel
+		  * \param[in] compute_hessian flag to calculate hessian, unnecessary for step calculation.
+		  */
+		double
+			updateDerivatives(Eigen::Matrix<double, 6, 1> &score_gradient,
+				Eigen::Matrix<double, 6, 6> &hessian,
+				const Eigen::Matrix<float, 4, 6> &point_gradient_,
+				const Eigen::Matrix<float, 24, 6> &point_hessian_,
+				const Eigen::Vector3d &x_trans, const Eigen::Matrix3d &c_inv,
+				bool compute_hessian = true) const;
+
+		/** \brief Precompute angular components of derivatives.
+		  * \note Equation 6.19 and 6.21 [Magnusson 2009].
+		  * \param[in] p the current transform vector
+		  * \param[in] compute_hessian flag to calculate hessian, unnecessary for step calculation.
+		  */
+		void
+			computeAngleDerivatives(Eigen::Matrix<double, 6, 1> &p, bool compute_hessian = true);
+
+		/** \brief Compute point derivatives.
+		  * \note Equation 6.18-21 [Magnusson 2009].
+		  * \param[in] x point from the input cloud
+		  * \param[in] compute_hessian flag to calculate hessian, unnecessary for step calculation.
+		  */
+		void
+			computePointDerivatives(Eigen::Vector3d &x, Eigen::Matrix<double, 3, 6>& point_gradient_, Eigen::Matrix<double, 18, 6>& point_hessian_, bool compute_hessian = true) const;
+
+		void
+			computePointDerivatives(Eigen::Vector3d &x, Eigen::Matrix<float, 4, 6>& point_gradient_, Eigen::Matrix<float, 24, 6>& point_hessian_, bool compute_hessian = true) const;
+
+		/** \brief Compute hessian of probability function w.r.t. the transformation vector.
+		  * \note Equation 6.13 [Magnusson 2009].
+		  * \param[out] hessian the hessian matrix of the probability function w.r.t. the transformation vector
+		  * \param[in] trans_cloud transformed point cloud
+		  * \param[in] p the current transform vector
+		  */
+		void
+			computeHessian(Eigen::Matrix<double, 6, 6> &hessian,
+				PointCloudSource &trans_cloud,
+				Eigen::Matrix<double, 6, 1> &p);
+
+		/** \brief Compute individual point contributions to hessian of probability function w.r.t. the transformation vector.
+		  * \note Equation 6.13 [Magnusson 2009].
+		  * \param[in,out] hessian the hessian matrix of the probability function w.r.t. the transformation vector
+		  * \param[in] x_trans transformed point minus mean of occupied covariance voxel
+		  * \param[in] c_inv covariance of occupied covariance voxel
+		  */
+		void
+			updateHessian(Eigen::Matrix<double, 6, 6> &hessian,
+				const Eigen::Matrix<double, 3, 6> &point_gradient_,
+				const Eigen::Matrix<double, 18, 6> &point_hessian_,
+				const Eigen::Vector3d &x_trans, const Eigen::Matrix3d &c_inv) const;
+
+		/** \brief Compute line search step length and update transform and probability derivatives using More-Thuente method.
+		  * \note Search Algorithm [More, Thuente 1994]
+		  * \param[in] x initial transformation vector, \f$ x \f$ in Equation 1.3 (Moore, Thuente 1994) and \f$ \vec{p} \f$ in Algorithm 2 [Magnusson 2009]
+		  * \param[in] step_dir descent direction, \f$ p \f$ in Equation 1.3 (Moore, Thuente 1994) and \f$ \delta \vec{p} \f$ normalized in Algorithm 2 [Magnusson 2009]
+		  * \param[in] step_init initial step length estimate, \f$ \alpha_0 \f$ in Moore-Thuente (1994) and the normal of \f$ \delta \vec{p} \f$ in Algorithm 2 [Magnusson 2009]
+		  * \param[in] step_max maximum step length, \f$ \alpha_max \f$ in Moore-Thuente (1994)
+		  * \param[in] step_min minimum step length, \f$ \alpha_min \f$ in Moore-Thuente (1994)
+		  * \param[out] score final score function value, \f$ f(x + \alpha p) \f$ in Equation 1.3 (Moore, Thuente 1994) and \f$ score \f$ in Algorithm 2 [Magnusson 2009]
+		  * \param[in,out] score_gradient gradient of score function w.r.t. transformation vector, \f$ f'(x + \alpha p) \f$ in Moore-Thuente (1994) and \f$ \vec{g} \f$ in Algorithm 2 [Magnusson 2009]
+		  * \param[out] hessian hessian of score function w.r.t. transformation vector, \f$ f''(x + \alpha p) \f$ in Moore-Thuente (1994) and \f$ H \f$ in Algorithm 2 [Magnusson 2009]
+		  * \param[in,out] trans_cloud transformed point cloud, \f$ X \f$ transformed by \f$ T(\vec{p},\vec{x}) \f$ in Algorithm 2 [Magnusson 2009]
+		  * \return final step length
+		  */
+		double
+			computeStepLengthMT(const Eigen::Matrix<double, 6, 1> &x,
+				Eigen::Matrix<double, 6, 1> &step_dir,
+				double step_init,
+				double step_max, double step_min,
+				double &score,
+				Eigen::Matrix<double, 6, 1> &score_gradient,
+				Eigen::Matrix<double, 6, 6> &hessian,
+				PointCloudSource &trans_cloud);
+
+		/** \brief Update interval of possible step lengths for More-Thuente method, \f$ I \f$ in More-Thuente (1994)
+		  * \note Updating Algorithm until some value satisfies \f$ \psi(\alpha_k) \leq 0 \f$ and \f$ \phi'(\alpha_k) \geq 0 \f$
+		  * and Modified Updating Algorithm from then on [More, Thuente 1994].
+		  * \param[in,out] a_l first endpoint of interval \f$ I \f$, \f$ \alpha_l \f$ in Moore-Thuente (1994)
+		  * \param[in,out] f_l value at first endpoint, \f$ f_l \f$ in Moore-Thuente (1994), \f$ \psi(\alpha_l) \f$ for Update Algorithm and \f$ \phi(\alpha_l) \f$ for Modified Update Algorithm
+		  * \param[in,out] g_l derivative at first endpoint, \f$ g_l \f$ in Moore-Thuente (1994), \f$ \psi'(\alpha_l) \f$ for Update Algorithm and \f$ \phi'(\alpha_l) \f$ for Modified Update Algorithm
+		  * \param[in,out] a_u second endpoint of interval \f$ I \f$, \f$ \alpha_u \f$ in Moore-Thuente (1994)
+		  * \param[in,out] f_u value at second endpoint, \f$ f_u \f$ in Moore-Thuente (1994), \f$ \psi(\alpha_u) \f$ for Update Algorithm and \f$ \phi(\alpha_u) \f$ for Modified Update Algorithm
+		  * \param[in,out] g_u derivative at second endpoint, \f$ g_u \f$ in Moore-Thuente (1994), \f$ \psi'(\alpha_u) \f$ for Update Algorithm and \f$ \phi'(\alpha_u) \f$ for Modified Update Algorithm
+		  * \param[in] a_t trial value, \f$ \alpha_t \f$ in Moore-Thuente (1994)
+		  * \param[in] f_t value at trial value, \f$ f_t \f$ in Moore-Thuente (1994), \f$ \psi(\alpha_t) \f$ for Update Algorithm and \f$ \phi(\alpha_t) \f$ for Modified Update Algorithm
+		  * \param[in] g_t derivative at trial value, \f$ g_t \f$ in Moore-Thuente (1994), \f$ \psi'(\alpha_t) \f$ for Update Algorithm and \f$ \phi'(\alpha_t) \f$ for Modified Update Algorithm
+		  * \return if interval converges
+		  */
+		bool
+			updateIntervalMT(double &a_l, double &f_l, double &g_l,
+				double &a_u, double &f_u, double &g_u,
+				double a_t, double f_t, double g_t);
+
+		/** \brief Select new trial value for More-Thuente method.
+		  * \note Trial Value Selection [More, Thuente 1994], \f$ \psi(\alpha_k) \f$ is used for \f$ f_k \f$ and \f$ g_k \f$
+		  * until some value satisfies the test \f$ \psi(\alpha_k) \leq 0 \f$ and \f$ \phi'(\alpha_k) \geq 0 \f$
+		  * then \f$ \phi(\alpha_k) \f$ is used from then on.
+		  * \note Interpolation Minimizer equations from Optimization Theory and Methods: Nonlinear Programming By Wenyu Sun, Ya-xiang Yuan (89-100).
+		  * \param[in] a_l first endpoint of interval \f$ I \f$, \f$ \alpha_l \f$ in Moore-Thuente (1994)
+		  * \param[in] f_l value at first endpoint, \f$ f_l \f$ in Moore-Thuente (1994)
+		  * \param[in] g_l derivative at first endpoint, \f$ g_l \f$ in Moore-Thuente (1994)
+		  * \param[in] a_u second endpoint of interval \f$ I \f$, \f$ \alpha_u \f$ in Moore-Thuente (1994)
+		  * \param[in] f_u value at second endpoint, \f$ f_u \f$ in Moore-Thuente (1994)
+		  * \param[in] g_u derivative at second endpoint, \f$ g_u \f$ in Moore-Thuente (1994)
+		  * \param[in] a_t previous trial value, \f$ \alpha_t \f$ in Moore-Thuente (1994)
+		  * \param[in] f_t value at previous trial value, \f$ f_t \f$ in Moore-Thuente (1994)
+		  * \param[in] g_t derivative at previous trial value, \f$ g_t \f$ in Moore-Thuente (1994)
+		  * \return new trial value
+		  */
+		double
+			trialValueSelectionMT(double a_l, double f_l, double g_l,
+				double a_u, double f_u, double g_u,
+				double a_t, double f_t, double g_t);
+
+		/** \brief Auxiliary function used to determine endpoints of More-Thuente interval.
+		  * \note \f$ \psi(\alpha) \f$ in Equation 1.6 (Moore, Thuente 1994)
+		  * \param[in] a the step length, \f$ \alpha \f$ in More-Thuente (1994)
+		  * \param[in] f_a function value at step length a, \f$ \phi(\alpha) \f$ in More-Thuente (1994)
+		  * \param[in] f_0 initial function value, \f$ \phi(0) \f$ in Moore-Thuente (1994)
+		  * \param[in] g_0 initial function gradiant, \f$ \phi'(0) \f$ in More-Thuente (1994)
+		  * \param[in] mu the step length, constant \f$ \mu \f$ in Equation 1.1 [More, Thuente 1994]
+		  * \return sufficient decrease value
+		  */
+		inline double
+			auxiliaryFunction_PsiMT(double a, double f_a, double f_0, double g_0, double mu = 1.e-4)
+		{
+			return (f_a - f_0 - mu * g_0 * a);
+		}
+
+		/** \brief Auxiliary function derivative used to determine endpoints of More-Thuente interval.
+		  * \note \f$ \psi'(\alpha) \f$, derivative of Equation 1.6 (Moore, Thuente 1994)
+		  * \param[in] g_a function gradient at step length a, \f$ \phi'(\alpha) \f$ in More-Thuente (1994)
+		  * \param[in] g_0 initial function gradiant, \f$ \phi'(0) \f$ in More-Thuente (1994)
+		  * \param[in] mu the step length, constant \f$ \mu \f$ in Equation 1.1 [More, Thuente 1994]
+		  * \return sufficient decrease derivative
+		  */
+		inline double
+			auxiliaryFunction_dPsiMT(double g_a, double g_0, double mu = 1.e-4)
+		{
+			return (g_a - mu * g_0);
+		}
+
+		/** \brief The voxel grid generated from target cloud containing point means and covariances. */
+		TargetGrid target_cells_;
+
+		//double fitness_epsilon_;
+
+		/** \brief The side length of voxels. */
+		float resolution_;
+
+		/** \brief The maximum step length. */
+		double step_size_;
+
+		/** \brief The ratio of outliers of points w.r.t. a normal distribution, Equation 6.7 [Magnusson 2009]. */
+		double outlier_ratio_;
+
+		/** \brief The normalization constants used fit the point distribution to a normal distribution, Equation 6.8 [Magnusson 2009]. */
+		double gauss_d1_, gauss_d2_, gauss_d3_;
+
+		/** \brief The probability score of the transform applied to the input cloud, Equation 6.9 and 6.10 [Magnusson 2009]. */
+		double trans_probability_;
+
+		/** \brief Precomputed Angular Gradient
+		  *
+		  * The precomputed angular derivatives for the jacobian of a transformation vector, Equation 6.19 [Magnusson 2009].
+		  */
+		Eigen::Vector3d j_ang_a_, j_ang_b_, j_ang_c_, j_ang_d_, j_ang_e_, j_ang_f_, j_ang_g_, j_ang_h_;
+
+		Eigen::Matrix<float, 8, 4> j_ang;
+
+		/** \brief Precomputed Angular Hessian
+		  *
+		  * The precomputed angular derivatives for the hessian of a transformation vector, Equation 6.19 [Magnusson 2009].
+		  */
+		Eigen::Vector3d h_ang_a2_, h_ang_a3_,
+			h_ang_b2_, h_ang_b3_,
+			h_ang_c2_, h_ang_c3_,
+			h_ang_d1_, h_ang_d2_, h_ang_d3_,
+			h_ang_e1_, h_ang_e2_, h_ang_e3_,
+			h_ang_f1_, h_ang_f2_, h_ang_f3_;
+
+		Eigen::Matrix<float, 16, 4> h_ang;
+
+		/** \brief The first order derivative of the transformation of a point w.r.t. the transform vector, \f$ J_E \f$ in Equation 6.18 [Magnusson 2009]. */
+  //      Eigen::Matrix<double, 3, 6> point_gradient_;
+
+		/** \brief The second order derivative of the transformation of a point w.r.t. the transform vector, \f$ H_E \f$ in Equation 6.20 [Magnusson 2009]. */
+  //      Eigen::Matrix<double, 18, 6> point_hessian_;
+
+    int num_threads_;
+
+	public:
+		NeighborSearchMethod search_method;
+
+		EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+	};
+
+}
+
+#endif // PCL_REGISTRATION_NDT_H_

src/detection/detection_ndt_pclomp/pclomp/pclomp/voxel_grid_covariance_omp.h

@@ -0,0 +1,557 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Point Cloud Library (PCL) - www.pointclouds.org
+ *  Copyright (c) 2010-2011, Willow Garage, Inc.
+ *
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the copyright holder(s) nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef PCL_VOXEL_GRID_COVARIANCE_OMP_H_
+#define PCL_VOXEL_GRID_COVARIANCE_OMP_H_
+
+#include <pcl/pcl_macros.h>
+#include <pcl/filters/boost.h>
+#include <pcl/filters/voxel_grid.h>
+#include <map>
+#include <unordered_map>
+#include <pcl/point_types.h>
+#include <pcl/kdtree/kdtree_flann.h>
+
+namespace pclomp
+{
+  /** \brief A searchable voxel structure containing the mean and covariance of the data.
+    * \note For more information please see
+    * <b>Magnusson, M. (2009). The Three-Dimensional Normal-Distributions Transform —
+    * an Efficient Representation for Registration, Surface Analysis, and Loop Detection.
+    * PhD thesis, Orebro University. Orebro Studies in Technology 36</b>
+    * \author Brian Okorn (Space and Naval Warfare Systems Center Pacific)
+    */
+  template<typename PointT>
+  class VoxelGridCovariance : public pcl::VoxelGrid<PointT>
+  {
+    protected:
+      using pcl::VoxelGrid<PointT>::filter_name_;
+      using pcl::VoxelGrid<PointT>::getClassName;
+      using pcl::VoxelGrid<PointT>::input_;
+      using pcl::VoxelGrid<PointT>::indices_;
+      using pcl::VoxelGrid<PointT>::filter_limit_negative_;
+      using pcl::VoxelGrid<PointT>::filter_limit_min_;
+      using pcl::VoxelGrid<PointT>::filter_limit_max_;
+      using pcl::VoxelGrid<PointT>::filter_field_name_;
+
+      using pcl::VoxelGrid<PointT>::downsample_all_data_;
+      using pcl::VoxelGrid<PointT>::leaf_layout_;
+      using pcl::VoxelGrid<PointT>::save_leaf_layout_;
+      using pcl::VoxelGrid<PointT>::leaf_size_;
+      using pcl::VoxelGrid<PointT>::min_b_;
+      using pcl::VoxelGrid<PointT>::max_b_;
+      using pcl::VoxelGrid<PointT>::inverse_leaf_size_;
+      using pcl::VoxelGrid<PointT>::div_b_;
+      using pcl::VoxelGrid<PointT>::divb_mul_;
+
+      typedef typename pcl::traits::fieldList<PointT>::type FieldList;
+      typedef typename pcl::Filter<PointT>::PointCloud PointCloud;
+      typedef typename PointCloud::Ptr PointCloudPtr;
+      typedef typename PointCloud::ConstPtr PointCloudConstPtr;
+
+    public:
+
+#if PCL_VERSION >= PCL_VERSION_CALC(1, 10, 0)
+      typedef pcl::shared_ptr< pcl::VoxelGrid<PointT> > Ptr;
+      typedef pcl::shared_ptr< const pcl::VoxelGrid<PointT> > ConstPtr;
+#else
+      typedef boost::shared_ptr< pcl::VoxelGrid<PointT> > Ptr;
+      typedef boost::shared_ptr< const pcl::VoxelGrid<PointT> > ConstPtr;
+#endif
+
+      /** \brief Simple structure to hold a centroid, covariance and the number of points in a leaf.
+        * Inverse covariance, eigen vectors and eigen values are precomputed. */
+      struct Leaf
+      {
+        /** \brief Constructor.
+         * Sets \ref nr_points, \ref icov_, \ref mean_ and \ref evals_ to 0 and \ref cov_ and \ref evecs_ to the identity matrix
+         */
+        Leaf () :
+          nr_points (0),
+          mean_ (Eigen::Vector3d::Zero ()),
+          centroid (),
+          cov_ (Eigen::Matrix3d::Identity ()),
+          icov_ (Eigen::Matrix3d::Zero ()),
+          evecs_ (Eigen::Matrix3d::Identity ()),
+          evals_ (Eigen::Vector3d::Zero ())
+        {
+        }
+
+        /** \brief Get the voxel covariance.
+          * \return covariance matrix
+          */
+        Eigen::Matrix3d
+        getCov () const
+        {
+          return (cov_);
+        }
+
+        /** \brief Get the inverse of the voxel covariance.
+          * \return inverse covariance matrix
+          */
+        Eigen::Matrix3d
+        getInverseCov () const
+        {
+          return (icov_);
+        }
+
+        /** \brief Get the voxel centroid.
+          * \return centroid
+          */
+        Eigen::Vector3d
+        getMean () const
+        {
+          return (mean_);
+        }
+
+        /** \brief Get the eigen vectors of the voxel covariance.
+          * \note Order corresponds with \ref getEvals
+          * \return matrix whose columns contain eigen vectors
+          */
+        Eigen::Matrix3d
+        getEvecs () const
+        {
+          return (evecs_);
+        }
+
+        /** \brief Get the eigen values of the voxel covariance.
+          * \note Order corresponds with \ref getEvecs
+          * \return vector of eigen values
+          */
+        Eigen::Vector3d
+        getEvals () const
+        {
+          return (evals_);
+        }
+
+        /** \brief Get the number of points contained by this voxel.
+          * \return number of points
+          */
+        int
+        getPointCount () const
+        {
+          return (nr_points);
+        }
+
+        /** \brief Number of points contained by voxel */
+        int nr_points;
+
+        /** \brief 3D voxel centroid */
+        Eigen::Vector3d mean_;
+
+        /** \brief Nd voxel centroid
+         * \note Differs from \ref mean_ when color data is used
+         */
+        Eigen::VectorXf centroid;
+
+        /** \brief Voxel covariance matrix */
+        Eigen::Matrix3d cov_;
+
+        /** \brief Inverse of voxel covariance matrix */
+        Eigen::Matrix3d icov_;
+
+        /** \brief Eigen vectors of voxel covariance matrix */
+        Eigen::Matrix3d evecs_;
+
+        /** \brief Eigen values of voxel covariance matrix */
+        Eigen::Vector3d evals_;
+
+      };
+
+      /** \brief Pointer to VoxelGridCovariance leaf structure */
+      typedef Leaf* LeafPtr;
+
+      /** \brief Const pointer to VoxelGridCovariance leaf structure */
+      typedef const Leaf* LeafConstPtr;
+
+    typedef std::map<size_t, Leaf> Map;
+
+    public:
+
+      /** \brief Constructor.
+       * Sets \ref leaf_size_ to 0 and \ref searchable_ to false.
+       */
+      VoxelGridCovariance () :
+        searchable_ (true),
+        min_points_per_voxel_ (6),
+        min_covar_eigvalue_mult_ (0.01),
+        leaves_ (),
+        voxel_centroids_ (),
+        voxel_centroids_leaf_indices_ (),
+        kdtree_ ()
+      {
+        downsample_all_data_ = false;
+        save_leaf_layout_ = false;
+        leaf_size_.setZero ();
+        min_b_.setZero ();
+        max_b_.setZero ();
+        filter_name_ = "VoxelGridCovariance";
+      }
+
+      /** \brief Set the minimum number of points required for a cell to be used (must be 3 or greater for covariance calculation).
+        * \param[in] min_points_per_voxel the minimum number of points for required for a voxel to be used
+        */
+      inline void
+      setMinPointPerVoxel (int min_points_per_voxel)
+      {
+        if(min_points_per_voxel > 2)
+        {
+          min_points_per_voxel_ = min_points_per_voxel;
+        }
+        else
+        {
+          PCL_WARN ("%s: Covariance calculation requires at least 3 points, setting Min Point per Voxel to 3 ", this->getClassName ().c_str ());
+          min_points_per_voxel_ = 3;
+        }
+      }
+
+      /** \brief Get the minimum number of points required for a cell to be used.
+        * \return the minimum number of points for required for a voxel to be used
+        */
+      inline int
+      getMinPointPerVoxel ()
+      {
+        return min_points_per_voxel_;
+      }
+
+      /** \brief Set the minimum allowable ratio between eigenvalues to prevent singular covariance matrices.
+        * \param[in] min_covar_eigvalue_mult the minimum allowable ratio between eigenvalues
+        */
+      inline void
+      setCovEigValueInflationRatio (double min_covar_eigvalue_mult)
+      {
+        min_covar_eigvalue_mult_ = min_covar_eigvalue_mult;
+      }
+
+      /** \brief Get the minimum allowable ratio between eigenvalues to prevent singular covariance matrices.
+        * \return the minimum allowable ratio between eigenvalues
+        */
+      inline double
+      getCovEigValueInflationRatio ()
+      {
+        return min_covar_eigvalue_mult_;
+      }
+
+      /** \brief Filter cloud and initializes voxel structure.
+       * \param[out] output cloud containing centroids of voxels containing a sufficient number of points
+       * \param[in] searchable flag if voxel structure is searchable, if true then kdtree is built
+       */
+      inline void
+      filter (PointCloud &output, bool searchable = false)
+      {
+        searchable_ = searchable;
+        applyFilter (output);
+
+        voxel_centroids_ = PointCloudPtr (new PointCloud (output));
+
+        if (searchable_ && voxel_centroids_->size() > 0)
+        {
+          // Initiates kdtree of the centroids of voxels containing a sufficient number of points
+          kdtree_.setInputCloud (voxel_centroids_);
+        }
+      }
+
+      /** \brief Initializes voxel structure.
+       * \param[in] searchable flag if voxel structure is searchable, if true then kdtree is built
+       */
+      inline void
+      filter (bool searchable = false)
+      {
+        searchable_ = searchable;
+        voxel_centroids_ = PointCloudPtr (new PointCloud);
+        applyFilter (*voxel_centroids_);
+
+        if (searchable_ && voxel_centroids_->size() > 0)
+        {
+          // Initiates kdtree of the centroids of voxels containing a sufficient number of points
+          kdtree_.setInputCloud (voxel_centroids_);
+        }
+      }
+
+      /** \brief Get the voxel containing point p.
+       * \param[in] index the index of the leaf structure node
+       * \return const pointer to leaf structure
+       */
+      inline LeafConstPtr
+      getLeaf (int index)
+      {
+        auto leaf_iter = leaves_.find (index);
+        if (leaf_iter != leaves_.end ())
+        {
+          LeafConstPtr ret (&(leaf_iter->second));
+          return ret;
+        }
+        else
+          return NULL;
+      }
+
+      /** \brief Get the voxel containing point p.
+       * \param[in] p the point to get the leaf structure at
+       * \return const pointer to leaf structure
+       */
+      inline LeafConstPtr
+      getLeaf (PointT &p)
+      {
+        // Generate index associated with p
+        int ijk0 = static_cast<int> (floor (p.x * inverse_leaf_size_[0]) - min_b_[0]);
+        int ijk1 = static_cast<int> (floor (p.y * inverse_leaf_size_[1]) - min_b_[1]);
+        int ijk2 = static_cast<int> (floor (p.z * inverse_leaf_size_[2]) - min_b_[2]);
+
+        // Compute the centroid leaf index
+        int idx = ijk0 * divb_mul_[0] + ijk1 * divb_mul_[1] + ijk2 * divb_mul_[2];
+
+        // Find leaf associated with index
+        auto leaf_iter = leaves_.find (idx);
+        if (leaf_iter != leaves_.end ())
+        {
+          // If such a leaf exists return the pointer to the leaf structure
+          LeafConstPtr ret (&(leaf_iter->second));
+          return ret;
+        }
+        else
+          return NULL;
+      }
+
+      /** \brief Get the voxel containing point p.
+       * \param[in] p the point to get the leaf structure at
+       * \return const pointer to leaf structure
+       */
+      inline LeafConstPtr
+      getLeaf (Eigen::Vector3f &p)
+      {
+        // Generate index associated with p
+        int ijk0 = static_cast<int> (floor (p[0] * inverse_leaf_size_[0]) - min_b_[0]);
+        int ijk1 = static_cast<int> (floor (p[1] * inverse_leaf_size_[1]) - min_b_[1]);
+        int ijk2 = static_cast<int> (floor (p[2] * inverse_leaf_size_[2]) - min_b_[2]);
+
+        // Compute the centroid leaf index
+        int idx = ijk0 * divb_mul_[0] + ijk1 * divb_mul_[1] + ijk2 * divb_mul_[2];
+
+        // Find leaf associated with index
+        auto leaf_iter = leaves_.find (idx);
+        if (leaf_iter != leaves_.end ())
+        {
+          // If such a leaf exists return the pointer to the leaf structure
+          LeafConstPtr ret (&(leaf_iter->second));
+          return ret;
+        }
+        else
+          return NULL;
+
+      }
+
+      /** \brief Get the voxels surrounding point p, not including the voxel containing point p.
+       * \note Only voxels containing a sufficient number of points are used (slower than radius search in practice).
+       * \param[in] reference_point the point to get the leaf structure at
+       * \param[out] neighbors
+       * \return number of neighbors found
+       */
+	  int getNeighborhoodAtPoint(const Eigen::MatrixXi&, const PointT& reference_point, std::vector<LeafConstPtr> &neighbors) const ;
+	  int getNeighborhoodAtPoint(const PointT& reference_point, std::vector<LeafConstPtr> &neighbors) const ;
+	  int getNeighborhoodAtPoint7(const PointT& reference_point, std::vector<LeafConstPtr> &neighbors) const ;
+	  int getNeighborhoodAtPoint1(const PointT& reference_point, std::vector<LeafConstPtr> &neighbors) const ;
+
+      /** \brief Get the leaf structure map
+       * \return a map containing all leaves
+       */
+      inline const Map&
+      getLeaves ()
+      {
+        return leaves_;
+      }
+
+      /** \brief Get a pointcloud containing the voxel centroids
+       * \note Only voxels containing a sufficient number of points are used.
+       * \return a map containing all leaves
+       */
+      inline PointCloudPtr
+      getCentroids ()
+      {
+        return voxel_centroids_;
+      }
+
+
+      /** \brief Get a cloud to visualize each voxels normal distribution.
+       * \param[out] cell_cloud a cloud created by sampling the normal distributions of each voxel
+       */
+      void
+      getDisplayCloud (pcl::PointCloud<pcl::PointXYZ>& cell_cloud);
+
+      /** \brief Search for the k-nearest occupied voxels for the given query point.
+       * \note Only voxels containing a sufficient number of points are used.
+       * \param[in] point the given query point
+       * \param[in] k the number of neighbors to search for
+       * \param[out] k_leaves the resultant leaves of the neighboring points
+       * \param[out] k_sqr_distances the resultant squared distances to the neighboring points
+       * \return number of neighbors found
+       */
+      int
+      nearestKSearch (const PointT &point, int k,
+                      std::vector<LeafConstPtr> &k_leaves, std::vector<float> &k_sqr_distances)
+      {
+        k_leaves.clear ();
+
+        // Check if kdtree has been built
+        if (!searchable_)
+        {
+          PCL_WARN ("%s: Not Searchable", this->getClassName ().c_str ());
+          return 0;
+        }
+
+        // Find k-nearest neighbors in the occupied voxel centroid cloud
+        std::vector<int> k_indices;
+        k = kdtree_.nearestKSearch (point, k, k_indices, k_sqr_distances);
+
+        // Find leaves corresponding to neighbors
+        k_leaves.reserve (k);
+        for (std::vector<int>::iterator iter = k_indices.begin (); iter != k_indices.end (); iter++)
+        {
+          k_leaves.push_back (&leaves_[voxel_centroids_leaf_indices_[*iter]]);
+        }
+        return k;
+      }
+
+      /** \brief Search for the k-nearest occupied voxels for the given query point.
+       * \note Only voxels containing a sufficient number of points are used.
+       * \param[in] cloud the given query point
+       * \param[in] index the index
+       * \param[in] k the number of neighbors to search for
+       * \param[out] k_leaves the resultant leaves of the neighboring points
+       * \param[out] k_sqr_distances the resultant squared distances to the neighboring points
+       * \return number of neighbors found
+       */
+      inline int
+      nearestKSearch (const PointCloud &cloud, int index, int k,
+                      std::vector<LeafConstPtr> &k_leaves, std::vector<float> &k_sqr_distances)
+      {
+        if (index >= static_cast<int> (cloud.points.size ()) || index < 0)
+          return (0);
+        return (nearestKSearch (cloud.points[index], k, k_leaves, k_sqr_distances));
+      }
+
+
+      /** \brief Search for all the nearest occupied voxels of the query point in a given radius.
+       * \note Only voxels containing a sufficient number of points are used.
+       * \param[in] point the given query point
+       * \param[in] radius the radius of the sphere bounding all of p_q's neighbors
+       * \param[out] k_leaves the resultant leaves of the neighboring points
+       * \param[out] k_sqr_distances the resultant squared distances to the neighboring points
+       * \param[in] max_nn
+       * \return number of neighbors found
+       */
+      int
+      radiusSearch (const PointT &point, double radius, std::vector<LeafConstPtr> &k_leaves,
+                    std::vector<float> &k_sqr_distances, unsigned int max_nn = 0) const
+      {
+        k_leaves.clear ();
+
+        // Check if kdtree has been built
+        if (!searchable_)
+        {
+          PCL_WARN ("%s: Not Searchable", this->getClassName ().c_str ());
+          return 0;
+        }
+
+        // Find neighbors within radius in the occupied voxel centroid cloud
+        std::vector<int> k_indices;
+        int k = kdtree_.radiusSearch (point, radius, k_indices, k_sqr_distances, max_nn);
+
+        // Find leaves corresponding to neighbors
+        k_leaves.reserve (k);
+        for (std::vector<int>::iterator iter = k_indices.begin (); iter != k_indices.end (); iter++)
+        {
+		  auto leaf = leaves_.find(voxel_centroids_leaf_indices_[*iter]);
+		  if (leaf == leaves_.end()) {
+			  std::cerr << "error : could not find the leaf corresponding to the voxel" << std::endl;
+			  std::cin.ignore(1);
+		  }
+          k_leaves.push_back (&(leaf->second));
+        }
+        return k;
+      }
+
+      /** \brief Search for all the nearest occupied voxels of the query point in a given radius.
+       * \note Only voxels containing a sufficient number of points are used.
+       * \param[in] cloud the given query point
+       * \param[in] index a valid index in cloud representing a valid (i.e., finite) query point
+       * \param[in] radius the radius of the sphere bounding all of p_q's neighbors
+       * \param[out] k_leaves the resultant leaves of the neighboring points
+       * \param[out] k_sqr_distances the resultant squared distances to the neighboring points
+       * \param[in] max_nn
+       * \return number of neighbors found
+       */
+      inline int
+      radiusSearch (const PointCloud &cloud, int index, double radius,
+                    std::vector<LeafConstPtr> &k_leaves, std::vector<float> &k_sqr_distances,
+                    unsigned int max_nn = 0) const
+      {
+        if (index >= static_cast<int> (cloud.points.size ()) || index < 0)
+          return (0);
+        return (radiusSearch (cloud.points[index], radius, k_leaves, k_sqr_distances, max_nn));
+      }
+
+    protected:
+
+      /** \brief Filter cloud and initializes voxel structure.
+       * \param[out] output cloud containing centroids of voxels containing a sufficient number of points
+       */
+      void applyFilter (PointCloud &output);
+
+      /** \brief Flag to determine if voxel structure is searchable. */
+      bool searchable_;
+
+      /** \brief Minimum points contained with in a voxel to allow it to be usable. */
+      int min_points_per_voxel_;
+
+      /** \brief Minimum allowable ratio between eigenvalues to prevent singular covariance matrices. */
+      double min_covar_eigvalue_mult_;
+
+      /** \brief Voxel structure containing all leaf nodes (includes voxels with less than a sufficient number of points). */
+	  Map leaves_;
+
+      /** \brief Point cloud containing centroids of voxels containing at least minimum number of points. */
+      PointCloudPtr voxel_centroids_;
+
+      /** \brief Indices of leaf structures associated with each point in \ref voxel_centroids_ (used for searching). */
+      std::vector<int> voxel_centroids_leaf_indices_;
+
+      /** \brief KdTree generated using \ref voxel_centroids_ (used for searching). */
+      pcl::KdTreeFLANN<PointT> kdtree_;
+  };
+}
+
+#endif  //#ifndef PCL_VOXEL_GRID_COVARIANCE_H_

src/detection/detection_ndt_pclomp/pclomp/voxel_grid_covariance_omp.cpp

@@ -0,0 +1,5 @@
+#include <pclomp/voxel_grid_covariance_omp.h>
+#include <voxel_grid_covariance_omp_impl.hpp>
+
+template class pclomp::VoxelGridCovariance<pcl::PointXYZ>;
+template class pclomp::VoxelGridCovariance<pcl::PointXYZI>;

src/detection/detection_ndt_pclomp/pclomp/voxel_grid_covariance_omp_impl.hpp

@@ -0,0 +1,487 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Point Cloud Library (PCL) - www.pointclouds.org
+ *  Copyright (c) 2010-2011, Willow Garage, Inc.
+ *
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the copyright holder(s) nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef PCL_VOXEL_GRID_COVARIANCE_IMPL_OMP_H_
+#define PCL_VOXEL_GRID_COVARIANCE_IMPL_OMP_H_
+
+#include <pcl/common/common.h>
+#include <pcl/filters/boost.h>
+#include <pclomp/voxel_grid_covariance_omp.h>
+#include <Eigen/Dense>
+#include <Eigen/Cholesky>
+
+//////////////////////////////////////////////////////////////////////////////////////////
+template<typename PointT> void
+pclomp::VoxelGridCovariance<PointT>::applyFilter (PointCloud &output)
+{
+  voxel_centroids_leaf_indices_.clear ();
+
+  // Has the input dataset been set already?
+  if (!input_)
+  {
+    PCL_WARN ("[pcl::%s::applyFilter] No input dataset given!\n", getClassName ().c_str ());
+    output.width = output.height = 0;
+    output.points.clear ();
+    return;
+  }
+
+  // Copy the header (and thus the frame_id) + allocate enough space for points
+  output.height = 1;                          // downsampling breaks the organized structure
+  output.is_dense = true;                     // we filter out invalid points
+  output.points.clear ();
+
+  Eigen::Vector4f min_p, max_p;
+  // Get the minimum and maximum dimensions
+  if (!filter_field_name_.empty ()) // If we don't want to process the entire cloud...
+      pcl::getMinMax3D<PointT> (input_, filter_field_name_, static_cast<float> (filter_limit_min_), static_cast<float> (filter_limit_max_), min_p, max_p, filter_limit_negative_);
+  else
+	  pcl::getMinMax3D<PointT> (*input_, min_p, max_p);
+
+  // Check that the leaf size is not too small, given the size of the data
+  int64_t dx = static_cast<int64_t>((max_p[0] - min_p[0]) * inverse_leaf_size_[0])+1;
+  int64_t dy = static_cast<int64_t>((max_p[1] - min_p[1]) * inverse_leaf_size_[1])+1;
+  int64_t dz = static_cast<int64_t>((max_p[2] - min_p[2]) * inverse_leaf_size_[2])+1;
+
+  if((dx*dy*dz) > std::numeric_limits<int32_t>::max())
+  {
+    PCL_WARN("[pcl::%s::applyFilter] Leaf size is too small for the input dataset. Integer indices would overflow.", getClassName().c_str());
+    output.clear();
+    return;
+  }
+
+  // Compute the minimum and maximum bounding box values
+  min_b_[0] = static_cast<int> (floor (min_p[0] * inverse_leaf_size_[0]));
+  max_b_[0] = static_cast<int> (floor (max_p[0] * inverse_leaf_size_[0]));
+  min_b_[1] = static_cast<int> (floor (min_p[1] * inverse_leaf_size_[1]));
+  max_b_[1] = static_cast<int> (floor (max_p[1] * inverse_leaf_size_[1]));
+  min_b_[2] = static_cast<int> (floor (min_p[2] * inverse_leaf_size_[2]));
+  max_b_[2] = static_cast<int> (floor (max_p[2] * inverse_leaf_size_[2]));
+
+  // Compute the number of divisions needed along all axis
+  div_b_ = max_b_ - min_b_ + Eigen::Vector4i::Ones ();
+  div_b_[3] = 0;
+
+  // Clear the leaves
+  leaves_.clear ();
+//  leaves_.reserve(8192);
+
+  // Set up the division multiplier
+  divb_mul_ = Eigen::Vector4i (1, div_b_[0], div_b_[0] * div_b_[1], 0);
+
+  int centroid_size = 4;
+
+  if (downsample_all_data_)
+    centroid_size = boost::mpl::size<FieldList>::value;
+
+  // ---[ RGB special case
+  std::vector<pcl::PCLPointField> fields;
+  int rgba_index = -1;
+  rgba_index = pcl::getFieldIndex<PointT> ("rgb", fields);
+  if (rgba_index == -1)
+    rgba_index = pcl::getFieldIndex<PointT> ("rgba", fields);
+  if (rgba_index >= 0)
+  {
+    rgba_index = fields[rgba_index].offset;
+    centroid_size += 3;
+  }
+
+  // If we don't want to process the entire cloud, but rather filter points far away from the viewpoint first...
+  if (!filter_field_name_.empty ())
+  {
+    // Get the distance field index
+    std::vector<pcl::PCLPointField> fields;
+    int distance_idx = pcl::getFieldIndex<PointT> (filter_field_name_, fields);
+    if (distance_idx == -1)
+      PCL_WARN ("[pcl::%s::applyFilter] Invalid filter field name. Index is %d.\n", getClassName ().c_str (), distance_idx);
+
+    // First pass: go over all points and insert them into the right leaf
+    for (size_t cp = 0; cp < input_->points.size (); ++cp)
+    {
+      if (!input_->is_dense)
+        // Check if the point is invalid
+        if (!std::isfinite (input_->points[cp].x) ||
+            !std::isfinite (input_->points[cp].y) ||
+            !std::isfinite (input_->points[cp].z))
+          continue;
+
+      // Get the distance value
+      const uint8_t* pt_data = reinterpret_cast<const uint8_t*> (&input_->points[cp]);
+      float distance_value = 0;
+      memcpy (&distance_value, pt_data + fields[distance_idx].offset, sizeof (float));
+
+      if (filter_limit_negative_)
+      {
+        // Use a threshold for cutting out points which inside the interval
+        if ((distance_value < filter_limit_max_) && (distance_value > filter_limit_min_))
+          continue;
+      }
+      else
+      {
+        // Use a threshold for cutting out points which are too close/far away
+        if ((distance_value > filter_limit_max_) || (distance_value < filter_limit_min_))
+          continue;
+      }
+
+      int ijk0 = static_cast<int> (floor (input_->points[cp].x * inverse_leaf_size_[0]) - static_cast<float> (min_b_[0]));
+      int ijk1 = static_cast<int> (floor (input_->points[cp].y * inverse_leaf_size_[1]) - static_cast<float> (min_b_[1]));
+      int ijk2 = static_cast<int> (floor (input_->points[cp].z * inverse_leaf_size_[2]) - static_cast<float> (min_b_[2]));
+
+      // Compute the centroid leaf index
+      int idx = ijk0 * divb_mul_[0] + ijk1 * divb_mul_[1] + ijk2 * divb_mul_[2];
+
+      Leaf& leaf = leaves_[idx];
+      if (leaf.nr_points == 0)
+      {
+        leaf.centroid.resize (centroid_size);
+        leaf.centroid.setZero ();
+      }
+
+      Eigen::Vector3d pt3d (input_->points[cp].x, input_->points[cp].y, input_->points[cp].z);
+      // Accumulate point sum for centroid calculation
+      leaf.mean_ += pt3d;
+      // Accumulate x*xT for single pass covariance calculation
+      leaf.cov_ += pt3d * pt3d.transpose ();
+
+      // Do we need to process all the fields?
+      if (!downsample_all_data_)
+      {
+        Eigen::Vector4f pt (input_->points[cp].x, input_->points[cp].y, input_->points[cp].z, 0);
+        leaf.centroid.template head<4> () += pt;
+      }
+      else
+      {
+        // Copy all the fields
+        Eigen::VectorXf centroid = Eigen::VectorXf::Zero (centroid_size);
+        // ---[ RGB special case
+        if (rgba_index >= 0)
+        {
+          // fill r/g/b data
+          int rgb;
+          memcpy (&rgb, reinterpret_cast<const char*> (&input_->points[cp]) + rgba_index, sizeof (int));
+          centroid[centroid_size - 3] = static_cast<float> ((rgb >> 16) & 0x0000ff);
+          centroid[centroid_size - 2] = static_cast<float> ((rgb >> 8) & 0x0000ff);
+          centroid[centroid_size - 1] = static_cast<float> ((rgb) & 0x0000ff);
+        }
+        pcl::for_each_type<FieldList> (pcl::NdCopyPointEigenFunctor<PointT> (input_->points[cp], centroid));
+        leaf.centroid += centroid;
+      }
+      ++leaf.nr_points;
+    }
+  }
+  // No distance filtering, process all data
+  else
+  {
+    // First pass: go over all points and insert them into the right leaf
+    for (size_t cp = 0; cp < input_->points.size (); ++cp)
+    {
+      if (!input_->is_dense)
+        // Check if the point is invalid
+        if (!std::isfinite (input_->points[cp].x) ||
+            !std::isfinite (input_->points[cp].y) ||
+            !std::isfinite (input_->points[cp].z))
+          continue;
+
+      int ijk0 = static_cast<int> (floor (input_->points[cp].x * inverse_leaf_size_[0]) - static_cast<float> (min_b_[0]));
+      int ijk1 = static_cast<int> (floor (input_->points[cp].y * inverse_leaf_size_[1]) - static_cast<float> (min_b_[1]));
+      int ijk2 = static_cast<int> (floor (input_->points[cp].z * inverse_leaf_size_[2]) - static_cast<float> (min_b_[2]));
+
+      // Compute the centroid leaf index
+      int idx = ijk0 * divb_mul_[0] + ijk1 * divb_mul_[1] + ijk2 * divb_mul_[2];
+
+      //int idx = (((input_->points[cp].getArray4fmap () * inverse_leaf_size_).template cast<int> ()).matrix () - min_b_).dot (divb_mul_);
+      Leaf& leaf = leaves_[idx];
+      if (leaf.nr_points == 0)
+      {
+        leaf.centroid.resize (centroid_size);
+        leaf.centroid.setZero ();
+      }
+
+      Eigen::Vector3d pt3d (input_->points[cp].x, input_->points[cp].y, input_->points[cp].z);
+      // Accumulate point sum for centroid calculation
+      leaf.mean_ += pt3d;
+      // Accumulate x*xT for single pass covariance calculation
+      leaf.cov_ += pt3d * pt3d.transpose ();
+
+      // Do we need to process all the fields?
+      if (!downsample_all_data_)
+      {
+        Eigen::Vector4f pt (input_->points[cp].x, input_->points[cp].y, input_->points[cp].z, 0);
+        leaf.centroid.template head<4> () += pt;
+      }
+      else
+      {
+        // Copy all the fields
+        Eigen::VectorXf centroid = Eigen::VectorXf::Zero (centroid_size);
+        // ---[ RGB special case
+        if (rgba_index >= 0)
+        {
+          // Fill r/g/b data, assuming that the order is BGRA
+          int rgb;
+          memcpy (&rgb, reinterpret_cast<const char*> (&input_->points[cp]) + rgba_index, sizeof (int));
+          centroid[centroid_size - 3] = static_cast<float> ((rgb >> 16) & 0x0000ff);
+          centroid[centroid_size - 2] = static_cast<float> ((rgb >> 8) & 0x0000ff);
+          centroid[centroid_size - 1] = static_cast<float> ((rgb) & 0x0000ff);
+        }
+        pcl::for_each_type<FieldList> (pcl::NdCopyPointEigenFunctor<PointT> (input_->points[cp], centroid));
+        leaf.centroid += centroid;
+      }
+      ++leaf.nr_points;
+    }
+  }
+
+  // Second pass: go over all leaves and compute centroids and covariance matrices
+  output.points.reserve (leaves_.size ());
+  if (searchable_)
+    voxel_centroids_leaf_indices_.reserve (leaves_.size ());
+  int cp = 0;
+  if (save_leaf_layout_)
+    leaf_layout_.resize (div_b_[0] * div_b_[1] * div_b_[2], -1);
+
+  // Eigen values and vectors calculated to prevent near singular matrices
+  Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigensolver;
+  Eigen::Matrix3d eigen_val;
+  Eigen::Vector3d pt_sum;
+
+  // Eigen values less than a threshold of max eigen value are inflated to a set fraction of the max eigen value.
+  double min_covar_eigvalue;
+
+  for (auto it = leaves_.begin (); it != leaves_.end (); ++it)
+  {
+
+    // Normalize the centroid
+    Leaf& leaf = it->second;
+
+    // Normalize the centroid
+    leaf.centroid /= static_cast<float> (leaf.nr_points);
+    // Point sum used for single pass covariance calculation
+    pt_sum = leaf.mean_;
+    // Normalize mean
+    leaf.mean_ /= leaf.nr_points;
+
+    // If the voxel contains sufficient points, its covariance is calculated and is added to the voxel centroids and output clouds.
+    // Points with less than the minimum points will have a can not be accurately approximated using a normal distribution.
+    if (leaf.nr_points >= min_points_per_voxel_)
+    {
+      if (save_leaf_layout_)
+        leaf_layout_[it->first] = cp++;
+
+      output.push_back (PointT ());
+
+      // Do we need to process all the fields?
+      if (!downsample_all_data_)
+      {
+        output.points.back ().x = leaf.centroid[0];
+        output.points.back ().y = leaf.centroid[1];
+        output.points.back ().z = leaf.centroid[2];
+      }
+      else
+      {
+        pcl::for_each_type<FieldList> (pcl::NdCopyEigenPointFunctor<PointT> (leaf.centroid, output.back ()));
+        // ---[ RGB special case
+        if (rgba_index >= 0)
+        {
+          // pack r/g/b into rgb
+          float r = leaf.centroid[centroid_size - 3], g = leaf.centroid[centroid_size - 2], b = leaf.centroid[centroid_size - 1];
+          int rgb = (static_cast<int> (r)) << 16 | (static_cast<int> (g)) << 8 | (static_cast<int> (b));
+          memcpy (reinterpret_cast<char*> (&output.points.back ()) + rgba_index, &rgb, sizeof (float));
+        }
+      }
+
+      // Stores the voxel indices for fast access searching
+      if (searchable_)
+        voxel_centroids_leaf_indices_.push_back (static_cast<int> (it->first));
+
+      // Single pass covariance calculation
+      leaf.cov_ = (leaf.cov_ - 2 * (pt_sum * leaf.mean_.transpose ())) / leaf.nr_points + leaf.mean_ * leaf.mean_.transpose ();
+      leaf.cov_ *= (leaf.nr_points - 1.0) / leaf.nr_points;
+
+      //Normalize Eigen Val such that max no more than 100x min.
+      eigensolver.compute (leaf.cov_);
+      eigen_val = eigensolver.eigenvalues ().asDiagonal ();
+      leaf.evecs_ = eigensolver.eigenvectors ();
+
+      if (eigen_val (0, 0) < 0 || eigen_val (1, 1) < 0 || eigen_val (2, 2) <= 0)
+      {
+        leaf.nr_points = -1;
+        continue;
+      }
+
+      // Avoids matrices near singularities (eq 6.11)[Magnusson 2009]
+
+      min_covar_eigvalue = min_covar_eigvalue_mult_ * eigen_val (2, 2);
+      if (eigen_val (0, 0) < min_covar_eigvalue)
+      {
+        eigen_val (0, 0) = min_covar_eigvalue;
+
+        if (eigen_val (1, 1) < min_covar_eigvalue)
+        {
+          eigen_val (1, 1) = min_covar_eigvalue;
+        }
+
+        leaf.cov_ = leaf.evecs_ * eigen_val * leaf.evecs_.inverse ();
+      }
+      leaf.evals_ = eigen_val.diagonal ();
+
+      leaf.icov_ = leaf.cov_.inverse ();
+      if (leaf.icov_.maxCoeff () == std::numeric_limits<float>::infinity ( )
+          || leaf.icov_.minCoeff () == -std::numeric_limits<float>::infinity ( ) )
+      {
+        leaf.nr_points = -1;
+      }
+
+    }
+  }
+
+  output.width = static_cast<uint32_t> (output.points.size ());
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////
+template<typename PointT> int
+pclomp::VoxelGridCovariance<PointT>::getNeighborhoodAtPoint(const Eigen::MatrixXi& relative_coordinates, const PointT& reference_point, std::vector<LeafConstPtr> &neighbors) const
+{
+	neighbors.clear();
+
+	// Find displacement coordinates
+	Eigen::Vector4i ijk(static_cast<int> (floor(reference_point.x / leaf_size_[0])),
+		static_cast<int> (floor(reference_point.y / leaf_size_[1])),
+		static_cast<int> (floor(reference_point.z / leaf_size_[2])), 0);
+	Eigen::Array4i diff2min = min_b_ - ijk;
+	Eigen::Array4i diff2max = max_b_ - ijk;
+	neighbors.reserve(relative_coordinates.cols());
+
+	// Check each neighbor to see if it is occupied and contains sufficient points
+	// Slower than radius search because needs to check 26 indices
+	for (int ni = 0; ni < relative_coordinates.cols(); ni++)
+	{
+		Eigen::Vector4i displacement = (Eigen::Vector4i() << relative_coordinates.col(ni), 0).finished();
+		// Checking if the specified cell is in the grid
+		if ((diff2min <= displacement.array()).all() && (diff2max >= displacement.array()).all())
+		{
+			auto leaf_iter = leaves_.find(((ijk + displacement - min_b_).dot(divb_mul_)));
+			if (leaf_iter != leaves_.end() && leaf_iter->second.nr_points >= min_points_per_voxel_)
+			{
+				LeafConstPtr leaf = &(leaf_iter->second);
+				neighbors.push_back(leaf);
+			}
+		}
+	}
+
+	return (static_cast<int> (neighbors.size()));
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////
+template<typename PointT> int
+pclomp::VoxelGridCovariance<PointT>::getNeighborhoodAtPoint(const PointT& reference_point, std::vector<LeafConstPtr> &neighbors) const
+{
+	neighbors.clear();
+
+	// Find displacement coordinates
+	Eigen::MatrixXi relative_coordinates = pcl::getAllNeighborCellIndices();
+	return getNeighborhoodAtPoint(relative_coordinates, reference_point, neighbors);
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////
+template<typename PointT> int
+pclomp::VoxelGridCovariance<PointT>::getNeighborhoodAtPoint7(const PointT& reference_point, std::vector<LeafConstPtr> &neighbors) const
+{
+	neighbors.clear();
+
+	Eigen::MatrixXi relative_coordinates(3, 7);
+	relative_coordinates.setZero();
+	relative_coordinates(0, 1) = 1;
+	relative_coordinates(0, 2) = -1;
+	relative_coordinates(1, 3) = 1;
+	relative_coordinates(1, 4) = -1;
+	relative_coordinates(2, 5) = 1;
+	relative_coordinates(2, 6) = -1;
+
+	return getNeighborhoodAtPoint(relative_coordinates, reference_point, neighbors);
+}
+
+
+//////////////////////////////////////////////////////////////////////////////////////////
+template<typename PointT> int
+pclomp::VoxelGridCovariance<PointT>::getNeighborhoodAtPoint1(const PointT& reference_point, std::vector<LeafConstPtr> &neighbors) const
+{
+	neighbors.clear();
+	return getNeighborhoodAtPoint(Eigen::MatrixXi::Zero(3,1), reference_point, neighbors);
+}
+
+
+
+//////////////////////////////////////////////////////////////////////////////////////////
+template<typename PointT> void
+pclomp::VoxelGridCovariance<PointT>::getDisplayCloud (pcl::PointCloud<pcl::PointXYZ>& cell_cloud)
+{
+  cell_cloud.clear ();
+
+  int pnt_per_cell = 1000;
+  boost::mt19937 rng;
+  boost::normal_distribution<> nd (0.0, leaf_size_.head (3).norm ());
+  boost::variate_generator<boost::mt19937&, boost::normal_distribution<> > var_nor (rng, nd);
+
+  Eigen::LLT<Eigen::Matrix3d> llt_of_cov;
+  Eigen::Matrix3d cholesky_decomp;
+  Eigen::Vector3d cell_mean;
+  Eigen::Vector3d rand_point;
+  Eigen::Vector3d dist_point;
+
+  // Generate points for each occupied voxel with sufficient points.
+  for (auto it = leaves_.begin (); it != leaves_.end (); ++it)
+  {
+    Leaf& leaf = it->second;
+
+    if (leaf.nr_points >= min_points_per_voxel_)
+    {
+      cell_mean = leaf.mean_;
+      llt_of_cov.compute (leaf.cov_);
+      cholesky_decomp = llt_of_cov.matrixL ();
+
+      // Random points generated by sampling the normal distribution given by voxel mean and covariance matrix
+      for (int i = 0; i < pnt_per_cell; i++)
+      {
+        rand_point = Eigen::Vector3d (var_nor (), var_nor (), var_nor ());
+        dist_point = cell_mean + cholesky_decomp * rand_point;
+        cell_cloud.push_back (pcl::PointXYZ (static_cast<float> (dist_point (0)), static_cast<float> (dist_point (1)), static_cast<float> (dist_point (2))));
+      }
+    }
+  }
+}
+
+#define PCL_INSTANTIATE_VoxelGridCovariance(T) template class PCL_EXPORTS pcl::VoxelGridCovariance<T>;
+
+#endif    // PCL_VOXEL_GRID_COVARIANCE_IMPL_H_

src/detection/laneATT_trt/main.cpp

@@ -17,8 +17,8 @@
 #include "lanearray.pb.h"
 #include "imageBuffer.h"
 
-#define INPUT_H 360
-#define INPUT_W 640
+#define INPUT_H 720
+#define INPUT_W 1280
 #define N_OFFSETS 72
 #define N_STRIPS (N_OFFSETS - 1)
 #define MAX_COL_BLOCKS 1000

src/detection/show_sign_detection/imageBuffer.h

@@ -0,0 +1,82 @@
+#ifndef IMAGEBUFFER_H
+#define IMAGEBUFFER_H
+
+#include <opencv2/opencv.hpp>
+#include <mutex>
+#include <condition_variable>
+#include <queue>
+template<typename T>
+class ConsumerProducerQueue
+{
+
+public:
+    ConsumerProducerQueue(int mxsz,bool dropFrame) :
+            maxSize(mxsz),dropFrame(dropFrame)
+    { }
+
+    bool add(T request)
+    {
+        std::unique_lock<std::mutex> lock(mutex);
+        if(dropFrame && isFull())
+        {
+            //lock.unlock();
+            //return false;
+            cpq.pop();
+            cpq.push(request);
+            cond.notify_all();
+            return true;
+        }
+        else {
+            cond.wait(lock, [this]() { return !isFull(); });
+            cpq.push(request);
+            //lock.unlock();
+            cond.notify_all();
+            return true;
+        }
+    }
+    void consume(T &request)
+    {
+        std::unique_lock<std::mutex> lock(mutex);
+        cond.wait(lock, [this]()
+        { return !isEmpty(); });
+        request = cpq.front();
+        cpq.pop();
+        //lock.unlock();
+        cond.notify_all();
+
+    }
+
+    bool isFull() const
+    {
+        return cpq.size() >= maxSize;
+    }
+
+    bool isEmpty() const
+    {
+        return cpq.size() == 0;
+    }
+
+    int length() const
+    {
+        return cpq.size();
+    }
+
+    void clear()
+    {
+        std::unique_lock<std::mutex> lock(mutex);
+        while (!isEmpty())
+        {
+            cpq.pop();
+        }
+        lock.unlock();
+        cond.notify_all();
+    }
+
+private:
+    std::condition_variable cond;  //条件变量允许通过通知进而实现线程同步
+    std::mutex mutex;     //提供了多种互斥操作，可以显式避免数据竞争
+    std::queue<T> cpq;    //容器适配器，它给予程序员队列的功能
+    int maxSize;
+    bool dropFrame;
+};
+#endif // IMAGEBUFFER_H

src/detection/show_sign_detection/main.cpp

@@ -0,0 +1,116 @@
+#include <iostream>
+#include <opencv2/opencv.hpp>
+#include "opencv2/imgcodecs/legacy/constants_c.h"
+#include <thread>
+#include <vector>
+#include "modulecomm.h"
+#include "cameraobject.pb.h"
+#include "cameraobjectarray.pb.h"
+#include "rawpic.pb.h"
+#include "imageBuffer.h"
+#include "ivlog.h"
+#include "ivfault.h"
+
+bool ivlog_flag =true;
+
+iv::Ivfault *gfault = nullptr;
+iv::Ivlog *givlog = nullptr;
+
+ConsumerProducerQueue<cv::Mat> * imageBuffer_sign =
+        new ConsumerProducerQueue<cv::Mat>(5,true);
+
+void ListenSignImage(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
+{
+    if(nSize<1000)return;
+    iv::vision::rawpic pic;
+    if(false == pic.ParseFromArray(strdata,nSize))
+    {
+        std::cout<<"picview Listenpic fail."<<std::endl;
+        return;
+    }
+    cv::Mat mat(pic.height(),pic.width(),pic.mattype());
+    if(pic.type() == 1)
+        memcpy(mat.data,pic.picdata().data(),mat.rows*mat.cols*mat.elemSize());
+    else
+    {
+        //     mat.release();
+        std::vector<unsigned char> buff(pic.picdata().data(),pic.picdata().data() + pic.picdata().size());
+        mat = cv::imdecode(buff,CV_LOAD_IMAGE_COLOR);
+    }
+    imageBuffer_sign->add(mat);
+    mat.release();
+}
+
+
+void ListenSignArray(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
+{
+    //if(nSize<10)return;
+    iv::vision::cameraobjectarray light_array;
+    if(false == light_array.ParseFromArray(strdata,nSize))
+    {
+        std::cout<<"listen signarray data fail."<<std::endl;
+        return;
+    }
+    for (int i=0;i<light_array.obj_size();i++) {
+        std::cout<<" index: "<<light_array.obj(i).id()
+                 <<" type: "<<light_array.obj(i).type()
+                 <<" confidence: "<<light_array.obj(i).con()
+                 <<" x: "<<light_array.obj(i).x()
+                 <<" y: "<<light_array.obj(i).y()
+                 <<" w: "<<light_array.obj(i).w()
+                 <<" h: "<<light_array.obj(i).h()
+                 <<std::endl;
+    }
+}
+
+
+int main(int argc, char *argv[])
+{
+    gfault = new iv::Ivfault("show_sign_detection");
+    givlog = new iv::Ivlog("show_sign_detection");
+    gfault->SetFaultState(0,0,"show sign detection initialize.");
+
+
+    //void * mpa_signarray;
+    //mpa_signarray= iv::modulecomm::RegisterRecv("signarray",ListenSignArray);
+
+    void * mpa_camera;
+    mpa_camera= iv::modulecomm::RegisterRecv("signimage",ListenSignImage);
+    double waittime = (double)cv::getTickCount();
+    while(1)
+    {
+        cv::Mat sign_image;
+        if(imageBuffer_sign->isEmpty())
+        {
+            double waittotal = (double)cv::getTickCount() - waittime;
+            double totaltime = waittotal/cv::getTickFrequency();
+            //                    if(totaltime>10.0)
+            //                    {
+            //                        cout<<"Cant't get frame and quit"<<endl;
+            //                        lightstart = false;
+            //                        cv::destroyAllWindows();
+            //                        std::cout<<"------end program------"<<std::endl;
+            //                        break;
+            //                    }
+
+            bool print_flag = (totaltime > 1.0);
+            if (print_flag)
+                std::cout<<"Wait for frame "<<totaltime<<"s"<<std::endl;
+
+            if(ivlog_flag & print_flag)
+                givlog->verbose("Wait for frame %f s",totaltime);
+
+            std::this_thread::sleep_for(std::chrono::milliseconds(100));
+            continue;
+        }
+        imageBuffer_sign->consume(sign_image);
+        cv::namedWindow("signResult",cv::WINDOW_NORMAL);
+        cv::imshow("signResult",sign_image);
+        if(cv::waitKey(10)=='q')
+            return 0;
+        //std::this_thread::sleep_for(std::chrono::milliseconds(100));
+        //std::cout<<"show sign result"<<std::endl;
+        waittime = (double)cv::getTickCount();
+    }
+    return 0;
+}

src/detection/show_sign_detection/show_signdetect.pro

@@ -0,0 +1,46 @@
+QT -= gui
+
+CONFIG += c++11 console
+CONFIG -= app_bundle
+
+# The following define makes your compiler emit warnings if you use
+# any Qt feature that has been marked deprecated (the exact warnings
+# depend on your compiler). Please consult the documentation of the
+# deprecated API in order to know how to port your code away from it.
+DEFINES += QT_DEPRECATED_WARNINGS
+
+# You can also make your code fail to compile if it uses deprecated APIs.
+# In order to do so, uncomment the following line.
+# You can also select to disable deprecated APIs only up to a certain version of Qt.
+#DEFINES += QT_DISABLE_DEPRECATED_BEFORE=0x060000    # disables all the APIs deprecated before Qt 6.0.0
+
+# Default rules for deployment.
+qnx: target.path = /tmp/$${TARGET}/bin
+else: unix:!android: target.path = /opt/$${TARGET}/bin
+!isEmpty(target.path): INSTALLS += target
+
+
+SOURCES += main.cpp \
+           $$PWD/../../include/msgtype/rawpic.pb.cc \
+           $$PWD/../../include/msgtype/cameraobjectarray.pb.cc \
+           $$PWD/../../include/msgtype/cameraobject.pb.cc
+
+HEADERS += \
+    imageBuffer.h
+
+LIBS += -L"/usr/local/lib" \
+        -lprotobuf
+# c++
+LIBS += -L/usr/lib/aarch64-linux-gnu -lstdc++fs
+LIBS += -L/usr/lib/aarch64-linux-gnu/ -lglog
+
+!include(../../../include/common.pri ) {
+    error( "Couldn't find the common.pri file!" )
+}
+
+!include(../../../include/ivprotobuf.pri ) {
+    error( "Couldn't find the ivprotobuf.pri file!" )
+}
+!include(../../../include/ivopencv.pri ) {
+    error( "Couldn't find the ivopencv.pri file!" )
+}

src/detection/sign_detection/README.md

@@ -0,0 +1,114 @@
+## B站教学视频
+
+https://www.bilibili.com/video/BV1Pa4y1N7HS
+
+## Introduction
+
+- 基于**Tensorrt**加速**Yolov8**，本项目采用**ONNX转Tensorrt**方案
+- 支持**Windows10**和**Linux**
+- 支持**Python/C++**
+
+## YOLOv8
+
+<div align="center">
+<img src="assets/1.png" width=800>
+</div>
+
+## Environment
+
+- **Tensorrt 8.4.3.**
+- **Cuda 11.6 Cudnn 8.4.1**
+- **onnx 1.12.0**
+
+## Quick Start
+
+安装**yolov8**仓库，并下载官方模型。
+
+```
+pip install ultralytics==8.0.5
+pip install onnx==1.12.0
+# download offical weights(".pt" file)
+https://github.com/ultralytics/assets/releases/download/v0.0.0/yolov8n.pt
+```
+
+使用官方命令**导出ONNX模型**。
+
+```
+yolo mode=export model=yolov8n.pt format=onnx dynamic=False
+```
+
+使用本仓库**v8_transform.py转换官方的ONNX模型**，会自动生成yolov8n.transd.onnx。
+
+```
+python v8_transform.py yolov8n.onnx
+```
+
+将生成的**onnx**模型复制到**tensorrt/bin**文件夹下，使用官方**trtexec**转化onnx模型。**FP32预测删除`--fp16`参数即可**。
+
+```
+trtexec --onnx=yolov8n.transd.onnx --saveEngine=yolov8n_fp16.trt --fp16
+```
+
+## C++
+
+配置**Opencv**、**Tensorrt**环境，具体可参考https://github.com/Monday-Leo/Yolov5_Tensorrt_Win10
+
+打开本仓库的**CMakeLists.txt**，修改**Opencv**、**Tensorrt**路径，之后cmake。
+
+```
+#change to your own path
+##################################################
+set(OpenCV_DIR "E:/opencv/build")  
+set(TRT_DIR "E:/TensorRT-8.4.3.1")  
+##################################################
+```
+
+<div align="center">
+<img src="assets/2.png" width=600>
+</div>
+
+将**预测图片zidane.jpg和模型yolov8n_fp16.trt放入exe文件夹**，直接运行程序，**没有做warmup预测，首次预测时间不准**，想要精确计时请自行修改代码做warmup。**想要修改模型路径和图片路径请修改主程序。**
+
+```
+int main() {
+	std::string img_path = "zidane.jpg";
+	std::string model_path = "yolov8n_fp16.trt";
+	single_inference(img_path,model_path);
+	return 0;
+}
+```
+
+<div align="center">
+<img src="assets/3.jpg" width=600>
+</div>
+
+## Python
+
+在刚才的C++工程中右键yolov8，点击属性，修改为**动态链接库**。
+
+<div align="center">
+<img src="assets/4.png" width=600>
+</div>
+
+将本仓库的**python_trt.py**复制到dll文件夹下。
+
+<div align="center">
+<img src="assets/5.png" width=600>
+</div>
+
+设置模型路径，**dll**路径和想要预测的图片路径，特别注意模型**路径需要加b''**
+
+```
+det = Detector(model_path=b"./yolov8n_fp16.trt",dll_path="./yolov8.dll")  # b'' is needed
+img = cv2.imread("./zidane.jpg")
+```
+
+<div align="center">
+<img src="assets/6.png" width=600>
+</div>
+
+## Reference
+
+https://github.com/ultralytics/ultralytics
+
+https://github.com/shouxieai/infer

src/detection/sign_detection/include/Hungarian.h

@@ -0,0 +1,36 @@
+//
+// Created by lqx on 20-4-23.
+//
+
+#ifndef TRACK_SORT_HUNGARIAN_H_H
+#define TRACK_SORT_HUNGARIAN_H_H
+
+#include <iostream>
+#include <vector>
+
+using namespace std;
+
+class HungarianAlgorithm
+{
+public:
+    HungarianAlgorithm();
+    ~HungarianAlgorithm();
+    double Solve(vector<vector<double>>& DistMatrix, vector<int>& Assignment);
+
+private:
+    void assignmentoptimal(int *assignment, double *cost, double *distMatrix, int nOfRows, int nOfColumns);
+    void buildassignmentvector(int *assignment, bool *starMatrix, int nOfRows, int nOfColumns);
+    void computeassignmentcost(int *assignment, double *cost, double *distMatrix, int nOfRows);
+    void step2a(int *assignment, double *distMatrix, bool *starMatrix, bool *newStarMatrix, bool *primeMatrix,
+                bool *coveredColumns, bool *coveredRows, int nOfRows, int nOfColumns, int minDim);
+    void step2b(int *assignment, double *distMatrix, bool *starMatrix, bool *newStarMatrix, bool *primeMatrix,
+                bool *coveredColumns, bool *coveredRows, int nOfRows, int nOfColumns, int minDim);
+    void step3(int *assignment, double *distMatrix, bool *starMatrix, bool *newStarMatrix, bool *primeMatrix,
+               bool *coveredColumns, bool *coveredRows, int nOfRows, int nOfColumns, int minDim);
+    void step4(int *assignment, double *distMatrix, bool *starMatrix, bool *newStarMatrix, bool *primeMatrix,
+               bool *coveredColumns, bool *coveredRows, int nOfRows, int nOfColumns, int minDim, int row, int col);
+    void step5(int *assignment, double *distMatrix, bool *starMatrix, bool *newStarMatrix, bool *primeMatrix,
+               bool *coveredColumns, bool *coveredRows, int nOfRows, int nOfColumns, int minDim);
+};
+
+#endif //TRACK_SORT_HUNGARIAN_H_H

src/detection/sign_detection/include/KalmanTracker.h

@@ -0,0 +1,90 @@
+//
+// Created by lqx on 20-4-23.
+//
+
+#ifndef TRACK_SORT_KALMANTRACKER_H
+#define TRACK_SORT_KALMANTRACKER_H
+
+///////////////////////////////////////////////////////////////////////////////
+// KalmanTracker.h: KalmanTracker Class Declaration
+
+#include "opencv2/video/tracking.hpp"
+#include "opencv2/highgui/highgui.hpp"
+
+using namespace std;
+using namespace cv;
+
+#define StateType Rect_<float>
+
+
+// This class represents the internel state of individual tracked objects observed as bounding box.
+class KalmanTracker
+{
+public:
+    KalmanTracker()
+    {
+        init_kf(StateType());
+        m_time_since_update = 0;
+        m_hits = 0;
+        m_hit_streak = 0;
+        m_age = 0;
+        m_id = kf_count;
+        //kf_count++;
+    }
+    KalmanTracker(StateType initRect)
+    {
+        init_kf(initRect);
+        m_time_since_update = 0;
+        m_hits = 0;
+        m_hit_streak = 0;
+        m_age = 0;
+        m_id = kf_count;
+        //kf_count++;
+    }
+
+    KalmanTracker(StateType initRect, int classId,float prob)
+    {
+        init_kf(initRect);
+        m_time_since_update = 0;
+        m_hits = 0;
+        m_hit_streak = 0;
+        m_age = 0;
+        m_id = kf_count;
+        //kf_count++;
+        m_class_id = classId;
+        m_prob = prob;
+    }
+
+    ~KalmanTracker()
+    {
+        m_history.clear();
+        m_class_history.clear();
+    }
+
+    StateType predict();
+    void update(StateType stateMat,int classId, float prob);
+
+    StateType get_state();
+    StateType get_rect_xysr(float cx, float cy, float s, float r);
+
+    static int kf_count;
+
+    int m_time_since_update;
+    int m_hits;
+    int m_hit_streak;
+    int m_age;
+    int m_id;
+    int m_class_id;
+    std::vector<int> m_class_history;
+    float m_prob;
+
+private:
+    void init_kf(StateType stateMat);
+
+    cv::KalmanFilter kf;
+    cv::Mat measurement;
+
+    std::vector<StateType> m_history;
+};
+
+#endif //TRACK_SORT_KALMANTRACKER_H

src/detection/sign_detection/include/cpm.hpp

@@ -0,0 +1,153 @@
+#ifndef __CPM_HPP__
+#define __CPM_HPP__
+
+// Comsumer Producer Model
+
+#include <algorithm>
+#include <condition_variable>
+#include <future>
+#include <memory>
+#include <queue>
+#include <thread>
+
+namespace cpm {
+
+template <typename Result, typename Input, typename Model>
+class Instance {
+ protected:
+  struct Item {
+    Input input;
+    std::shared_ptr<std::promise<Result>> pro;
+  };
+
+  std::condition_variable cond_;
+  std::queue<Item> input_queue_;
+  std::mutex queue_lock_;
+  std::shared_ptr<std::thread> worker_;
+  volatile bool run_ = false;
+  volatile int max_items_processed_ = 0;
+  void *stream_ = nullptr;
+
+ public:
+  virtual ~Instance() { stop(); }
+
+  void stop() {
+    run_ = false;
+    cond_.notify_one();
+    {
+      std::unique_lock<std::mutex> l(queue_lock_);
+      while (!input_queue_.empty()) {
+        auto &item = input_queue_.front();
+        if (item.pro) item.pro->set_value(Result());
+        input_queue_.pop();
+      }
+    };
+
+    if (worker_) {
+      worker_->join();
+      worker_.reset();
+    }
+  }
+
+  virtual std::shared_future<Result> commit(const Input &input) {
+    Item item;
+    item.input = input;
+    item.pro.reset(new std::promise<Result>());
+    {
+      std::unique_lock<std::mutex> __lock_(queue_lock_);
+      input_queue_.push(item);
+    }
+    cond_.notify_one();
+    return item.pro->get_future();
+  }
+
+  virtual std::vector<std::shared_future<Result>> commits(const std::vector<Input> &inputs) {
+    std::vector<std::shared_future<Result>> output;
+    {
+      std::unique_lock<std::mutex> __lock_(queue_lock_);
+      for (int i = 0; i < (int)inputs.size(); ++i) {
+        Item item;
+        item.input = inputs[i];
+        item.pro.reset(new std::promise<Result>());
+        output.emplace_back(item.pro->get_future());
+        input_queue_.push(item);
+      }
+    }
+    cond_.notify_one();
+    return output;
+  }
+
+  template <typename LoadMethod>
+  bool start(const LoadMethod &loadmethod, int max_items_processed = 1, void *stream = nullptr) {
+    stop();
+
+    this->stream_ = stream;
+    this->max_items_processed_ = max_items_processed;
+    std::promise<bool> status;
+    worker_ = std::make_shared<std::thread>(&Instance::worker<LoadMethod>, this,
+                                            std::ref(loadmethod), std::ref(status));
+    return status.get_future().get();
+  }
+
+ private:
+  template <typename LoadMethod>
+  void worker(const LoadMethod &loadmethod, std::promise<bool> &status) {
+    std::shared_ptr<Model> model = loadmethod();
+    if (model == nullptr) {
+      status.set_value(false);
+      return;
+    }
+
+    run_ = true;
+    status.set_value(true);
+
+    std::vector<Item> fetch_items;
+    std::vector<Input> inputs;
+    while (get_items_and_wait(fetch_items, max_items_processed_)) {
+      inputs.resize(fetch_items.size());
+      std::transform(fetch_items.begin(), fetch_items.end(), inputs.begin(),
+                     [](Item &item) { return item.input; });
+
+      auto ret = model->forwards(inputs, stream_);
+      for (int i = 0; i < (int)fetch_items.size(); ++i) {
+        if (i < (int)ret.size()) {
+          fetch_items[i].pro->set_value(ret[i]);
+        } else {
+          fetch_items[i].pro->set_value(Result());
+        }
+      }
+      inputs.clear();
+      fetch_items.clear();
+    }
+    model.reset();
+    run_ = false;
+  }
+
+  virtual bool get_items_and_wait(std::vector<Item> &fetch_items, int max_size) {
+    std::unique_lock<std::mutex> l(queue_lock_);
+    cond_.wait(l, [&]() { return !run_ || !input_queue_.empty(); });
+
+    if (!run_) return false;
+
+    fetch_items.clear();
+    for (int i = 0; i < max_size && !input_queue_.empty(); ++i) {
+      fetch_items.emplace_back(std::move(input_queue_.front()));
+      input_queue_.pop();
+    }
+    return true;
+  }
+
+  virtual bool get_item_and_wait(Item &fetch_item) {
+    std::unique_lock<std::mutex> l(queue_lock_);
+    cond_.wait(l, [&]() { return !run_ || !input_queue_.empty(); });
+
+    if (!run_) return false;
+
+    fetch_item = std::move(input_queue_.front());
+    input_queue_.pop();
+    return true;
+  }
+};
+};  // namespace cpm
+
+#endif  // __CPM_HPP__