reset

src/decition/decition_brain_sf/decition/adc_tools/compute_00.cpp

@@ -8,11 +8,8 @@
 #include <iostream>
 #include <fstream>
 #include <control/can.h>
-#include <sstream>
-#include <cstdlib>
-#include <string>
-#include <vector>
 using namespace std;
+
 #define PI (3.1415926535897932384626433832795)
 
 iv::decition::Compute00::Compute00() {
@@ -191,156 +188,8 @@ int iv::decition::Compute00::getDesiredSpeed(std::vector<GPSData> gpsMap)
     return 1;
 }
 
-int iv::decition::Compute00::getMapDelta(std::vector<GPSData> MapTrace){
-    vector<vector<double>> doubledata;//大地坐标系下x,y,phi,delta
-    double distance,deltaphi;
-    //x,y,phi in rad
-    for (int i = 0; i < MapTrace.size(); i++) {   //                                       1225/14:25
-                 std::vector<double> temp;
-                 temp.push_back(0);temp.push_back(0);temp.push_back(0);temp.push_back(0);//先push四个空量，然后就可以给量赋值了
-                 doubledata.push_back(temp);
-                 doubledata[i][0] = MapTrace.at(i)->gps_x;
-                 doubledata[i][1] = MapTrace.at(i)->gps_y;
-                 doubledata[i][2] = (MapTrace.at(i)->ins_heading_angle) / 180 * PI;
-    }
-    // compute delta///////////////////////////////////////////////////////////////
-    for (int i = 0; i < doubledata.size()-1; i++)
-    {
-                deltaphi=doubledata[i+1][2]-doubledata[i][2];
-                if (deltaphi>PI)
-                        {deltaphi=deltaphi-2*PI; }
-                if (deltaphi<-PI)
-                        {deltaphi=deltaphi+2*PI;}
-                distance=sqrt( pow((doubledata[i+1][0]-doubledata[i][0]),2)+pow((doubledata[i+1][1]-doubledata[i][1]),2));
-                doubledata[i][3]=-atan( 4.0* (deltaphi) / distance  );//车辆坐标系和惯导坐标系方向相反，delta变号
-
-                MapTrace[i]->delta=doubledata[i][3];
-    }
-                MapTrace[MapTrace.size()-1]->delta = MapTrace[MapTrace.size()-2]->delta;
-
-    //delta filter
-    for(int j=10;j<MapTrace.size()-10;j++)
-    {
-        double delta_sum=0;
-        for(int k=j-10;k<j+10;k++)
-        {
-            delta_sum+= MapTrace[k]->delta;
-        }
-        MapTrace[j]->delta=delta_sum/20;
-    }
-}
-
 
-int iv::decition::Compute00::getPlanSpeed(std::vector<GPSData> MapTrace)
-{
-                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);
-                double Curve_SmallSpeed=min(ServiceCarStatus.mroadmode_vel.mfmode18,15.0);
-                double Curve_LargeSpeed=min(ServiceCarStatus.mroadmode_vel.mfmode14,8.0);
-                getMapDelta(MapTrace);
-                for(int i=0;i<MapTrace.size();i++)
-                {
-                    if((MapTrace[i]->delta>-0.4)&&(MapTrace[i]->delta<0.4)){
-                        MapTrace[i]->roadMode=0;
-                    }else if(((MapTrace[i]->delta>0.4)&&(MapTrace[i]->delta<4))||((MapTrace[i]->delta>-4)&&(MapTrace[i]->delta<-0.4))){
-                        MapTrace[i]->roadMode=5;
-                    }else if(((MapTrace[i]->delta>4)&&(MapTrace[i]->delta<10))||((MapTrace[i]->delta>-10)&&(MapTrace[i]->delta<-4))){
-                        MapTrace[i]->roadMode=18;
-                    }else if(((MapTrace[i]->delta>10))||((MapTrace[i]->delta<-10))){
-                        MapTrace[i]->roadMode=14;
-                    }
-                }
-                for(int i=0;i<MapTrace.size();i++)
-                {
-                    if(MapTrace[i]->roadMode==0){
-                        MapTrace[i]->plan_speed=straightSpeed;
-                        mode0to5count++;
-                        //mode0to18count++;
-                        mode18count=0;
-                        mode0to5flash=mode0to5count;
-                    }else if(MapTrace[i]->roadMode==5){
-                        MapTrace[i]->plan_speed=straightCurveSpeed;
-                        mode0to5count++;
-                        //mode0to18count++;
-                        mode18count=0;
-                        mode0to5flash=mode0to5count;
-                    }else if(MapTrace[i]->roadMode==18){
-                        mode0to5countSum=mode0to5count;
-                        MapTrace[i]->plan_speed=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 j=i;j>i-brake_distance;j--){
-                                MapTrace[j]->plan_speed=min((Curve_SmallSpeed+step_count*step_speed),straightCurveSpeed);
-                                step_count++;
-                            }
-                        }else if(mode0to5countSum>0){
-                            for(int j=i;j>i-mode0to5countSum;j--){
-                                MapTrace[j]->plan_speed=Curve_SmallSpeed;
-                                step_count++;
-                            }
-                        }else{
-                            MapTrace[i]->plan_speed=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;
-                        MapTrace[i]->plan_speed=Curve_LargeSpeed;
-
-                        if(mode18countSum>brake_distanceLarge)
-                        {
-                                double step_speed=(Curve_SmallSpeed-Curve_LargeSpeed)/brake_distanceLarge;
-
-                                for(int j=i;j>i-brake_distanceLarge;j--){
-                                    MapTrace[j]->plan_speed=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--){
-                                    MapTrace[j]->plan_speed=min((Curve_LargeSpeed+step_count*step_speed),straightCurveSpeed);
-                                    step_count++;
-                                }
-                        }else{
-                                MapTrace[i]->plan_speed=Curve_LargeSpeed;
-                        }
-
-                        mode0to5count=0;
-                        mode18count=0;
-                        mode0to5flash=0;
-                        mode18flash=0;
-                    }
-                }
-
-                for(int i=0;i<MapTrace.size();i++){
-                    //将数据写入文件开始
-                    ofstream outfile;
-                    outfile.open("ctr0108003.txt", ostream::app);        /*以添加模式打开文件*/
-                    outfile<<"Delta"<< ","  <<MapTrace[i]->delta<< ","<<"roadMode"<< ","  <<MapTrace[i]->roadMode<< ","
-                           <<"plan_speed"<< ","  << MapTrace[i]->plan_speed<< ","<<endl;
-                    outfile.close();                                 /*关闭文件*/
-                    //将数据写入文件结束
-                }
-
-}
 
 
 //首次找点

src/decition/decition_brain_sf/decition/adc_tools/compute_00.h

@@ -35,8 +35,6 @@ namespace iv {
             static	int getFirstNearestPointIndex(GPS_INS rp, std::vector<GPSData> gpsMap, int lastIndex, double mindis, double maxAngle);
 
             static int getDesiredSpeed(std::vector<GPSData> gpsMap);
-            static int getPlanSpeed(std::vector<GPSData> MapTrace);
-            static int getMapDelta(std::vector<GPSData> MapTrace);
 
             static double getDecideAngle(std::vector<Point2D>  gpsTrace, double realSpeed);
             static double getAveDef(std::vector<Point2D> farTrace);

src/decition/decition_brain_sf/decition/decide_gps_00.cpp

@@ -241,8 +241,6 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
         if(ServiceCarStatus.speed_control==true){
             Compute00().getDesiredSpeed(gpsMapLine);   //add by zj
         }
-        //Compute00().getPlanSpeed(gpsMapLine);
-
 
         //	ServiceCarStatus.carState = 1;
         //        roadOri = gpsMapLine[PathPoint]->roadOri;

src1/common/modulecomm/.gitignore

@@ -1,73 +0,0 @@
-# This file is used to ignore files which are generated
-# ----------------------------------------------------------------------------
-
-*~
-*.autosave
-*.a
-*.core
-*.moc
-*.o
-*.obj
-*.orig
-*.rej
-*.so
-*.so.*
-*_pch.h.cpp
-*_resource.rc
-*.qm
-.#*
-*.*#
-core
-!core/
-tags
-.DS_Store
-.directory
-*.debug
-Makefile*
-*.prl
-*.app
-moc_*.cpp
-ui_*.h
-qrc_*.cpp
-Thumbs.db
-*.res
-*.rc
-/.qmake.cache
-/.qmake.stash
-
-# qtcreator generated files
-*.pro.user*
-
-# xemacs temporary files
-*.flc
-
-# Vim temporary files
-.*.swp
-
-# Visual Studio generated files
-*.ib_pdb_index
-*.idb
-*.ilk
-*.pdb
-*.sln
-*.suo
-*.vcproj
-*vcproj.*.*.user
-*.ncb
-*.sdf
-*.opensdf
-*.vcxproj
-*vcxproj.*
-
-# MinGW generated files
-*.Debug
-*.Release
-
-# Python byte code
-*.pyc
-
-# Binaries
-# --------
-*.dll
-*.exe
-

src1/common/modulecomm/modulecomm.cpp

@@ -1,152 +0,0 @@
-#include "modulecomm.h"
-#include <iostream>
-
-namespace iv {
-namespace modulecomm {
-
-struct ModeduleInfo
-{
-    void * mphandle;
-    ModuleComm_TYPE mmctype;
-};
-
-void *  RegisterSend(const char * strcommname,const unsigned int nBufSize,const unsigned int nMsgBufCount,
-                     ModuleComm_TYPE xmctype)
-{
-
-    iv::modulecomm::ModeduleInfo * pmi = new iv::modulecomm::ModeduleInfo;
-    pmi->mmctype = xmctype;
-    pmi->mphandle = 0;
-    switch (xmctype) {
-    case ModuleComm_SHAREMEM:
-        pmi->mphandle = iv::modulecomm_shm::RegisterSend(strcommname,nBufSize,nMsgBufCount);
-        break;
-    case ModuleComm_FASTRTPS:
-        pmi->mphandle = iv::modulecomm_fastrtps::RegisterSend(strcommname,nBufSize,nMsgBufCount);
-        break;
-    case ModuleComm_INTERIOR:
-        pmi->mphandle = iv::modulecomm_inter::RegisterSend(strcommname,nBufSize,nMsgBufCount);
-        break;
-    default:
-        break;
-    }
-    return pmi;
-}
-
-void  *  RegisterRecv(const char * strcommname,SMCallBack pCall,ModuleComm_TYPE xmctype)
-{
-    iv::modulecomm::ModeduleInfo * pmi = new iv::modulecomm::ModeduleInfo;
-    pmi->mmctype = xmctype;
-    pmi->mphandle = 0;
-    switch (xmctype) {
-    case ModuleComm_SHAREMEM:
-        pmi->mphandle = iv::modulecomm_shm::RegisterRecv(strcommname,pCall);
-        break;
-    case ModuleComm_FASTRTPS:
-        pmi->mphandle = iv::modulecomm_fastrtps::RegisterRecv(strcommname,pCall);
-        break;
-    case ModuleComm_INTERIOR:
-        pmi->mphandle = iv::modulecomm_inter::RegisterRecv(strcommname,pCall);
-        break;
-    default:
-        break;
-    }
-    return pmi;
-}
-
-void *  RegisterRecvPlus(const char * strcommname,ModuleFun xFun,
-                                                ModuleComm_TYPE xmctype)
-{
-    iv::modulecomm::ModeduleInfo * pmi = new iv::modulecomm::ModeduleInfo;
-    pmi->mmctype = xmctype;
-    pmi->mphandle = 0;
-    switch (xmctype) {
-    case ModuleComm_SHAREMEM:
-        pmi->mphandle = iv::modulecomm_shm::RegisterRecvPlus(strcommname,xFun);
-        break;
-    case ModuleComm_FASTRTPS:
-        pmi->mphandle = iv::modulecomm_fastrtps::RegisterRecvPlus(strcommname,xFun);
-        break;
-    case ModuleComm_INTERIOR:
-        pmi->mphandle = iv::modulecomm_inter::RegisterRecvPlus(strcommname,xFun);
-        break;
-    default:
-        break;
-    }
-    return pmi;
-}
-
-void  ModuleSendMsg(void * pHandle,const char * strdata,const unsigned int nDataLen)
-{
-    iv::modulecomm::ModeduleInfo * pmi = (iv::modulecomm::ModeduleInfo  * )pHandle;
-    switch (pmi->mmctype) {
-    case ModuleComm_SHAREMEM:
-        iv::modulecomm_shm::ModuleSendMsg(pmi->mphandle,strdata,nDataLen);
-        break;
-    case ModuleComm_FASTRTPS:
-        iv::modulecomm_fastrtps::ModuleSendMsg(pmi->mphandle,strdata,nDataLen);
-        break;
-    case ModuleComm_INTERIOR:
-        iv::modulecomm_inter::ModuleSendMsg(pmi->mphandle,strdata,nDataLen);
-        break;
-    default:
-        break;
-    }
-}
-
-void  Unregister(void * pHandle)
-{
-    iv::modulecomm::ModeduleInfo * pmi = (iv::modulecomm::ModeduleInfo  * )pHandle;
-    switch (pmi->mmctype) {
-    case ModuleComm_SHAREMEM:
-        iv::modulecomm_shm::Unregister(pmi->mphandle);
-        break;
-    case ModuleComm_FASTRTPS:
-        iv::modulecomm_fastrtps::Unregister(pmi->mphandle);
-        break;
-    case ModuleComm_INTERIOR:
-        iv::modulecomm_inter::Unregister(pmi->mphandle);
-        break;
-    default:
-        break;
-    }
-}
-
-void PauseComm(void *pHandle)
-{
-    iv::modulecomm::ModeduleInfo * pmi = (iv::modulecomm::ModeduleInfo  * )pHandle;
-    switch (pmi->mmctype) {
-    case ModuleComm_SHAREMEM:
-        iv::modulecomm_shm::PauseComm(pmi->mphandle);
-        break;
-    case ModuleComm_FASTRTPS:
-        iv::modulecomm_fastrtps::PauseComm(pmi->mphandle);
-        break;
-    case ModuleComm_INTERIOR:
-        iv::modulecomm_inter::PauseComm(pmi->mphandle);
-        break;
-    default:
-        break;
-    }
-}
-
-void ContintuComm(void *pHandle)
-{
-    iv::modulecomm::ModeduleInfo * pmi = (iv::modulecomm::ModeduleInfo  * )pHandle;
-    switch (pmi->mmctype) {
-    case ModuleComm_SHAREMEM:
-        iv::modulecomm_shm::ContintuComm(pmi->mphandle);
-        break;
-    case ModuleComm_FASTRTPS:
-        iv::modulecomm_fastrtps::ContintuComm(pmi->mphandle);
-        break;
-    case ModuleComm_INTERIOR:
-        iv::modulecomm_inter::ContintuComm(pmi->mphandle);
-        break;
-    default:
-        break;
-    }
-}
-
-}
-}

src1/common/modulecomm/modulecomm.h

@@ -1,64 +0,0 @@
-#ifndef MODULECOMM_H
-#define MODULECOMM_H
-
-
-#include <QtCore/qglobal.h>
-#include <QDateTime>
-
-#include <functional>
-
-
-#include "modulecomm_shm.h"
-#include "modulecomm_fastrtps.h"
-#include "modulecomm_inter.h"
-
-#if defined(MODULECOMM_LIBRARY)
-#  define MODULECOMMSHARED_EXPORT Q_DECL_EXPORT
-#else
-#  define MODULECOMMSHARED_EXPORT Q_DECL_IMPORT
-#endif
-
-
-
-
-
-
-//#include <iostream>
-//#include <thread>
-
-//using namespace std::placeholders;
-
-#ifndef IV_MODULE_FUN
-
-typedef std::function<void(const char * ,const unsigned int , const unsigned int , QDateTime * ,const char *)> ModuleFun;
-typedef void (* SMCallBack)(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname);
-#define IV_MODULE_FUN
-#endif
-
-namespace iv {
-namespace modulecomm {
-
-enum ModuleComm_TYPE
-{
-    ModuleComm_SHAREMEM = 0,
-    ModuleComm_INTERIOR = 1,
-    ModuleComm_FASTRTPS = 2
-};
-
-
-void * MODULECOMMSHARED_EXPORT RegisterSend(const char * strcommname,const unsigned int nBufSize,const unsigned int nMsgBufCount
-                                            ,ModuleComm_TYPE xmctype = ModuleComm_SHAREMEM);
-void * MODULECOMMSHARED_EXPORT RegisterRecv(const char * strcommname,SMCallBack pCall,
-                                            ModuleComm_TYPE xmctype = ModuleComm_SHAREMEM);
-void * MODULECOMMSHARED_EXPORT RegisterRecvPlus(const char * strcommname,ModuleFun xFun,
-                                                ModuleComm_TYPE xmctype = ModuleComm_SHAREMEM);
-void MODULECOMMSHARED_EXPORT ModuleSendMsg(void * pHandle,const char * strdata,const unsigned int nDataLen);
-void MODULECOMMSHARED_EXPORT Unregister(void * pHandle);
-void MODULECOMMSHARED_EXPORT PauseComm(void * pHandle);
-void MODULECOMMSHARED_EXPORT ContintuComm(void * pHandle);
-
-}
-
-}
-
-#endif // MODULECOMM_H

src1/common/modulecomm/modulecomm.pro

@@ -1,36 +0,0 @@
-QT -= gui
-
-TEMPLATE = lib
-DEFINES += MODULECOMM_LIBRARY
-
-CONFIG += c++11
-
-# 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
-
-SOURCES += \
-    modulecomm.cpp
-
-HEADERS += \
-    modulecomm.h
-
-CONFIG += plugin
-
-# Default rules for deployment.
-unix {
-    target.path = /usr/lib
-}
-!isEmpty(target.path): INSTALLS += target
-
-
-INCLUDEPATH += $$PWD/../modulecomm_shm
-INCLUDEPATH += $$PWD/../modulecomm_fastrtps
-INCLUDEPATH += $$PWD/../modulecomm_inter

src1/common/modulecomm_fastrtps/Topics.cxx

@@ -1,323 +0,0 @@
-// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima).
-//
-// 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.
-
-/*!
- * @file Topics.cpp
- * This source file contains the definition of the described types in the IDL file.
- *
- * This file was generated by the tool gen.
- */
-
-#ifdef _WIN32
-// Remove linker warning LNK4221 on Visual Studio
-namespace { char dummy; }
-#endif
-
-#include "Topics.h"
-#include <fastcdr/Cdr.h>
-
-#include <fastcdr/exceptions/BadParamException.h>
-using namespace eprosima::fastcdr::exception;
-
-#include <utility>
-
-
-TopicSample::Message::Message()
-{
-    // m_msgname com.eprosima.idl.parser.typecode.StringTypeCode@76329302
-    m_msgname ="";
-    // m_counter com.eprosima.idl.parser.typecode.PrimitiveTypeCode@5e25a92e
-    m_counter = 0;
-    // m_sendtime com.eprosima.idl.parser.typecode.PrimitiveTypeCode@4df828d7
-    m_sendtime = 0;
-    // m_xdata com.eprosima.idl.parser.typecode.AliasTypeCode@b59d31
-
-    m_xdata.push_back(1);
-    m_xdata.push_back(2);
-
-
-}
-
-TopicSample::Message::~Message()
-{
-
-
-
-
-}
-
-TopicSample::Message::Message(const Message &x)
-{
-    m_msgname = x.m_msgname;
-    m_counter = x.m_counter;
-    m_sendtime = x.m_sendtime;
-    m_xdata = x.m_xdata;
-}
-
-TopicSample::Message::Message(Message &&x)
-{
-    m_msgname = std::move(x.m_msgname);
-    m_counter = x.m_counter;
-    m_sendtime = x.m_sendtime;
-    m_xdata = std::move(x.m_xdata);
-}
-
-TopicSample::Message& TopicSample::Message::operator=(const Message &x)
-{
-
-    m_msgname = x.m_msgname;
-    m_counter = x.m_counter;
-    m_sendtime = x.m_sendtime;
-    m_xdata = x.m_xdata;
-
-    return *this;
-}
-
-TopicSample::Message& TopicSample::Message::operator=(Message &&x)
-{
-
-    m_msgname = std::move(x.m_msgname);
-    m_counter = x.m_counter;
-    m_sendtime = x.m_sendtime;
-    m_xdata = std::move(x.m_xdata);
-
-    return *this;
-}
-
-size_t TopicSample::Message::getMaxCdrSerializedSize(size_t current_alignment)
-{
-    size_t initial_alignment = current_alignment;
-
-
-    current_alignment += 4 + eprosima::fastcdr::Cdr::alignment(current_alignment, 4) + 255 + 1;
-
-    current_alignment += 4 + eprosima::fastcdr::Cdr::alignment(current_alignment, 4);
-
-
-    current_alignment += 8 + eprosima::fastcdr::Cdr::alignment(current_alignment, 8);
-
-
-    current_alignment += 4 + eprosima::fastcdr::Cdr::alignment(current_alignment, 4);
-
-    current_alignment += (100 * 1) + eprosima::fastcdr::Cdr::alignment(current_alignment, 1);
-
-
-
-    return 10000000;
-//    return current_alignment - initial_alignment;
-}
-
-size_t TopicSample::Message::getCdrSerializedSize(const TopicSample::Message& data, size_t current_alignment)
-{
-    (void)data;
-    size_t initial_alignment = current_alignment;
-
-
-    current_alignment += 4 + eprosima::fastcdr::Cdr::alignment(current_alignment, 4) + data.msgname().size() + 1;
-
-    current_alignment += 4 + eprosima::fastcdr::Cdr::alignment(current_alignment, 4);
-
-
-    current_alignment += 8 + eprosima::fastcdr::Cdr::alignment(current_alignment, 8);
-
-
-    current_alignment += 4 + eprosima::fastcdr::Cdr::alignment(current_alignment, 4);
-
-    if (data.xdata().size() > 0)
-    {
-        current_alignment += (data.xdata().size() * 1) + eprosima::fastcdr::Cdr::alignment(current_alignment, 1);
-    }
-
-
-
-
-    return current_alignment - initial_alignment;
-}
-
-void TopicSample::Message::serialize(eprosima::fastcdr::Cdr &scdr) const
-{
-
-    scdr << m_msgname;
-    scdr << m_counter;
-    scdr << m_sendtime;
-    scdr << m_xdata;
-
-//    std::cout<<"serialize."<<std::endl;
-}
-
-void TopicSample::Message::deserialize(eprosima::fastcdr::Cdr &dcdr)
-{
-
-    dcdr >> m_msgname;
-    dcdr >> m_counter;
-    dcdr >> m_sendtime;
-    dcdr >> m_xdata;
-}
-
-/*!
- * @brief This function copies the value in member msgname
- * @param _msgname New value to be copied in member msgname
- */
-void TopicSample::Message::msgname(const std::string &_msgname)
-{
-m_msgname = _msgname;
-}
-
-/*!
- * @brief This function moves the value in member msgname
- * @param _msgname New value to be moved in member msgname
- */
-void TopicSample::Message::msgname(std::string &&_msgname)
-{
-m_msgname = std::move(_msgname);
-}
-
-/*!
- * @brief This function returns a constant reference to member msgname
- * @return Constant reference to member msgname
- */
-const std::string& TopicSample::Message::msgname() const
-{
-    return m_msgname;
-}
-
-/*!
- * @brief This function returns a reference to member msgname
- * @return Reference to member msgname
- */
-std::string& TopicSample::Message::msgname()
-{
-    return m_msgname;
-}
-/*!
- * @brief This function sets a value in member counter
- * @param _counter New value for member counter
- */
-void TopicSample::Message::counter(int32_t _counter)
-{
-m_counter = _counter;
-}
-
-/*!
- * @brief This function returns the value of member counter
- * @return Value of member counter
- */
-int32_t TopicSample::Message::counter() const
-{
-    return m_counter;
-}
-
-/*!
- * @brief This function returns a reference to member counter
- * @return Reference to member counter
- */
-int32_t& TopicSample::Message::counter()
-{
-    return m_counter;
-}
-
-/*!
- * @brief This function sets a value in member sendtime
- * @param _sendtime New value for member sendtime
- */
-void TopicSample::Message::sendtime(int64_t _sendtime)
-{
-m_sendtime = _sendtime;
-}
-
-/*!
- * @brief This function returns the value of member sendtime
- * @return Value of member sendtime
- */
-int64_t TopicSample::Message::sendtime() const
-{
-    return m_sendtime;
-}
-
-/*!
- * @brief This function returns a reference to member sendtime
- * @return Reference to member sendtime
- */
-int64_t& TopicSample::Message::sendtime()
-{
-    return m_sendtime;
-}
-
-/*!
- * @brief This function copies the value in member xdata
- * @param _xdata New value to be copied in member xdata
- */
-void TopicSample::Message::xdata(const TopicSample::SomeBytes &_xdata)
-{
- //   return;
-  m_xdata = _xdata;
-//int i;
-//for(i=0;i<300;i++)
-//    m_xdata.push_back(_xdata.at(i));
-}
-
-/*!
- * @brief This function moves the value in member xdata
- * @param _xdata New value to be moved in member xdata
- */
-void TopicSample::Message::xdata(TopicSample::SomeBytes &&_xdata)
-{
-m_xdata = std::move(_xdata);
-}
-
-/*!
- * @brief This function returns a constant reference to member xdata
- * @return Constant reference to member xdata
- */
-const TopicSample::SomeBytes& TopicSample::Message::xdata() const
-{
-    return m_xdata;
-}
-
-/*!
- * @brief This function returns a reference to member xdata
- * @return Reference to member xdata
- */
-TopicSample::SomeBytes& TopicSample::Message::xdata()
-{
-    return m_xdata;
-}
-
-size_t TopicSample::Message::getKeyMaxCdrSerializedSize(size_t current_alignment)
-{
-    size_t current_align = current_alignment;
-
-
-
-
-
-
-
-    return current_align;
-}
-
-bool TopicSample::Message::isKeyDefined()
-{
-   return false;
-}
-
-void TopicSample::Message::serializeKey(eprosima::fastcdr::Cdr &scdr) const
-{
-    (void) scdr;
-     
-     
-     
-     
-}
-

src1/common/modulecomm_fastrtps/Topics.h

@@ -1,253 +0,0 @@
-// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima).
-//
-// 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.
-
-/*!
- * @file Topics.h
- * This header file contains the declaration of the described types in the IDL file.
- *
- * This file was generated by the tool gen.
- */
-
-#ifndef _TOPICSAMPLE_TOPICS_H_
-#define _TOPICSAMPLE_TOPICS_H_
-
-// TODO Poner en el contexto.
-
-#include <stdint.h>
-#include <array>
-#include <string>
-#include <vector>
-#include <map>
-#include <bitset>
-
-#if defined(_WIN32)
-#if defined(EPROSIMA_USER_DLL_EXPORT)
-#define eProsima_user_DllExport __declspec( dllexport )
-#else
-#define eProsima_user_DllExport
-#endif
-#else
-#define eProsima_user_DllExport
-#endif
-
-#if defined(_WIN32)
-#if defined(EPROSIMA_USER_DLL_EXPORT)
-#if defined(Topics_SOURCE)
-#define Topics_DllAPI __declspec( dllexport )
-#else
-#define Topics_DllAPI __declspec( dllimport )
-#endif // Topics_SOURCE
-#else
-#define Topics_DllAPI
-#endif
-#else
-#define Topics_DllAPI
-#endif // _WIN32
-
-namespace eprosima
-{
-    namespace fastcdr
-    {
-        class Cdr;
-    }
-}
-
-
-namespace TopicSample
-{
-    typedef std::vector<uint8_t> SomeBytes;
-    /*!
-     * @brief This class represents the structure Message defined by the user in the IDL file.
-     * @ingroup TOPICS
-     */
-    class Message
-    {
-    public:
-
-        /*!
-         * @brief Default constructor.
-         */
-        eProsima_user_DllExport Message();
-
-        /*!
-         * @brief Default destructor.
-         */
-        eProsima_user_DllExport ~Message();
-
-        /*!
-         * @brief Copy constructor.
-         * @param x Reference to the object TopicSample::Message that will be copied.
-         */
-        eProsima_user_DllExport Message(const Message &x);
-
-        /*!
-         * @brief Move constructor.
-         * @param x Reference to the object TopicSample::Message that will be copied.
-         */
-        eProsima_user_DllExport Message(Message &&x);
-
-        /*!
-         * @brief Copy assignment.
-         * @param x Reference to the object TopicSample::Message that will be copied.
-         */
-        eProsima_user_DllExport Message& operator=(const Message &x);
-
-        /*!
-         * @brief Move assignment.
-         * @param x Reference to the object TopicSample::Message that will be copied.
-         */
-        eProsima_user_DllExport Message& operator=(Message &&x);
-
-        /*!
-         * @brief This function copies the value in member msgname
-         * @param _msgname New value to be copied in member msgname
-         */
-        eProsima_user_DllExport void msgname(const std::string &_msgname);
-
-        /*!
-         * @brief This function moves the value in member msgname
-         * @param _msgname New value to be moved in member msgname
-         */
-        eProsima_user_DllExport void msgname(std::string &&_msgname);
-
-        /*!
-         * @brief This function returns a constant reference to member msgname
-         * @return Constant reference to member msgname
-         */
-        eProsima_user_DllExport const std::string& msgname() const;
-
-        /*!
-         * @brief This function returns a reference to member msgname
-         * @return Reference to member msgname
-         */
-        eProsima_user_DllExport std::string& msgname();
-        /*!
-         * @brief This function sets a value in member counter
-         * @param _counter New value for member counter
-         */
-        eProsima_user_DllExport void counter(int32_t _counter);
-
-        /*!
-         * @brief This function returns the value of member counter
-         * @return Value of member counter
-         */
-        eProsima_user_DllExport int32_t counter() const;
-
-        /*!
-         * @brief This function returns a reference to member counter
-         * @return Reference to member counter
-         */
-        eProsima_user_DllExport int32_t& counter();
-
-        /*!
-         * @brief This function sets a value in member sendtime
-         * @param _sendtime New value for member sendtime
-         */
-        eProsima_user_DllExport void sendtime(int64_t _sendtime);
-
-        /*!
-         * @brief This function returns the value of member sendtime
-         * @return Value of member sendtime
-         */
-        eProsima_user_DllExport int64_t sendtime() const;
-
-        /*!
-         * @brief This function returns a reference to member sendtime
-         * @return Reference to member sendtime
-         */
-        eProsima_user_DllExport int64_t& sendtime();
-
-        /*!
-         * @brief This function copies the value in member xdata
-         * @param _xdata New value to be copied in member xdata
-         */
-        eProsima_user_DllExport void xdata(const TopicSample::SomeBytes &_xdata);
-
-        /*!
-         * @brief This function moves the value in member xdata
-         * @param _xdata New value to be moved in member xdata
-         */
-        eProsima_user_DllExport void xdata(TopicSample::SomeBytes &&_xdata);
-
-        /*!
-         * @brief This function returns a constant reference to member xdata
-         * @return Constant reference to member xdata
-         */
-        eProsima_user_DllExport const TopicSample::SomeBytes& xdata() const;
-
-        /*!
-         * @brief This function returns a reference to member xdata
-         * @return Reference to member xdata
-         */
-        eProsima_user_DllExport TopicSample::SomeBytes& xdata();
-
-        /*!
-         * @brief This function returns the maximum serialized size of an object
-         * depending on the buffer alignment.
-         * @param current_alignment Buffer alignment.
-         * @return Maximum serialized size.
-         */
-        eProsima_user_DllExport static size_t getMaxCdrSerializedSize(size_t current_alignment = 0);
-
-        /*!
-         * @brief This function returns the serialized size of a data depending on the buffer alignment.
-         * @param data Data which is calculated its serialized size.
-         * @param current_alignment Buffer alignment.
-         * @return Serialized size.
-         */
-        eProsima_user_DllExport static size_t getCdrSerializedSize(const TopicSample::Message& data, size_t current_alignment = 0);
-
-
-        /*!
-         * @brief This function serializes an object using CDR serialization.
-         * @param cdr CDR serialization object.
-         */
-        eProsima_user_DllExport void serialize(eprosima::fastcdr::Cdr &cdr) const;
-
-        /*!
-         * @brief This function deserializes an object using CDR serialization.
-         * @param cdr CDR serialization object.
-         */
-        eProsima_user_DllExport void deserialize(eprosima::fastcdr::Cdr &cdr);
-
-
-
-        /*!
-         * @brief This function returns the maximum serialized size of the Key of an object
-         * depending on the buffer alignment.
-         * @param current_alignment Buffer alignment.
-         * @return Maximum serialized size.
-         */
-        eProsima_user_DllExport static size_t getKeyMaxCdrSerializedSize(size_t current_alignment = 0);
-
-        /*!
-         * @brief This function tells you if the Key has been defined for this type
-         */
-        eProsima_user_DllExport static bool isKeyDefined();
-
-        /*!
-         * @brief This function serializes the key members of an object using CDR serialization.
-         * @param cdr CDR serialization object.
-         */
-        eProsima_user_DllExport void serializeKey(eprosima::fastcdr::Cdr &cdr) const;
-
-    private:
-        std::string m_msgname;
-        int32_t m_counter;
-        int64_t m_sendtime;
-        TopicSample::SomeBytes m_xdata;
-    };
-}
-
-#endif // _TOPICSAMPLE_TOPICS_H_

src1/common/modulecomm_fastrtps/Topics.idl

@@ -1,13 +0,0 @@
-module TopicSample {
-
-#pragma DCPS_DATA_TYPE "TopicSample::Message"
-
-typedef sequence<octet> SomeBytes;
-struct Message {
-  string msgname;
-  long counter;
-  long long sendtime;
-  SomeBytes   xdata;
-};
-
-};

src1/common/modulecomm_fastrtps/TopicsPubSubTypes.cxx

@@ -1,138 +0,0 @@
-// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima).
-//
-// 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.
-
-/*!
- * @file TopicsPubSubTypes.cpp
- * This header file contains the implementation of the serialization functions.
- *
- * This file was generated by the tool fastcdrgen.
- */
-
-
-#include <fastcdr/FastBuffer.h>
-#include <fastcdr/Cdr.h>
-
-#include "TopicsPubSubTypes.h"
-
-using namespace eprosima::fastrtps;
-using namespace eprosima::fastrtps::rtps;
-
-namespace TopicSample
-{
-
-    MessagePubSubType::MessagePubSubType()
-    {
-        setName("TopicSample::Message");
-        m_typeSize = static_cast<uint32_t>(Message::getMaxCdrSerializedSize()) + 4 /*encapsulation*/;
-        m_isGetKeyDefined = Message::isKeyDefined();
-        size_t keyLength = Message::getKeyMaxCdrSerializedSize()>16 ? Message::getKeyMaxCdrSerializedSize() : 16;
-        m_keyBuffer = reinterpret_cast<unsigned char*>(malloc(keyLength));
-        memset(m_keyBuffer, 0, keyLength);
-    }
-
-    MessagePubSubType::~MessagePubSubType()
-    {
-        if(m_keyBuffer!=nullptr)
-            free(m_keyBuffer);
-    }
-
-    bool MessagePubSubType::serialize(void *data, SerializedPayload_t *payload)
-    {
-        Message *p_type = static_cast<Message*>(data);
-        eprosima::fastcdr::FastBuffer fastbuffer(reinterpret_cast<char*>(payload->data), payload->max_size); // Object that manages the raw buffer.
-        eprosima::fastcdr::Cdr ser(fastbuffer, eprosima::fastcdr::Cdr::DEFAULT_ENDIAN,
-                eprosima::fastcdr::Cdr::DDS_CDR); // Object that serializes the data.
-        payload->encapsulation = ser.endianness() == eprosima::fastcdr::Cdr::BIG_ENDIANNESS ? CDR_BE : CDR_LE;
-        // Serialize encapsulation
-        ser.serialize_encapsulation();
-
-        try
-        {
-            p_type->serialize(ser); // Serialize the object:
-        }
-        catch(eprosima::fastcdr::exception::NotEnoughMemoryException& /*exception*/)
-        {
-            return false;
-        }
-
-        payload->length = static_cast<uint32_t>(ser.getSerializedDataLength()); //Get the serialized length
-        return true;
-    }
-
-    bool MessagePubSubType::deserialize(SerializedPayload_t* payload, void* data)
-    {
-        Message* p_type = static_cast<Message*>(data); //Convert DATA to pointer of your type
-        eprosima::fastcdr::FastBuffer fastbuffer(reinterpret_cast<char*>(payload->data), payload->length); // Object that manages the raw buffer.
-        eprosima::fastcdr::Cdr deser(fastbuffer, eprosima::fastcdr::Cdr::DEFAULT_ENDIAN,
-                eprosima::fastcdr::Cdr::DDS_CDR); // Object that deserializes the data.
-        // Deserialize encapsulation.
-        deser.read_encapsulation();
-        payload->encapsulation = deser.endianness() == eprosima::fastcdr::Cdr::BIG_ENDIANNESS ? CDR_BE : CDR_LE;
-
-        try
-        {
-            p_type->deserialize(deser); //Deserialize the object:
-        }
-        catch(eprosima::fastcdr::exception::NotEnoughMemoryException& /*exception*/)
-        {
-            return false;
-        }
-
-        return true;
-    }
-
-    std::function<uint32_t()> MessagePubSubType::getSerializedSizeProvider(void* data)
-    {
-        return [data]() -> uint32_t
-        {
-            return static_cast<uint32_t>(type::getCdrSerializedSize(*static_cast<Message*>(data))) + 4 /*encapsulation*/;
-        };
-    }
-
-    void* MessagePubSubType::createData()
-    {
-        return reinterpret_cast<void*>(new Message());
-    }
-
-    void MessagePubSubType::deleteData(void* data)
-    {
-        delete(reinterpret_cast<Message*>(data));
-    }
-
-    bool MessagePubSubType::getKey(void *data, InstanceHandle_t* handle, bool force_md5)
-    {
-        if(!m_isGetKeyDefined)
-            return false;
-        Message* p_type = static_cast<Message*>(data);
-        eprosima::fastcdr::FastBuffer fastbuffer(reinterpret_cast<char*>(m_keyBuffer),Message::getKeyMaxCdrSerializedSize());     // Object that manages the raw buffer.
-        eprosima::fastcdr::Cdr ser(fastbuffer, eprosima::fastcdr::Cdr::BIG_ENDIANNESS);     // Object that serializes the data.
-        p_type->serializeKey(ser);
-        if(force_md5 || Message::getKeyMaxCdrSerializedSize()>16)    {
-            m_md5.init();
-            m_md5.update(m_keyBuffer, static_cast<unsigned int>(ser.getSerializedDataLength()));
-            m_md5.finalize();
-            for(uint8_t i = 0;i<16;++i)        {
-                handle->value[i] = m_md5.digest[i];
-            }
-        }
-        else    {
-            for(uint8_t i = 0;i<16;++i)        {
-                handle->value[i] = m_keyBuffer[i];
-            }
-        }
-        return true;
-    }
-
-
-} //End of namespace TopicSample

src1/common/modulecomm_fastrtps/TopicsPubSubTypes.h

@@ -1,61 +0,0 @@
-// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima).
-//
-// 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.
-
-/*!
- * @file TopicsPubSubTypes.h
- * This header file contains the declaration of the serialization functions.
- *
- * This file was generated by the tool fastcdrgen.
- */
-
-
-#ifndef _TOPICSAMPLE_TOPICS_PUBSUBTYPES_H_
-#define _TOPICSAMPLE_TOPICS_PUBSUBTYPES_H_
-
-#include <fastrtps/config.h>
-#include <fastrtps/TopicDataType.h>
-
-#include "Topics.h"
-
-#if !defined(GEN_API_VER) || (GEN_API_VER != 1)
-#error Generated Topics is not compatible with current installed Fast-RTPS. Please, regenerate it with fastrtpsgen.
-#endif
-
-namespace TopicSample
-{
-    typedef std::vector<uint8_t> SomeBytes;
-    /*!
-     * @brief This class represents the TopicDataType of the type Message defined by the user in the IDL file.
-     * @ingroup TOPICS
-     */
-    class MessagePubSubType : public eprosima::fastrtps::TopicDataType {
-    public:
-        typedef Message type;
-
-        eProsima_user_DllExport MessagePubSubType();
-
-        eProsima_user_DllExport virtual ~MessagePubSubType();
-        eProsima_user_DllExport virtual bool serialize(void *data, eprosima::fastrtps::rtps::SerializedPayload_t *payload) override;
-        eProsima_user_DllExport virtual bool deserialize(eprosima::fastrtps::rtps::SerializedPayload_t *payload, void *data) override;
-        eProsima_user_DllExport virtual std::function<uint32_t()> getSerializedSizeProvider(void* data) override;
-        eProsima_user_DllExport virtual bool getKey(void *data, eprosima::fastrtps::rtps::InstanceHandle_t *ihandle,
-            bool force_md5 = false) override;
-        eProsima_user_DllExport virtual void* createData() override;
-        eProsima_user_DllExport virtual void deleteData(void * data) override;
-        MD5 m_md5;
-        unsigned char* m_keyBuffer;
-    };
-}
-
-#endif // _TOPICSAMPLE_TOPICS_PUBSUBTYPES_H_

src1/common/modulecomm_fastrtps/TopicsPublisher.cxx

@@ -1,198 +0,0 @@
-// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima).
-//
-// 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.
-
-/*!
- * @file TopicsPublisher.cpp
- * This file contains the implementation of the publisher functions.
- *
- * This file was generated by the tool fastcdrgen.
- */
-
-
-#include <fastrtps/participant/Participant.h>
-#include <fastrtps/attributes/ParticipantAttributes.h>
-#include <fastrtps/publisher/Publisher.h>
-#include <fastrtps/attributes/PublisherAttributes.h>
-
-#include <fastrtps/Domain.h>
-
-#include <fastrtps/participant/Participant.h>
-#include <fastrtps/attributes/ParticipantAttributes.h>
-#include <fastrtps/attributes/PublisherAttributes.h>
-#include <fastrtps/publisher/Publisher.h>
-#include <fastrtps/Domain.h>
-#include <fastdds/rtps/transport/shared_mem/SharedMemTransportDescriptor.h>
-#include <fastdds/rtps/transport/UDPv4TransportDescriptor.h>
-
-#include <thread>
-#include <chrono>
-
-#include "TopicsPublisher.h"
-
-using namespace eprosima::fastrtps;
-using namespace eprosima::fastrtps::rtps;
-
-using namespace eprosima::fastdds::rtps;
-
-TopicsPublisher::TopicsPublisher() : mp_participant(nullptr), mp_publisher(nullptr) {}
-
-TopicsPublisher::~TopicsPublisher() {	Domain::removeParticipant(mp_participant);}
-
-bool TopicsPublisher::init(const char * strtopic)
-{
-    strncpy(mstrtopic,strtopic,255);
-    // Create RTPSParticipant
-
-    ParticipantAttributes PParam;
-    PParam.rtps.sendSocketBufferSize = 100000000;
-    PParam.rtps.setName("Participant_publisher");  //You can put here the name you want
-
-    PParam.rtps.builtin.discovery_config.discoveryProtocol = DiscoveryProtocol_t::SIMPLE;
-    PParam.rtps.builtin.discovery_config.use_SIMPLE_EndpointDiscoveryProtocol = true;
-    PParam.rtps.builtin.discovery_config.m_simpleEDP.use_PublicationReaderANDSubscriptionWriter = true;
-    PParam.rtps.builtin.discovery_config.m_simpleEDP.use_PublicationWriterANDSubscriptionReader = true;
-    PParam.rtps.builtin.discovery_config.leaseDuration = c_TimeInfinite;
-
-    // SharedMem transport configuration
-    PParam.rtps.useBuiltinTransports = false;
-
-    auto shm_transport = std::make_shared<SharedMemTransportDescriptor>();
-    shm_transport->segment_size(100*1024*1024);
-    PParam.rtps.userTransports.push_back(shm_transport);
-
-    // UDP
-    auto udp_transport = std::make_shared<UDPv4TransportDescriptor>();
-    //udp_transport->interfaceWhiteList.push_back("127.0.0.1");
-    PParam.rtps.userTransports.push_back(udp_transport);
-
-
-    mp_participant = Domain::createParticipant(PParam);
-    if(mp_participant == nullptr)
-    {
-        return false;
-    }
-
-    //Register the type
-
-    Domain::registerType(mp_participant, static_cast<TopicDataType*>(&myType));
-
-    // Create Publisher
-
-    PublisherAttributes Wparam;
-    Wparam.topic.topicKind = NO_KEY;
-    Wparam.topic.topicDataType = myType.getName();  //This type MUST be registered
-    Wparam.topic.topicName = strtopic;//"TopicsPubSubTopic";
-
-    Wparam.topic.historyQos.depth = 30;
-    Wparam.topic.resourceLimitsQos.max_samples = 50;
-    Wparam.topic.resourceLimitsQos.allocated_samples = 20;
-    Wparam.times.heartbeatPeriod.seconds = 2;
-    Wparam.times.heartbeatPeriod.nanosec = 200 * 1000 * 1000;
-    Wparam.qos.m_reliability.kind = RELIABLE_RELIABILITY_QOS;
-    Wparam.qos.m_publishMode.kind = ASYNCHRONOUS_PUBLISH_MODE;
-
-    mp_publisher = Domain::createPublisher(mp_participant,Wparam,static_cast<PublisherListener*>(&m_listener));
-
-    if(mp_publisher == nullptr)
-    {
-        return false;
-    }
-
-    std::cout << "Publisher created, waiting for Subscribers." << std::endl;
-    return true;
-}
-
-void TopicsPublisher::PubListener::onPublicationMatched(Publisher* pub,MatchingInfo& info)
-{
-    (void)pub;
-
-    if (info.status == MATCHED_MATCHING)
-    {
-        n_matched++;
-        std::cout << "Publisher matched" << std::endl;
-    }
-    else
-    {
-        n_matched--;
-        std::cout << "Publisher unmatched" << std::endl;
-    }
-}
-
-void TopicsPublisher::run()
-{
-    while(m_listener.n_matched == 0)
-    {
-        std::this_thread::sleep_for(std::chrono::milliseconds(250)); // Sleep 250 ms
-    }
-
-    // Publication code
-
-    TopicSample::Message st;
-
-    /* Initialize your structure here */
-
-    int msgsent = 0;
-    char ch = 'y';
-    do
-    {
-        if(ch == 'y')
-        {
-            mp_publisher->write(&st);  ++msgsent;
-            std::cout << "Sending sample, count=" << msgsent << ", send another sample?(y-yes,n-stop): ";
-        }
-        else if(ch == 'n')
-        {
-            std::cout << "Stopping execution " << std::endl;
-            break;
-        }
-        else
-        {
-            std::cout << "Command " << ch << " not recognized, please enter \"y/n\":";
-        }
-    } while(std::cin >> ch);
-}
-
-#include <QDateTime>
-
-void TopicsPublisher::senddata(const char *str, int nsize)
-{
-
-
-    static int ncount = 1;
-    std::cout<<"send data."<<std::endl;
-//    while(m_listener.n_matched == 0)
-    TopicSample::SomeBytes x;
-    x.resize(nsize);
-    memcpy(x.data(),str,nsize);
-
-    TopicSample::Message st;
-
-    st.msgname(mstrtopic);
-//    st.msgname("topictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopictopic");
-    st.counter(ncount);ncount++;
-    st.sendtime(QDateTime::currentMSecsSinceEpoch());
-    TopicSample::SomeBytes & px = x;
-    st.xdata(x);
-
-    int ndatasize = TopicSample::Message::getCdrSerializedSize(st);
-
-    std::cout<<"size is "<<ndatasize<<std::endl;
-
-
-    mp_publisher->write(&st);
-
-
-
-}
-

src1/common/modulecomm_fastrtps/TopicsPublisher.h

@@ -1,56 +0,0 @@
-// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima).
-//
-// 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.
-
-/*!
- * @file TopicsPublisher.h
- * This header file contains the declaration of the publisher functions.
- *
- * This file was generated by the tool fastcdrgen.
- */
-
-
-#ifndef _TOPICSAMPLE_TOPICS_PUBLISHER_H_
-#define _TOPICSAMPLE_TOPICS_PUBLISHER_H_
-
-#include <fastrtps/fastrtps_fwd.h>
-#include <fastrtps/publisher/PublisherListener.h>
-
-#include "TopicsPubSubTypes.h"
-
-class TopicsPublisher
-{
-public:
-	TopicsPublisher();
-	virtual ~TopicsPublisher();
-    bool init(const char * strtopic);
-	void run();
-    void senddata(const char *str, int nsize);
-private:
-	eprosima::fastrtps::Participant *mp_participant;
-	eprosima::fastrtps::Publisher *mp_publisher;
-
-    char mstrtopic[256];
-
-	class PubListener : public eprosima::fastrtps::PublisherListener
-	{
-	public:
-		PubListener() : n_matched(0){};
-		~PubListener(){};
-		void onPublicationMatched(eprosima::fastrtps::Publisher* pub,eprosima::fastrtps::rtps::MatchingInfo& info);
-		int n_matched;
-	} m_listener;
-	TopicSample::MessagePubSubType myType;
-};
-
-#endif // _TOPICSAMPLE_TOPICS_PUBLISHER_H_

src1/common/modulecomm_fastrtps/TopicsSubscriber.cxx

@@ -1,206 +0,0 @@
-// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima).
-//
-// 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.
-
-/*!
- * @file TopicsSubscriber.cpp
- * This file contains the implementation of the subscriber functions.
- *
- * This file was generated by the tool fastcdrgen.
- */
-
-#include <fastrtps/participant/Participant.h>
-#include <fastrtps/attributes/ParticipantAttributes.h>
-#include <fastrtps/subscriber/Subscriber.h>
-#include <fastrtps/attributes/SubscriberAttributes.h>
-
-#include <fastrtps/Domain.h>
-
-#include <fastcdr/FastBuffer.h>
-#include <fastcdr/FastCdr.h>
-#include <fastcdr/Cdr.h>
-
-#include <fastrtps/participant/Participant.h>
-#include <fastrtps/attributes/ParticipantAttributes.h>
-#include <fastrtps/attributes/PublisherAttributes.h>
-#include <fastrtps/publisher/Publisher.h>
-#include <fastrtps/Domain.h>
-#include <fastdds/rtps/transport/shared_mem/SharedMemTransportDescriptor.h>
-#include <fastdds/rtps/transport/UDPv4TransportDescriptor.h>
-
-#include "TopicsSubscriber.h"
-
-using namespace eprosima::fastrtps;
-using namespace eprosima::fastrtps::rtps;
-
-using namespace eprosima::fastdds::rtps;
-
-
-
-
-#include <QDateTime>
-
-TopicsSubscriber::TopicsSubscriber() : mp_participant(nullptr), mp_subscriber(nullptr) {}
-
-TopicsSubscriber::~TopicsSubscriber() {	Domain::removeParticipant(mp_participant);}
-
-bool TopicsSubscriber::init(const char * strtopic)
-{
-
-    strncpy(mstrtopic,strtopic,255);
-    // Create RTPSParticipant
-
-    ParticipantAttributes PParam;
-    PParam.rtps.setName("Participant_subscriber"); //You can put the name you want
-
-    PParam.rtps.builtin.discovery_config.discoveryProtocol = DiscoveryProtocol_t::SIMPLE;
-    PParam.rtps.builtin.discovery_config.use_SIMPLE_EndpointDiscoveryProtocol = true;
-    PParam.rtps.builtin.discovery_config.m_simpleEDP.use_PublicationReaderANDSubscriptionWriter = true;
-    PParam.rtps.builtin.discovery_config.m_simpleEDP.use_PublicationWriterANDSubscriptionReader = true;
-    PParam.rtps.builtin.discovery_config.leaseDuration = c_TimeInfinite;
-
-    // SharedMem transport configuration
-    PParam.rtps.useBuiltinTransports = false;
-
-    auto sm_transport = std::make_shared<SharedMemTransportDescriptor>();
-    sm_transport->segment_size(100*1024*1024);
-    PParam.rtps.userTransports.push_back(sm_transport);
-
-    // UDP
-    auto udp_transport = std::make_shared<UDPv4TransportDescriptor>();
-    //udp_transport->interfaceWhiteList.push_back("127.0.0.1");
-    PParam.rtps.userTransports.push_back(udp_transport);
-
-
-
-    mp_participant = Domain::createParticipant(PParam);
-    if(mp_participant == nullptr)
-    {
-        return false;
-    }
-
-    //Register the type
-
-    Domain::registerType(mp_participant, static_cast<TopicDataType*>(&myType));
-
-    // Create Subscriber
-
-    SubscriberAttributes Rparam;
-    Rparam.topic.topicKind = NO_KEY;
-    Rparam.topic.topicDataType = myType.getName(); //Must be registered before the creation of the subscriber
-    Rparam.topic.topicName = strtopic;//"TopicsPubSubTopic";
-
-    Rparam.topic.historyQos.depth = 30;
-    Rparam.topic.resourceLimitsQos.max_samples = 50;
-    Rparam.topic.resourceLimitsQos.allocated_samples = 20;
-
-    Rparam.qos.m_reliability.kind = RELIABLE_RELIABILITY_QOS;
-
-
-    mp_subscriber = Domain::createSubscriber(mp_participant,Rparam, static_cast<SubscriberListener*>(&m_listener));
-    if(mp_subscriber == nullptr)
-    {
-        return false;
-    }
-    return true;
-}
-
-void TopicsSubscriber::SubListener::onSubscriptionMatched(Subscriber* sub,MatchingInfo& info)
-{
-    (void)sub;
-
-    if (info.status == MATCHED_MATCHING)
-    {
-        n_matched++;
-        std::cout << "Subscriber matched" << std::endl;
-    }
-    else
-    {
-        n_matched--;
-        std::cout << "Subscriber unmatched" << std::endl;
-    }
-}
-
-
-void TopicsSubscriber::SubListener::setReceivedTopicFunction(ModuleFun xFun)
-{
-    mFun = xFun;
-    mbSetFun = true;
-}
-void TopicsSubscriber::SubListener::onNewDataMessage(Subscriber* sub)
-{
-    // Take data
-    TopicSample::Message st;
-    static int ncount = 0;
-
-    static int nmaxlatancy = 0;
-
-    std::cout<<"new msg"<<std::endl;
-
-
-//    char * strbuf = new char[1000000];
-//    eprosima::fastcdr::FastBuffer pbuf(strbuf,1000000);
-//    eprosima::fastcdr::Cdr * pxcdr;//
-//    pxcdr = new eprosima::fastcdr::Cdr(pbuf);
-
-//    if(sub->takeNextData(pxcdr, &m_info))
-//    {
-//        if(m_info.sampleKind == ALIVE)
-//        {
-//            // Print your structure data here.
-//            ++n_msg;
-//            std::cout << "Sample received, count=" << n_msg<<std::endl;
-//            st.deserialize(*pxcdr);
-
-//            std::cout<<" size is "<<TopicSample::Message::getCdrSerializedSize(st)<<std::endl;
-//        }
-//    }
-
-//    return;
-
-//    sub->get_first_untaken_info(&m_info);
-    std::cout<<"count is "<<sub->getUnreadCount()<<std::endl;
-
-    if(sub->takeNextData(&st, &m_info))
-    {
-        if(m_info.sampleKind == ALIVE)
-        {
-            // Print your structure data here.
-            ++n_msg;
-            ncount++;
-            std::cout << "Sample received, count=" << st.counter() <<" total: "<<ncount<<std::endl;
-            qint64 timex = QDateTime::currentMSecsSinceEpoch();
-            int nlatancy = (timex - st.sendtime());
-            if(nlatancy>nmaxlatancy)nmaxlatancy = nlatancy;
-            std::cout<<"  latency is "<<nlatancy<<" max: "<<nmaxlatancy<<std::endl;
-            std::cout<<" size is "<<st.xdata().size()<<std::endl;
-            QDateTime dt = QDateTime::fromMSecsSinceEpoch(st.sendtime());
-            if(mbSetFun) mFun((char *)(st.xdata().data()),st.xdata().size(),st.counter(),&dt,st.msgname().data());
-
-
-        }
-    }
-}
-
-void TopicsSubscriber::setReceivedTopicFunction(ModuleFun xFun)
-{
-    m_listener.setReceivedTopicFunction(xFun);
-}
-
-void TopicsSubscriber::run()
-{
-    std::cout << "Waiting for Data, press Enter to stop the Subscriber. "<<std::endl;
-    std::cin.ignore();
-    std::cout << "Shutting down the Subscriber." << std::endl;
-}
-

src1/common/modulecomm_fastrtps/TopicsSubscriber.h

@@ -1,72 +0,0 @@
-// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima).
-//
-// 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.
-
-/*!
- * @file TopicsSubscriber.h
- * This header file contains the declaration of the subscriber functions.
- *
- * This file was generated by the tool fastcdrgen.
- */
-
-
-#ifndef _TOPICSAMPLE_TOPICS_SUBSCRIBER_H_
-#define _TOPICSAMPLE_TOPICS_SUBSCRIBER_H_
-
-#include <fastrtps/fastrtps_fwd.h>
-#include <fastrtps/subscriber/SubscriberListener.h>
-#include <fastrtps/subscriber/SampleInfo.h>
-#include "TopicsPubSubTypes.h"
-
-
-#include <QDateTime>
-
-typedef std::function<void(const char * ,const unsigned int , const unsigned int , QDateTime * ,const char *)> ModuleFun;
-
-class TopicsSubscriber
-{
-public:
-	TopicsSubscriber();
-	virtual ~TopicsSubscriber();
-    bool init(const char * strtopic);
-	void run();
-
-    void setReceivedTopicFunction(ModuleFun xFun);
-private:
-	eprosima::fastrtps::Participant *mp_participant;
-	eprosima::fastrtps::Subscriber *mp_subscriber;
-
-
-    char mstrtopic[256];
-
-	class SubListener : public eprosima::fastrtps::SubscriberListener
-	{
-	public:
-		SubListener() : n_matched(0),n_msg(0){};
-		~SubListener(){};
-		void onSubscriptionMatched(eprosima::fastrtps::Subscriber* sub,eprosima::fastrtps::rtps::MatchingInfo& info);
-		void onNewDataMessage(eprosima::fastrtps::Subscriber* sub);
-		eprosima::fastrtps::SampleInfo_t m_info;
-		int n_matched;
-		int n_msg;
-
-        void setReceivedTopicFunction(ModuleFun xFun);
-
-    private:
-        bool mbSetFun = false;
-        ModuleFun mFun;
-	} m_listener;
-	TopicSample::MessagePubSubType myType;
-};
-
-#endif // _TOPICSAMPLE_TOPICS_SUBSCRIBER_H_

src1/common/modulecomm_fastrtps/ivmodulemsg_type.h

@@ -1,13 +0,0 @@
-#ifndef IVMODULEMSG_TYPE_H
-#define IVMODULEMSG_TYPE_H
-
-namespace iv {
-struct modulemsg_type
-{
-    char mstrmsgname[256];
-    int mnBufSize;
-    int mnMsgBufCount;
-};
-
-}
-#endif // IVMODULEMSG_TYPE_H

src1/common/modulecomm_fastrtps/modulecomm_fastrtps.cpp

@@ -1,63 +0,0 @@
-#include "modulecomm_fastrtps.h".h"
-//#include "procsm_if.h"
-//#include "procsm.h"
-
-#include "modulecomm_impl.h"
-
-namespace iv {
-namespace modulecomm_fastrtps {
-
-
-void *  RegisterSend(const char * strcommname,const unsigned int nBufSize,const unsigned int nMsgBufCount)
-{
-
-
-    modulecomm_impl * pif = new modulecomm_impl(strcommname,modulecomm_impl::type_send);
-
-//    procsm_if * pif = new procsm_if(strcommname,nBufSize,nMsgBufCount,procsm::ModeWrite);
-    return (void *)pif;
-}
-
-void  *  RegisterRecv(const char * strcommname,SMCallBack pCall)
-{
-    modulecomm_impl * pif = new modulecomm_impl(strcommname,modulecomm_impl::type_recv);
- //   procsm_if * pif = new procsm_if(strcommname,0,0,procsm::ModeRead);
-    pif->listenmsg(pCall);
-    return (void *)pif;
-}
-
-void * MODULECOMMFASTRTPSSHARED_EXPORT RegisterRecvPlus(const char * strcommname,ModuleFun xFun)
-{
-    modulecomm_impl * pif = new modulecomm_impl(strcommname,modulecomm_impl::type_recv);
- //   procsm_if * pif = new procsm_if(strcommname,0,0,procsm::ModeRead);
-    pif->listenmsg(xFun);
-    return (void *)pif;
-}
-void  ModuleSendMsg(void * pHandle,const char * strdata,const unsigned int nDataLen)
-{
-    modulecomm_impl * pif= (modulecomm_impl *)pHandle;
- //   procsm_if * pif = (procsm_if *)pHandle;
-    pif->writemsg(strdata,nDataLen);
-}
-
-void  Unregister(void * pHandle)
-{
-    modulecomm_impl * pif= (modulecomm_impl *)pHandle;
-//    procsm_if * pif = (procsm_if *)pHandle;
-    delete pif;
-}
-
-void PauseComm(void *pHandle)
-{
-//    procsm_if * pif = (procsm_if *)pHandle;
-//    pif->pausecomm();
-}
-
-void ContintuComm(void *pHandle)
-{
- //   procsm_if * pif = (procsm_if *)pHandle;
- //   pif->continuecomm();
-}
-
-}
-}

src1/common/modulecomm_fastrtps/modulecomm_fastrtps.h

@@ -1,40 +0,0 @@
-#ifndef MODULECOMM_FASTRTPS_H
-#define MODULECOMM_FASTRTPS_H
-
-#include <QtCore/qglobal.h>
-#include <QDateTime>
-
-#include <functional>
-
-#if defined(MODULECOMM_FASTRTPS_LIBRARY)
-#  define MODULECOMMFASTRTPSSHARED_EXPORT Q_DECL_EXPORT
-#else
-#  define MODULECOMMFASTRTPSSHARED_EXPORT Q_DECL_IMPORT
-#endif
-
-
-
-//#include <iostream>
-//#include <thread>
-
-//using namespace std::placeholders;
-typedef std::function<void(const char * ,const unsigned int , const unsigned int , QDateTime * ,const char *)> ModuleFun;
-typedef void (* SMCallBack)(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname);
-
-namespace iv {
-namespace modulecomm_fastrtps {
-void * MODULECOMMFASTRTPSSHARED_EXPORT RegisterSend(const char * strcommname,const unsigned int nBufSize,const unsigned int nMsgBufCount);
-void * MODULECOMMFASTRTPSSHARED_EXPORT RegisterRecv(const char * strcommname,SMCallBack pCall);
-void * MODULECOMMFASTRTPSSHARED_EXPORT RegisterRecvPlus(const char * strcommname,ModuleFun xFun);
-void MODULECOMMFASTRTPSSHARED_EXPORT ModuleSendMsg(void * pHandle,const char * strdata,const unsigned int nDataLen);
-void MODULECOMMFASTRTPSSHARED_EXPORT Unregister(void * pHandle);
-void MODULECOMMFASTRTPSSHARED_EXPORT PauseComm(void * pHandle);
-void MODULECOMMFASTRTPSSHARED_EXPORT ContintuComm(void * pHandle);
-
-}
-
-}
-
-
-
-#endif 

src1/common/modulecomm_fastrtps/modulecomm_fastrtps.pro

@@ -1,45 +0,0 @@
-#-------------------------------------------------
-#
-# Project created by QtCreator 2018-07-10T05:46:48
-#
-#-------------------------------------------------
-
-QT       -= gui
-
-
-#DEFINES += dds_use_shm
-
-TARGET = modulecomm_fastrtps
-TEMPLATE = lib
-
-DEFINES += MODULECOMM_FASTRTPS_LIBRARY
-
-VERSION = 1.0.1
-CONFIG += plugin
-
-
-SOURCES += modulecomm_fastrtps.cpp \
-    Topics.cxx \
-    TopicsPubSubTypes.cxx \
-    TopicsPublisher.cxx \
-    TopicsSubscriber.cxx \
-    modulecomm_impl.cpp
-
-HEADERS += modulecomm_fastrtps.h \
-    Topics.h \
-    TopicsPubSubTypes.h \
-    TopicsPublisher.h \
-    TopicsSubscriber.h \
-    modulecomm_impl.h \
-    ivmodulemsg_type.h
-
-unix {
-    target.path = /usr/lib
-    INSTALLS += target
-}
-
-#INCLUDEPATH += $$PWD/../../../include/
-
-
-LIBS += -L$$PWD -lfastcdr -lfastrtps
-

src1/common/modulecomm_fastrtps/modulecomm_impl.cpp

@@ -1,140 +0,0 @@
-#include "modulecomm_impl.h"
-
-#include <thread>
-#include <iostream>
-#include <QDateTime>
-
-#include <QMutex>
-#include <QFile>
-
-namespace iv {
-namespace modulecomm {
-QMutex gmodulecomm_dds_Mutex;
-int createcount = 0;
-}
-
-}
-
-
-void modulecomm_impl::callbackTopic(const char * strdata,const unsigned int nSize,const unsigned int index, QDateTime * dt,const char * strmemname) {
-
-  if(mbFunPlus)
-  {
-      mFun(strdata,nSize,index,dt,strmemname);
-  }
-  else
-  {
-     (*mpCall)(strdata,nSize,index,dt,strmemname);
-  }
-}
-
-
-int modulecomm_impl::GetTempConfPath(char *strpath)
-{
-    char strtmppath[256];
-    QDateTime dt = QDateTime::currentDateTime();
-    snprintf(strtmppath,256,"/tmp/adc_modulecomm_conf_%04d%02d%02d%02d%02d.ini",dt.date().year(),
-             dt.date().month(),dt.date().day(),dt.time().hour(),dt.time().minute());
-    QFile xFile;
-    xFile.setFileName(strtmppath);
-    char strtem[256];
-    char strdata[10000];
-    snprintf(strdata,10000,"");
-    if(!xFile.exists())
-    {
-        if(xFile.open(QIODevice::ReadWrite))
-        {
-            snprintf(strtem,256,"[common]\n");strncat(strdata,strtem,10000);
-            snprintf(strtem,256,"DCPSDefaultDiscovery=TheRTPSConfig\n");strncat(strdata,strtem,10000);
-#ifdef dds_use_shm
-            snprintf(strtem,256,"DCPSGlobalTransportConfig=myconfig\n");strncat(strdata,strtem,10000);
-            snprintf(strtem,256,"[config/myconfig]\n");strncat(strdata,strtem,10000);
-            snprintf(strtem,256,"transports=share\n");strncat(strdata,strtem,10000);
-            snprintf(strtem,256,"[transport/share]\n");strncat(strdata,strtem,10000);
-            snprintf(strtem,256,"transport_type=shmem\n");strncat(strdata,strtem,10000);
-            snprintf(strtem,256,"pool_size=100000000\n");strncat(strdata,strtem,10000);
-#endif
-            snprintf(strtem,256,"[rtps_discovery/TheRTPSConfig]\n");strncat(strdata,strtem,10000);
-            snprintf(strtem,256,"ResendPeriod=5\n");strncat(strdata,strtem,10000);
-            xFile.write(strdata,strnlen(strdata,10000));
-            xFile.close();
-        }
-    }
-    strncpy(strpath,strtmppath,255);
-    return 0;
-}
-
-modulecomm_impl::modulecomm_impl(const char * strcommname,int ntype )
-{
-
-    strncpy(mstrtopic,strcommname,255);
-
-    iv::modulecomm::gmodulecomm_dds_Mutex.lock();
-    if(ntype == type_recv)
-    {
-        mpSub = new TopicsSubscriber();
-        mpSub->init(strcommname);
-//        std::this_thread::sleep_for(std::chrono::milliseconds(10));
-
-        mnType = type_recv;
-    }
-    else
-    {
-        mpPub = new TopicsPublisher();
-        mpPub->init(strcommname);
- //       std::this_thread::sleep_for(std::chrono::milliseconds(10));
-        mnType = type_send;
-    }
-    iv::modulecomm::createcount++;
-    std::cout<<"count is "<<iv::modulecomm::createcount<<std::endl;
-    iv::modulecomm::gmodulecomm_dds_Mutex.unlock();
-
-}
-
-int modulecomm_impl::listenmsg(ModuleFun xFun)
-{
-    if(mnType == type_send)
-    {
-        std::cout<<"send not listen."<<std::endl;;
-        return -1;
-    }
-    mbFunPlus = true;
-    mFun = xFun;
-    ModuleFun topicFunction = std::bind(&modulecomm_impl::callbackTopic,this,std::placeholders::_1,
-                                                                                std::placeholders::_2,
-                                                                                std::placeholders::_3,
-                                                                                std::placeholders::_4,
-                                                                                std::placeholders::_5);
-    mpSub->setReceivedTopicFunction(topicFunction);
-    return 0;
-}
-
-int modulecomm_impl::listenmsg(SMCallBack pCall)
-{
-    if(mnType == type_send)
-    {
-        std::cout<<"send not listen."<<std::endl;
-        return -1;
-    }
-    mbFunPlus = false;
-    mpCall = pCall;
-    ModuleFun topicFunction = std::bind(&modulecomm_impl::callbackTopic,this,std::placeholders::_1,
-                                                                                std::placeholders::_2,
-                                                                                std::placeholders::_3,
-                                                                                std::placeholders::_4,
-                                                                                std::placeholders::_5);
-    mpSub->setReceivedTopicFunction(topicFunction);
-    return 0;
-}
-
-void modulecomm_impl::writemsg(const char *str, int nlen)
-{
-    if(mnType == type_recv)
-    {
-        std::cout<<"recv not send."<<std::endl;
-        return ;
-    }
-
-    mpPub->senddata(str,nlen);
-
-}

src1/common/modulecomm_fastrtps/modulecomm_impl.h

@@ -1,41 +0,0 @@
-#ifndef MODULECOMM_IMPL_H
-#define MODULECOMM_IMPL_H
-
-#include <QDateTime>
-
-#include "TopicsPublisher.h"
-#include "TopicsSubscriber.h"
-
-
-typedef void (* SMCallBack)(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname);
-
-
-class modulecomm_impl
-{
-public:
-    const static int type_send = 1;
-    const static int type_recv = 2;
-public:
-    modulecomm_impl(const char * strcommname,int ntype = 2);
-    int listenmsg(SMCallBack pCall);
-    int listenmsg(ModuleFun xFun);
-    void writemsg(const char * str,int nlen);
-
-private:
-    char mstrtopic[256];
-    int mnType = type_recv;
-    TopicsPublisher * mpPub;
-    TopicsSubscriber * mpSub;
-    SMCallBack  mpCall;
-    ModuleFun mFun;
-    void callbackTopic(const char * strdata,const unsigned int nSize,const unsigned int index,QDateTime * dt,const char * strmemname);
-
-    int GetTempConfPath(char * strpath);
-
-
-
-    bool mbFunPlus = false;
-
-};
-
-#endif // MODULECOMM_IMPL_H

src1/common/modulecomm_inter/.gitignore

@@ -1,73 +0,0 @@
-# This file is used to ignore files which are generated
-# ----------------------------------------------------------------------------
-
-*~
-*.autosave
-*.a
-*.core
-*.moc
-*.o
-*.obj
-*.orig
-*.rej
-*.so
-*.so.*
-*_pch.h.cpp
-*_resource.rc
-*.qm
-.#*
-*.*#
-core
-!core/
-tags
-.DS_Store
-.directory
-*.debug
-Makefile*
-*.prl
-*.app
-moc_*.cpp
-ui_*.h
-qrc_*.cpp
-Thumbs.db
-*.res
-*.rc
-/.qmake.cache
-/.qmake.stash
-
-# qtcreator generated files
-*.pro.user*
-
-# xemacs temporary files
-*.flc
-
-# Vim temporary files
-.*.swp
-
-# Visual Studio generated files
-*.ib_pdb_index
-*.idb
-*.ilk
-*.pdb
-*.sln
-*.suo
-*.vcproj
-*vcproj.*.*.user
-*.ncb
-*.sdf
-*.opensdf
-*.vcxproj
-*vcxproj.*
-
-# MinGW generated files
-*.Debug
-*.Release
-
-# Python byte code
-*.pyc
-
-# Binaries
-# --------
-*.dll
-*.exe
-

src1/common/modulecomm_inter/intercomm.cpp

@@ -1,480 +0,0 @@
-#include "intercomm.h"
-
-#include <QMutex>
-#include <QWaitCondition>
-#include <iostream>
-
-namespace  iv {
-
-struct InterListenUnit
-{
-    QMutex mWaitMutex;
-    SMCallBack  mpCall;
-    ModuleFun mFun;
-    QWaitCondition * mpwc;
-    bool mbFunPlus = false;
-};
-
-struct interunit
-{
-    char strintername[256];
-    char * strdatabuf;
-    int nbufsize = 0;
-    int nPacCount;
-    QMutex mMutexUnit;
-    std::vector<InterListenUnit *> mvectorlisten;
-    QWaitCondition mwc;
-    bool mbHaveWriter = false;
-
-};
-
-std::vector<interunit *> gvectorinter;
-QMutex gMutexInter;
-
-
-static interunit * FindInterUnitByName(const char * strname)
-{
-    interunit * p = 0;
-    int i;
-    int nsize;
-    nsize = gvectorinter.size();
-    for(i=0;i<nsize;i++)
-    {
-        if(strncmp(strname,gvectorinter.at(i)->strintername,256) == 0)
-        {
-            return (gvectorinter.at(i));
-        }
-    }
-    return p;
-}
-
-
-intercomm::intercomm(const char * strsmname,const unsigned int nBufSize,const unsigned int nMaxPacCount,const int nMode)
-{
-    strncpy(mstrsmname,strsmname,256);
-
-    mnMode = nMode;
-    if(nMode == ModeWrite)
-    {
-        gMutexInter.lock();
-        interunit * p = FindInterUnitByName(strsmname);
-        if(p == 0)
-        {
-            interunit * pnewinter = new interunit;
-            strncpy(pnewinter->strintername,strsmname,256);
-            pnewinter->strdatabuf = new char[sizeof(procinter_info)+nMaxPacCount*sizeof(procinter_head) + nBufSize];
-            pnewinter->nPacCount = nMaxPacCount;
-            pnewinter->nbufsize = nBufSize;
-            gvectorinter.push_back(pnewinter);
-            mpa = pnewinter;
-
-
-        }
-        else
-        {
-            p->mMutexUnit.lock();
-            delete p->strdatabuf;
-            p->strdatabuf = new char[sizeof(procinter_info)+nMaxPacCount*sizeof(procinter_head) + nBufSize];
-            p->nPacCount = nMaxPacCount;
-            p->nbufsize = nBufSize;
-            p->mMutexUnit.unlock();
-            mpa = p;
-        }
-
-        interunit * pinter = (interunit * )mpa;
-        char * pdata = (char *)pinter->strdatabuf;
-        mpinfo = (procinter_info *)pdata;
-        mphead = (procinter_head *)(pdata+sizeof(procinter_info));
-        mpinfo->mCap = nMaxPacCount;
-        mpinfo->mnBufSize = nBufSize;
-        mpinfo->mFirst = 0;
-        mpinfo->mNext = 0;
-        mpinfo->mLock = 0;
-        pinter->mbHaveWriter = true;
-
-        gMutexInter.unlock();
-
-    }
-    else
-    {
-        gMutexInter.lock();
-        interunit * p = FindInterUnitByName(strsmname);
-        if(p == 0)
-        {
-            interunit * pnewinter = new interunit;
-            strncpy(pnewinter->strintername,strsmname,256);
-            gvectorinter.push_back(pnewinter);
-            mpa = pnewinter;
-        }
-        else
-        {
-            mpa = p;
-        }
-
-        interunit * pinter = (interunit * )mpa;
-        char * pdata = (char *)pinter->strdatabuf;
-        mpinfo = (procinter_info *)pdata;
-        mphead = (procinter_head *)(pdata+sizeof(procinter_info));
-
-        gMutexInter.unlock();
-    }
-}
-
-intercomm::~intercomm()
-{
-    if(mnMode == ModeRead)
-    {
-        stoplisten();
-        interunit * p = (interunit *)mpa;
-        p->mMutexUnit.lock();
-        InterListenUnit * plisten = (InterListenUnit *)mplistenunit;
-        int i;
-        for(i=0;i<p->mvectorlisten.size();i++)
-        {
-            if(plisten == p->mvectorlisten.at(i))
-            {
-                p->mvectorlisten.erase(p->mvectorlisten.begin() + i);
-                delete plisten;
-                break;
-            }
-        }
-        p->mMutexUnit.unlock();
-    }
-}
-
-int intercomm::listenmsg(ModuleFun xFun)
-{
-    if(mnMode == ModeWrite)
-    {
-        std::cout<<"intercomm::listenmsg this is Write. Can't Listen."<<std::endl;
-        return - 1;
-    }
-
-    interunit * p = (interunit *)mpa;
-    p->mMutexUnit.lock();
-    InterListenUnit * pnewlisten = new InterListenUnit;
-    pnewlisten->mbFunPlus = true;
-    pnewlisten->mFun = xFun;
-    pnewlisten->mpwc = &p->mwc;
-    p->mvectorlisten.push_back(pnewlisten);
-    p->mMutexUnit.unlock();
-
-    mplistenunit = (void *)pnewlisten;
-
-    mplistenthread = new std::thread(&intercomm::listernrun,this);
-    return 0;
-}
-
-int intercomm::listenmsg(SMCallBack pCall)
-{
-    if(mnMode == ModeWrite)
-    {
-        std::cout<<"intercomm::listenmsg this is Write. Can't Listen."<<std::endl;
-        return - 1;
-    }
-
-    interunit * p = (interunit *)mpa;
-    p->mMutexUnit.lock();
-    InterListenUnit * pnewlisten = new InterListenUnit;
-    pnewlisten->mbFunPlus = false;
-    pnewlisten->mpCall = pCall;
-    pnewlisten->mpwc = &p->mwc;
-    p->mvectorlisten.push_back(pnewlisten);
-    p->mMutexUnit.unlock();
-    mplistenunit = (void *)pnewlisten;
-    mplistenthread = new std::thread(&intercomm::listernrun,this);
-    return 0;
-}
-
-void intercomm::stoplisten()
-{
-    mblistenrun = false;
-    if(mplistenthread != 0)
-    {
-        mplistenthread->join();
-        mplistenthread = 0;
-    }
-}
-
-void intercomm::pausecomm()
-{
-    mbPause = true;
-}
-
-void intercomm::continuecomm()
-{
-    mbPause = false;
-}
-
-void intercomm::listernrun()
-{
-    InterListenUnit * pILU = (InterListenUnit * )mplistenunit;
-    QTime xTime;
-    xTime.start();
-    unsigned int nBufLen = 1;
-    unsigned int nRead;
-    char * str = new char[nBufLen];
-    unsigned int index =0;
-    QDateTime *pdt = new QDateTime();
-    interunit * pinter = (interunit *)mpa;
-    while(mblistenrun)
-    {
-        if(mbPause)
-        {
-            std::this_thread::sleep_for(std::chrono::milliseconds(100));
-            continue;
-        }
-        if(pinter->mbHaveWriter == false)
-        {
-            std::this_thread::sleep_for(std::chrono::milliseconds(100));
-            continue;
-        }
-        pILU->mWaitMutex.lock();
-        pILU->mpwc->wait(&pILU->mWaitMutex,100);
-        pILU->mWaitMutex.unlock();
-        int nRtn = readmsg(index,str,nBufLen,&nRead,pdt);
-        while ((nRtn != 0)&&(mblistenrun))
-        {
-            if(nRtn == -1)
-            {
-                nBufLen = nRead;
-                delete str;
-                if(nBufLen < 1)nBufLen = 1;
-                str = new char[nBufLen];
-            }
-            else
-            {
-                if(nRtn == -2)
-                {
-                   index = getcurrentnext();
-                }
-                else
-                {
-                   if(nRtn >0)
-                   {
-                       if(pILU->mbFunPlus)
-                       {
-                           pILU->mFun(str,nRtn,index,pdt,mstrsmname);
-                       }
-                       else
-                       {
-                          (*pILU->mpCall)(str,nRtn,index,pdt,mstrsmname);
-                       }
-                       index++;
-                   }
-                   else
-                   {
-                       std::this_thread::sleep_for(std::chrono::milliseconds(100));
-                   }
-                }
-            }
-            nRtn = readmsg(index,str,nBufLen,&nRead,pdt);
-        }
-
-    }
-
-    delete str;
-    delete pdt;
-}
-
-int intercomm::writemsg(const char *str, const unsigned int nSize)
-{
-    if(mbPause)return -2;
-    if(mnMode == ModeRead)
-    {
-        std::cout<<"Register read. can't write."<<std::endl;
-        return -1;
-    }
-    interunit * pinter = (interunit *)mpa;
-
-    pinter->mMutexUnit.lock();
-    char * pdata = (char *)pinter->strdatabuf;
-    mpinfo = (procinter_info *)pdata;
-    mphead = (procinter_head *)(pdata+sizeof(procinter_info));
-    if(nSize > pinter->nbufsize)
-    {
-        qDebug("procsm::writemsg message size is very big");
-        return -1;
-    }
-
-WRITEMSG:
-
-    char * pH,*pD;
-    QDateTime dt;
-    pH = (char *)pinter->strdatabuf;pH = pH + sizeof(procinter_info);
-    pD = (char *)pinter->strdatabuf;pD = pD + sizeof(procinter_info) + pinter->nPacCount * sizeof(procinter_head);
-    procinter_head * phh = (procinter_head *)pH;
-    unsigned int nPac = mpinfo->mNext - mpinfo->mFirst;
-    if(nPac>=pinter->nPacCount)
-    {
-        unsigned int nRemove = pinter->nPacCount/3;
-        if(nRemove == 0)nRemove = 1;
-        MoveMem(nRemove);
-        goto WRITEMSG;
-    }
-    if(nPac == 0)
-    {
-        memcpy(pD,str,nSize);
-        dt = QDateTime::currentDateTime();
-        phh->SetDate(dt);
-        phh->mindex = mpinfo->mNext;
-        phh->mnPos = 0;
-        phh->mnLen = nSize;
-        mpinfo->mNext = mpinfo->mNext+1;
-    }
-    else
-    {
-        if(((phh+nPac-1)->mnPos+(phh+nPac-1)->mnLen + nSize)>=pinter->nbufsize)
-        {
-            unsigned int nRemove = pinter->nPacCount/2;
-            if(nRemove == 0)nRemove = 1;
-            MoveMem(nRemove);
-            goto WRITEMSG;
-        }
-        else
-        {
-            unsigned int nPos = (phh+nPac-1)->mnPos + (phh+nPac-1)->mnLen;
-            memcpy(pD+nPos,str,nSize);
-            dt = QDateTime::currentDateTime();
-            (phh+nPac)->SetDate(dt);
-            (phh+nPac)->mindex = mpinfo->mNext;
-            (phh+nPac)->mnPos = nPos;
-            (phh+nPac)->mnLen = nSize;
-            mpinfo->mNext = mpinfo->mNext+1;
-        }
-    }
-
-    const unsigned int nTM = 0x6fffffff;
-    if((mpinfo->mNext >nTM)&&(mpinfo->mFirst>nTM))
-    {
-       nPac = mpinfo->mNext - mpinfo->mFirst;
-       unsigned int i;
-       for(i=0;i<nPac;i++)
-       {
-           (phh+i)->mindex = (phh+i)->mindex-nTM;
-       }
-       mpinfo->mFirst = mpinfo->mFirst-nTM;
-       mpinfo->mNext = mpinfo->mNext - nTM;
-    }
-
-    pinter->mMutexUnit.unlock();
-    pinter->mwc.wakeAll();
-    return 0;
-}
-
-int intercomm::MoveMem(const unsigned int nSize)
-{
-    unsigned int nRemove = nSize;
-    if(nRemove == 0)return -1;
-    interunit * pinter = (interunit *)mpa;
-    char * pH,*pD;
-    pH = (char *)pinter->strdatabuf;pH = pH + sizeof(procinter_info);
-    pD = (char *)pinter->strdatabuf;pD = pD + sizeof(procinter_info) + pinter->nPacCount * sizeof(procinter_head);
-    procinter_head * phh = (procinter_head *)pH;
-    unsigned int nPac = mpinfo->mNext - mpinfo->mFirst;
-
-    if(nRemove >nPac)
-    {
-        nRemove = nPac;
-    }
-
-    if(nRemove == nPac)
-    {
-        mpinfo->mFirst = mpinfo->mFirst + (unsigned int)nRemove;
-        return 0;
-    }
-
-    unsigned int i;
-    int nDataMove = 0;
-    for(i=0;i<nRemove;i++)
-    {
-        procinter_head * phd = phh+i;
-        nDataMove = nDataMove + phd->mnLen;
-    }
-    unsigned int nDataTotal;
-    for(i=0;i<(nPac - nRemove);i++)
-    {
-        memcpy(phh+i,phh+i+nRemove,sizeof(procinter_head));
-        (phh+i)->mnPos = (phh+i)->mnPos - nDataMove;
-    }
-    nDataTotal = (phh + nPac-nRemove-1)->mnPos + (phh+nPac-nRemove-1)->mnLen;
-    char * strtem = new char[pinter->nbufsize];
-    memcpy(strtem,pD+nDataMove,nDataTotal);
-    memcpy(pD,strtem,nDataTotal);
-    delete strtem;
-
-    mpinfo->mFirst = mpinfo->mFirst + (unsigned int)nRemove;
-    return 0;
-}
-
-int intercomm::readmsg(unsigned int index, char *str, unsigned int nMaxSize, unsigned int *nRead, QDateTime *pdt)
-{
-    int nRtn = 0;
-
-    interunit * pinter = (interunit *)mpa;
-
-    if(pinter->nbufsize == 0)return 0;
-    char * pdata = (char *)pinter->strdatabuf;
-    mpinfo = (procinter_info *)pdata;
-    mphead = (procinter_head *)(pdata+sizeof(procinter_info));
-
-    if((index< mpinfo->mFirst)||(index > mpinfo->mNext))
-    {
-        nRtn = -2;
-    }
-    if(nRtn != (-2))
-    {
-        if(index == mpinfo->mNext)
-        {
-            nRtn = 0;
-        }
-        else
-        {
-            char * pH,*pD;
- //           pH = (char *)mpASM->data();pH = pH + 2*sizeof(unsigned int);
-
- //           pD = (char *)mpASM->data();pD = pD + 2*sizeof(unsigned int) + mnMaxPacCount * sizeof(procsm_head);
-            pD = (char *)pinter->strdatabuf;pD = pD+ sizeof(procinter_info) + mpinfo->mCap*sizeof(procinter_head);
-            pH = (char *)pinter->strdatabuf;pH = pH+sizeof(procinter_info);
-            procinter_head * phh = (procinter_head *)pH;
-            unsigned int nPac = mpinfo->mNext - mpinfo->mFirst;
-            if(nPac == 0)
-            {
-                nRtn = 0;
-            }
-            else
-            {
-                unsigned int nPos = index - mpinfo->mFirst;
-                *nRead = (phh+nPos)->mnLen;
-                if((phh+nPos)->mnLen > nMaxSize)
-                {
-                    nRtn = -1;
-
-                }
-                else
-                {
-        //            qDebug("read pos = %d",(phh+nPos)->mnPos);
-                   memcpy(str,pD + (phh+nPos)->mnPos,(phh+nPos)->mnLen);
-          //         qDebug("read pos = %d",(phh+nPos)->mnPos);
-                   nRtn = (phh+nPos)->mnLen;
-                   (phh+nPos)->GetDate(pdt);
-           //        memcpy(pdt,&((phh+nPos)->mdt),sizeof(QDateTime));
-                }
-            }
-        }
-    }
-    return nRtn;
-}
-
-unsigned int intercomm::getcurrentnext()
-{
-    unsigned int nNext;
-    interunit * pinter = (interunit *)mpa;
-    char * pdata = (char *)pinter->strdatabuf;
-    mpinfo = (procinter_info *)pdata;
-    mphead = (procinter_head *)(pdata+sizeof(procinter_info));
-    nNext = mpinfo->mNext;
-    return nNext;
-}
-
-}

src1/common/modulecomm_inter/intercomm.h

@@ -1,109 +0,0 @@
-#ifndef INTERCOMM_H
-#define INTERCOMM_H
-
-#include <QDateTime>
-#include "vector"
-#include <functional>
-#include <thread>
-
-typedef void (* SMCallBack)(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname);
-using namespace std::placeholders;
-typedef std::function<void(const char * ,const unsigned int , const unsigned int , QDateTime * ,const char *)> ModuleFun;
-
-
-class procinter_info
-{
-public:
-  unsigned int mFirst;
-  unsigned int mNext;
-  unsigned int mCap;
-  unsigned int mLock;
-  unsigned int mnBufSize;
-};
-
-class procinter_head
-{
-public:
-    unsigned short mYear;
-    unsigned char mMonth;
-    unsigned char mDay;
-    unsigned char mHour;
-    unsigned char mMinute;
-    unsigned char mSec;
-    unsigned short mMSec;
-    unsigned int mindex;
-    unsigned int mnPos;
-    unsigned int mnLen;
-public:
-    void SetDate(QDateTime dt)
-    {
-        mYear = dt.date().year();
-        mMonth = dt.date().month();
-        mDay = dt.date().day();
-        mHour = dt.time().hour();
-        mMinute = dt.time().minute();
-        mSec = dt.time().second();
-        mMSec = dt.time().msec();
-    }
-    void GetDate(QDateTime * pdt)
-    {
-        QDate dt;
-        dt.setDate(mYear,mMonth,mDay);
-        QTime time;
-        time.setHMS(mHour,mMinute,mSec,mMSec);
-        pdt->setDate(dt);
-        pdt->setTime(time);
-
-    }
-};
-
-
-namespace  iv {
-
-class intercomm
-{
-public:
-    intercomm(const char * strsmname,const unsigned int nBufSize,const unsigned int nMaxPacCount,const int nMode);
-    ~intercomm();
-
-    int listenmsg(SMCallBack pCall);
-    int listenmsg(ModuleFun xFun);
-    void stoplisten();
-
-    void pausecomm();
-    void continuecomm();
-
-    const static int ModeRead = 1;
-    const static int ModeWrite = 0;
-
-    int writemsg(const char * str,const unsigned int nSize);
-
-private:
-
-
-    char mstrsmname[256];
-    int mnMode;
-    void * mpa;
-    void * mplistenunit;
-
-private:
-    void listernrun();
-    bool mblistenrun = true;
-    bool mbPause = false;
-    std::thread * mplistenthread = 0;
-
-    procinter_info * mpinfo;
-    procinter_head * mphead;
-
-
-private:
-
-    int readmsg(unsigned int index,char * str,unsigned int nMaxSize,unsigned int * nRead,QDateTime * pdt);
-    int MoveMem(const unsigned int nSize);
-    unsigned int getcurrentnext();
-};
-}
-
-
-
-#endif // INTERCOMM_H

src1/common/modulecomm_inter/modulecomm_inter.cpp

@@ -1,56 +0,0 @@
-#include "modulecomm_inter.h"
-#include <iostream>
-
-#include "intercomm.h"
-
-namespace iv {
-namespace modulecomm_inter {
-
-void *  RegisterSend(const char * strcommname,const unsigned int nBufSize,const unsigned int nMsgBufCount)
-{
-    iv::intercomm * pinter = new iv::intercomm(strcommname,nBufSize,nMsgBufCount,iv::intercomm::ModeWrite);
-    return (void *)pinter;
-}
-
-void  *  RegisterRecv(const char * strcommname,SMCallBack pCall)
-{
-    iv::intercomm * pif = new iv::intercomm(strcommname,0,0,iv::intercomm::ModeRead);
-    pif->listenmsg(pCall);
-    return (void *)pif;
-}
-
-void * MODULECOMM_INTER_EXPORT RegisterRecvPlus(const char * strcommname,ModuleFun xFun)
-{
-    iv::intercomm * pif = new iv::intercomm(strcommname,0,0,iv::intercomm::ModeRead);
-    pif->listenmsg(xFun);
-    return (void *)pif;
-}
-void  ModuleSendMsg(void * pHandle,const char * strdata,const unsigned int nDataLen)
-{
-    iv::intercomm * pif = (iv::intercomm *)pHandle;
-    pif->writemsg(strdata,nDataLen);
-}
-
-void  Unregister(void * pHandle)
-{
-    iv::intercomm * pif = (iv::intercomm *)pHandle;
-    delete pif;
-}
-
-void PauseComm(void *pHandle)
-{
-    iv::intercomm * pif = (iv::intercomm *)pHandle;
-    pif->pausecomm();
-}
-
-void ContintuComm(void *pHandle)
-{
-    iv::intercomm * pif = (iv::intercomm *)pHandle;
-    pif->continuecomm();
-}
-
-}
-}
-
-
-

src1/common/modulecomm_inter/modulecomm_inter.h

@@ -1,37 +0,0 @@
-#ifndef MODULECOMM_INTER_H
-#define MODULECOMM_INTER_H
-
-#include <QtCore/qglobal.h>
-
-#include <functional>
-#include <QDateTime>
-
-#if defined(MODULECOMM_INTER_LIBRARY)
-#  define MODULECOMM_INTER_EXPORT Q_DECL_EXPORT
-#else
-#  define MODULECOMM_INTER_EXPORT Q_DECL_IMPORT
-#endif
-
-#ifndef IV_MODULE_FUN
-
-typedef std::function<void(const char * ,const unsigned int , const unsigned int , QDateTime * ,const char *)> ModuleFun;
-typedef void (* SMCallBack)(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname);
-#define IV_MODULE_FUN
-#endif
-
-
-namespace iv {
-namespace modulecomm_inter {
-void * MODULECOMM_INTER_EXPORT RegisterSend(const char * strcommname,const unsigned int nBufSize,const unsigned int nMsgBufCount);
-void * MODULECOMM_INTER_EXPORT RegisterRecv(const char * strcommname,SMCallBack pCall);
-void * MODULECOMM_INTER_EXPORT RegisterRecvPlus(const char * strcommname,ModuleFun xFun);
-void MODULECOMM_INTER_EXPORT ModuleSendMsg(void * pHandle,const char * strdata,const unsigned int nDataLen);
-void MODULECOMM_INTER_EXPORT Unregister(void * pHandle);
-void MODULECOMM_INTER_EXPORT PauseComm(void * pHandle);
-void MODULECOMM_INTER_EXPORT ContintuComm(void * pHandle);
-
-}
-
-}
-
-#endif // MODULECOMM_INTER_H

src1/common/modulecomm_inter/modulecomm_inter.pro

@@ -1,33 +0,0 @@
-QT -= gui
-
-TEMPLATE = lib
-DEFINES += MODULECOMM_INTER_LIBRARY
-
-CONFIG += c++11
-
-CONFIG += plugin
-
-# 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
-
-SOURCES += \
-    intercomm.cpp \
-    modulecomm_inter.cpp
-
-HEADERS += \
-    intercomm.h \
-    modulecomm_inter.h
-
-# Default rules for deployment.
-unix {
-    target.path = /usr/lib
-}
-!isEmpty(target.path): INSTALLS += target

src1/common/modulecomm_shm/ReadMe.md

@@ -1,90 +0,0 @@
-# How to use share memory
-
-共享内存使用分为发送端和接收端，以下分别介绍
-
-## 发送端
-
-**1. 添加共享内存消息格式文件**
-
-共享内存中数据采用protobuf格式存储，Protobuf是一种平台无关、语言无关、可扩展且轻便高效的序列化数据结构的协议，可以用于**网络通信**和**数据存储**。
-
-为使用共享内存，需添加一个相应的proto文件，文件位于`modularization/src/include/proto/hmi.proto`，可参考已有的文件编写
-
-```protobuf
-syntax = "proto2";
-package iv.hmi;
-message hmimsg
-{
- required bool mbPause = 1;
- required bool mbBocheMode = 2;
- required bool mbbusmode = 3;
-};
-```
-
-**2. 注册共享内存**
-
-在工程配置文件***.pro中添加引用信息：
-
-```
-SOURCES += \
-    ../../include/msgtype/ui.pb.cc
-HEADERS += \   
-    ../../include/msgtype/ui.pb.h
-LIBS += -lprotobuf
-INCLUDEPATH += $$PWD/../../include/msgtype
-LIBS += -L$$PWD/../../../bin/ -lmodulecomm 
-```
-
-设置共享内存名并注册：
-
-```c++
-std::string strmemvbox = "test"
-gpa = iv::modulecomm::RegisterSend(strmemvbox.data(),100000,3);iv::modulecomm::RegisterSend(strmemvbox.data(),100000,3);
-```
-
-写入数据：
-
-```c++
-char * str = new char[gobj.ByteSize()]; //gobj是一个probuf数据，使用方法参考protobuf的使用
-int nsize = gobj.ByteSize();
-if(gobj.SerializeToArray(str,nsize))
-{
-    iv::modulecomm::ModuleSendMsg(gpa,str,nsize);
-}
-```
-
-## 接收端
-
-**从共享内中读取数据**
-
-工程配置文件中添加配置信息：
-
-```pro
-SOURCES += \
-    ../../include/msgtype/ui.pb.cc
-HEADERS += \   
-    ../../include/msgtype/ui.pb.h
-LIBS += -lprotobuf
-INCLUDEPATH += $$PWD/../../include/msgtype
-LIBS += -L$$PWD/../../../bin/ -lmodulecomm 
-```
-
-源码中实现：
-
-```c++
-std::string strmemcan = “test”;//设置共享内存名字
-iv::modulecomm::RegisterRecv(strmemcan.data(),listen); 
-//数据接收函数，共享内存收到数据时，会调用：
-void listen(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
-{
-    if(nSize<1)return;
-    iv::can::canmsg xmsg;
-    if(false == xmsg.ParseFromArray(strdata,nSize))
-    {
-        std::cout<<"esr Listencan0 fail."<<std::endl;
-        return;
-    }
-    DecodeVboxData(xmsg);
-}
-```
-

src1/common/modulecomm_shm/ivmodulemsg_type.h

@@ -1,13 +0,0 @@
-#ifndef IVMODULEMSG_TYPE_H
-#define IVMODULEMSG_TYPE_H
-
-namespace iv {
-struct modulemsg_type
-{
-    char mstrmsgname[256];
-    int mnBufSize;
-    int mnMsgBufCount;
-};
-
-}
-#endif // IVMODULEMSG_TYPE_H

src1/common/modulecomm_shm/modulecomm_shm.cpp

@@ -1,54 +0,0 @@
-#include "modulecomm_shm.h"
-#include "procsm_if.h"
-#include "procsm.h"
-#include <iostream>
-
-namespace iv {
-namespace modulecomm_shm {
-
-void *  RegisterSend(const char * strcommname,const unsigned int nBufSize,const unsigned int nMsgBufCount)
-{
-
-    procsm_if * pif = new procsm_if(strcommname,nBufSize,nMsgBufCount,procsm::ModeWrite);
-    return (void *)pif;
-}
-
-void  *  RegisterRecv(const char * strcommname,SMCallBack pCall)
-{
-    procsm_if * pif = new procsm_if(strcommname,0,0,procsm::ModeRead);
-    pif->listenmsg(pCall);
-    return (void *)pif;
-}
-
-void * MODULECOMMSHARED_SHM_EXPORT RegisterRecvPlus(const char * strcommname,ModuleFun xFun)
-{
-    procsm_if * pif = new procsm_if(strcommname,0,0,procsm::ModeRead);
-    pif->listenmsg(xFun);
-    return (void *)pif;
-}
-void  ModuleSendMsg(void * pHandle,const char * strdata,const unsigned int nDataLen)
-{
-    procsm_if * pif = (procsm_if *)pHandle;
-    pif->writemsg(strdata,nDataLen);
-}
-
-void  Unregister(void * pHandle)
-{
-    procsm_if * pif = (procsm_if *)pHandle;
-    delete pif;
-}
-
-void PauseComm(void *pHandle)
-{
-    procsm_if * pif = (procsm_if *)pHandle;
-    pif->pausecomm();
-}
-
-void ContintuComm(void *pHandle)
-{
-    procsm_if * pif = (procsm_if *)pHandle;
-    pif->continuecomm();
-}
-
-}
-}

src1/common/modulecomm_shm/modulecomm_shm.h

@@ -1,42 +0,0 @@
-#ifndef MODULECOMM_SHM_H
-#define MODULECOMM_SHM_H
-
-#include <QtCore/qglobal.h>
-#include <QDateTime>
-
-#include <functional>
-
-#if defined(MODULECOMM_SHM_LIBRARY)
-#  define MODULECOMMSHARED_SHM_EXPORT Q_DECL_EXPORT
-#else
-#  define MODULECOMMSHARED_SHM_EXPORT Q_DECL_IMPORT
-#endif
-
-
-
-//#include <iostream>
-//#include <thread>
-
-#ifndef IV_MODULE_FUN
-
-typedef std::function<void(const char * ,const unsigned int , const unsigned int , QDateTime * ,const char *)> ModuleFun;
-typedef void (* SMCallBack)(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname);
-#define IV_MODULE_FUN
-#endif
-namespace iv {
-namespace modulecomm_shm {
-void * MODULECOMMSHARED_SHM_EXPORT RegisterSend(const char * strcommname,const unsigned int nBufSize,const unsigned int nMsgBufCount);
-void * MODULECOMMSHARED_SHM_EXPORT RegisterRecv(const char * strcommname,SMCallBack pCall);
-void * MODULECOMMSHARED_SHM_EXPORT RegisterRecvPlus(const char * strcommname,ModuleFun xFun);
-void MODULECOMMSHARED_SHM_EXPORT ModuleSendMsg(void * pHandle,const char * strdata,const unsigned int nDataLen);
-void MODULECOMMSHARED_SHM_EXPORT Unregister(void * pHandle);
-void MODULECOMMSHARED_SHM_EXPORT PauseComm(void * pHandle);
-void MODULECOMMSHARED_SHM_EXPORT ContintuComm(void * pHandle);
-
-}
-
-}
-
-
-
-#endif 

src1/common/modulecomm_shm/modulecomm_shm.pro

@@ -1,41 +0,0 @@
-#-------------------------------------------------
-#
-# Project created by QtCreator 2018-07-10T05:46:48
-#
-#-------------------------------------------------
-
-QT       -= gui
-
-QT       += dbus
-
-
-#DEFINES += USELCM
-
-DEFINES += USEDBUS
-
-TARGET = modulecomm_shm
-TEMPLATE = lib
-
-DEFINES += MODULECOMM_SHM_LIBRARY
-
-#VERSION = 1.0.1
-CONFIG += plugin
-
-
-SOURCES += modulecomm_shm.cpp \
-    procsm.cpp \
-    procsm_if.cpp
-
-HEADERS += modulecomm_shm.h \
-    procsm.h \
-    procsm_if.h \
-    ivmodulemsg_type.h
-
-unix {
-    target.path = /usr/lib
-    INSTALLS += target
-}
-
-#INCLUDEPATH += $$PWD/../../../include/
-LIBS += -L$$PWD
-

src1/common/modulecomm_shm/procsm.cpp

@@ -1,378 +0,0 @@
-#include <iostream>
-#include <thread>
-#include <QTime>
-#include <QThread>
-
-#include "procsm.h"
-
-
-class AttachThread : public QThread
-{
-  public:
-    AttachThread(QSharedMemory * pa,bool & bAttach)
-    {
-       mbAttach = bAttach;
-       mpa = pa;
-       mbrun = true;
-    }
-    QSharedMemory * mpa;
-    bool mbAttach = false;
-    bool mbrun = true;
-    void run()
-    {
-        mbAttach = mpa->attach();
-        mbrun = false;
-    }
-};
-
-procsm::procsm(const char * strsmname,const unsigned int nBufSize,const unsigned int nMaxPacCount,const int nMode)
-{
-//    mnBufSize = nBufSize;
-
-//    qDebug("create dbus");
-
-    mpASM = new QSharedMemory(strsmname);
-
-    if(nMode == ModeWrite)
-    {
-
-        mmodulemsg_type.mnBufSize = nBufSize;
-        mmodulemsg_type.mnMsgBufCount = nMaxPacCount;
-        strncpy(mmodulemsg_type.mstrmsgname,strsmname,255);
-#ifdef USEDBUS
-        mmsg = QDBusMessage::createSignal("/catarc/adc",  "adc.adciv.modulecomm", strsmname);
-        mmsg<<1;
-#endif
-
-        bool bAttach = false;
-        AttachThread AT(mpASM,bAttach);
-        AT.start();
-        QTime xTime;
-        xTime.start();
-        while(xTime.elapsed()<100)
-        {
-            if(AT.mbrun == false)
-            {
-                bAttach = AT.mbAttach;
-                break;
-            }
-        }
- //       qDebug("time is %d",xTime.elapsed());
-        if(xTime.elapsed()>= 1000)
-        {
-            qDebug("in 1000ms Attach fail.terminate it .");
-            AT.terminate();
-            bAttach = false;
-        }
-
- //       if(!mpASM->attach())
-        if(!bAttach)
-        {
-
-            mpASM->create(sizeof(procsm_info)+nMaxPacCount*sizeof(procsm_head) + nBufSize);
-            char * p = (char *)mpASM->data();
-            mpinfo = (procsm_info *)p;
-            mphead = (procsm_head *)(p+sizeof(procsm_info));
-            mpinfo->mCap = nMaxPacCount;
-            mpinfo->mnBufSize = nBufSize;
-            mpinfo->mFirst = 0;
-            mpinfo->mNext = 0;
-            mpinfo->mLock = 0;
-        }
-
-
-
-
-        if(mpASM->isAttached())
-        {
-
-            mbAttach = true;
-            char * p = (char *)mpASM->data();
-            mpinfo = (procsm_info *)p;
-            mphead = (procsm_head *)(p+sizeof(procsm_info));
-            mnMaxPacCount = mpinfo->mCap;
-            mnBufSize = mpinfo->mnBufSize;
-    //        qDebug("attach successful");
-            mstrtem = new char[mnBufSize];
-
-#ifdef USEDBUS
-            mmsgres = QDBusMessage::createSignal("/catarc/adc",  "adciv.interface", "modulemsgres");
-            mmsgres<<1;
-
-            bool bconnect = QDBusConnection::sessionBus().connect(QString(),"/catarc/adc",  "adciv.interface", "modulemsgquery",this,SLOT(onQuery()));
-            if(bconnect == false)
-            {
-                std::cout<<"procsm_if_readthread::procsm_if_readthread bconect is false"<<std::endl;
-            }
-#endif
-        }
-        else
-        {
-          mbAttach = false;
-            qDebug("Share Memory Error.");
-        }
-
-
-    }
-}
-
-#ifdef USEDBUS
-    void procsm::onQuery()
-    {
-        QByteArray ba;
-        ba.append((char *)&mmodulemsg_type,sizeof(iv::modulemsg_type));
-
-        QList<QVariant> x;
-        x<<ba;
-        mmsgres.setArguments(x);
-        QDBusConnection::sessionBus().send(mmsgres);
-    }
-
-#endif
-
-bool procsm::AttachMem()
-{
-    mpASM->attach();
-    if(mpASM->isAttached())
-    {
-        mbAttach = true;
-        char * p = (char *)mpASM->data();
-        mpinfo = (procsm_info *)p;
-        mphead = (procsm_head *)(p+sizeof(procsm_info));
-        mnMaxPacCount = mpinfo->mCap;
-        mnBufSize = mpinfo->mnBufSize;
-        return true;
-    }
-    else
-    {
-        return false;
-    }
-}
-
-int procsm::MoveMem(const unsigned int nSize)
-{
-//    qDebug("move mem");
-     unsigned int nRemove = nSize;
-     if(nRemove == 0)return -1;
-
-//     unsigned int * pIndexFirst = (unsigned int *)mpASM->data();
-//     unsigned int * pIndexNext = pIndexFirst+1;
-//     qDebug("first = %d next = %d",*pIndexFirst,*pIndexNext);
-//    unsigned int * pIndexNext = pIndexFirst;
-    char * pH,*pD;
-    pH = (char *)mpASM->data();pH = pH + sizeof(procsm_info);
-    pD = (char *)mpASM->data();pD = pD + sizeof(procsm_info) + mnMaxPacCount * sizeof(procsm_head);
-    procsm_head * phh = (procsm_head *)pH;
-    unsigned int nPac = mpinfo->mNext - mpinfo->mFirst;
-
-    if(nRemove >nPac)
-    {
-  //      qDebug("procsm::MoveMem nRemove > nPac nRemove = %d",nRemove);
-        nRemove = nPac;
-    }
-
-    if(nRemove == nPac)
-    {
-        mpinfo->mFirst = mpinfo->mFirst + (unsigned int)nRemove;
-        return 0;
-    }
-
-    unsigned int i;
-    int nDataMove = 0;
-    for(i=0;i<nRemove;i++)
-    {
-        procsm_head * phd = phh+i;
-        nDataMove = nDataMove + phd->mnLen;
-    }
-    unsigned int nDataTotal;
-    for(i=0;i<(nPac - nRemove);i++)
-    {
-        memcpy(phh+i,phh+i+nRemove,sizeof(procsm_head));
-        (phh+i)->mnPos = (phh+i)->mnPos - nDataMove;
-    }
-    nDataTotal = (phh + nPac-nRemove-1)->mnPos + (phh+nPac-nRemove-1)->mnLen;
-    memcpy(mstrtem,pD+nDataMove,nDataTotal);
-    memcpy(pD,mstrtem,nDataTotal);
-
-//    for(i=0;i<nDataTotal;i++)
-//    {
-//        *(pD+i) = *(pD+i+nDataMove);
-//    }
-    mpinfo->mFirst = mpinfo->mFirst + (unsigned int)nRemove;
-    return 0;
-}
-
-int procsm::writemsg(const char *str, const unsigned int nSize)
-{
-    if(nSize > mnBufSize)
-    {
-        qDebug("procsm::writemsg message size is very big");
-        return -1;
-    }
-    if(mbAttach == false)
-    {
-        std::cout<<"ShareMemory Attach fail."<<std::endl;
-        return -1;
-    }
-    mpASM->lock();
-
-
-
-//    unsigned int * pIndexFirst = (unsigned int *)mpASM->data();
-//    unsigned int * pIndexNext = pIndexFirst+1;
-    if(mpinfo->mLock == 1)
-    {
-        std::cout<<"ShareMemory have lock.Init."<<std::endl;
-        mpinfo->mLock = 0;
-        mpinfo->mFirst = 0;
-        mpinfo->mNext = 0;
-    }
-    mpinfo->mLock =1;
-WRITEMSG:
-    char * pH,*pD;
-    QDateTime dt;
-    pH = (char *)mpASM->data();pH = pH + sizeof(procsm_info);
-    pD = (char *)mpASM->data();pD = pD + sizeof(procsm_info) + mnMaxPacCount * sizeof(procsm_head);
-    procsm_head * phh = (procsm_head *)pH;
-    unsigned int nPac = mpinfo->mNext - mpinfo->mFirst;
-    if(nPac>=mnMaxPacCount)
-    {
-        unsigned int nRemove = mnMaxPacCount/3;
-        if(nRemove == 0)nRemove = 1;
-        MoveMem(nRemove);
-        goto WRITEMSG;
-    }
-    if(nPac == 0)
-    {
-        memcpy(pD,str,nSize);
-        dt = QDateTime::currentDateTime();
-    //    phh->mdt = dt;
-        phh->SetDate(dt);
-   //     memcpy(&phh->mdt,&dt,sizeof(QDateTime));
-   //     phh->mdt = QDateTime::currentDateTime();
-        phh->mindex = mpinfo->mNext;
-        phh->mnPos = 0;
-        phh->mnLen = nSize;
-        mpinfo->mNext = mpinfo->mNext+1;
-    }
-    else
-    {
-        if(((phh+nPac-1)->mnPos+(phh+nPac-1)->mnLen + nSize)>=mnBufSize)
-        {
-            unsigned int nRemove = mnMaxPacCount/2;
-            if(nRemove == 0)nRemove = 1;
-            MoveMem(nRemove);
-            goto WRITEMSG;
-        }
-        else
-        {
-            unsigned int nPos = (phh+nPac-1)->mnPos + (phh+nPac-1)->mnLen;
-     //       qDebug("write pos = %d",nPos);
-            memcpy(pD+nPos,str,nSize);
-            dt = QDateTime::currentDateTime();
-            (phh+nPac)->SetDate(dt);
- //           memcpy(&(phh+nPac)->mdt,&dt,sizeof(QDateTime));
- //           (phh+nPac)->mdt = QDateTime::currentDateTime();
-            (phh+nPac)->mindex = mpinfo->mNext;
-            (phh+nPac)->mnPos = nPos;
-            (phh+nPac)->mnLen = nSize;
-            mpinfo->mNext = mpinfo->mNext+1;
-        }
-    }
-
-    const unsigned int nTM = 0x6fffffff;
-    if((mpinfo->mNext >nTM)&&(mpinfo->mFirst>nTM))
-    {
-       nPac = mpinfo->mNext - mpinfo->mFirst;
-       unsigned int i;
-       for(i=0;i<nPac;i++)
-       {
-           (phh+i)->mindex = (phh+i)->mindex-nTM;
-       }
-       mpinfo->mFirst = mpinfo->mFirst-nTM;
-       mpinfo->mNext = mpinfo->mNext - nTM;
-    }
-
-    mpinfo->mLock = 0;
-    mpASM->unlock();
-#ifdef USEDBUS
-    QDBusConnection::sessionBus().send(mmsg);
-#endif
-    return 0;
-}
-
-unsigned int procsm::getcurrentnext()
-{
-
-
-    unsigned int nNext;
-    mpASM->lock();
-    nNext = mpinfo->mNext;
-    mpASM->unlock();
-    return nNext;
-}
-
-
-//if return 0 No Data.
-//if return -1 nMaxSize is small
-//if retrun -2 index is not in range,call getcurrentnext get position
-//if return > 0 readdata
-int procsm::readmsg(unsigned int index, char *str, unsigned int nMaxSize,unsigned int * nRead,QDateTime * pdt)
-{
-    if(mbAttach == false)
-    {
-        std::cout<<"ShareMemory Attach fail."<<std::endl;
-        return -1;
-    }
-    int nRtn = 0;
-    mpASM->lock();
-
-    if((index< mpinfo->mFirst)||(index > mpinfo->mNext))
-    {
-        nRtn = -2;
-    }
-    if(nRtn != (-2))
-    {
-        if(index == mpinfo->mNext)
-        {
-            nRtn = 0;
-        }
-        else
-        {
-            char * pH,*pD;
- //           pH = (char *)mpASM->data();pH = pH + 2*sizeof(unsigned int);
-
- //           pD = (char *)mpASM->data();pD = pD + 2*sizeof(unsigned int) + mnMaxPacCount * sizeof(procsm_head);
-            pD = (char *)mpASM->data();pD = pD+ sizeof(procsm_info) + mpinfo->mCap*sizeof(procsm_head);
-            pH = (char *)mpASM->data();pH = pH+sizeof(procsm_info);
-            procsm_head * phh = (procsm_head *)pH;
-            unsigned int nPac = mpinfo->mNext - mpinfo->mFirst;
-            if(nPac == 0)
-            {
-                nRtn = 0;
-            }
-            else
-            {
-                unsigned int nPos = index - mpinfo->mFirst;
-                *nRead = (phh+nPos)->mnLen;
-                if((phh+nPos)->mnLen > nMaxSize)
-                {
-                    nRtn = -1;
-
-                }
-                else
-                {
-        //            qDebug("read pos = %d",(phh+nPos)->mnPos);
-                   memcpy(str,pD + (phh+nPos)->mnPos,(phh+nPos)->mnLen);
-          //         qDebug("read pos = %d",(phh+nPos)->mnPos);
-                   nRtn = (phh+nPos)->mnLen;
-                   (phh+nPos)->GetDate(pdt);
-           //        memcpy(pdt,&((phh+nPos)->mdt),sizeof(QDateTime));
-                }
-            }
-        }
-    }
-    mpASM->unlock();
-    return nRtn;
-}
-

src1/common/modulecomm_shm/procsm.h

@@ -1,112 +0,0 @@
-#ifndef PROCSM_H
-#define PROCSM_H
-
-#include <QThread>
-#include <QSharedMemory>
-#include <QDateTime>
-#include <QList>
-#include <QVariant>
-
-#ifdef USEDBUS
-#include <QtDBus/QDBusMessage>
-#include <QtDBus/QDBusConnection>
-
-#endif
-
-#include "ivmodulemsg_type.h"
-
-class procsm_info
-{
-public:
-  unsigned int mFirst;
-  unsigned int mNext;
-  unsigned int mCap;
-  unsigned int mLock;
-  unsigned int mnBufSize;
-};
-
-class procsm_head
-{
-public:
-    unsigned short mYear;
-    unsigned char mMonth;
-    unsigned char mDay;
-    unsigned char mHour;
-    unsigned char mMinute;
-    unsigned char mSec;
-    unsigned short mMSec;
-    unsigned int mindex;
-    unsigned int mnPos;
-    unsigned int mnLen;
-public:
-    void SetDate(QDateTime dt)
-    {
-        mYear = dt.date().year();
-        mMonth = dt.date().month();
-        mDay = dt.date().day();
-        mHour = dt.time().hour();
-        mMinute = dt.time().minute();
-        mSec = dt.time().second();
-        mMSec = dt.time().msec();
-    }
-    void GetDate(QDateTime * pdt)
-    {
-        QDate dt;
-        dt.setDate(mYear,mMonth,mDay);
-        QTime time;
-        time.setHMS(mHour,mMinute,mSec,mMSec);
-        pdt->setDate(dt);
-        pdt->setTime(time);
-
-    }
-};
-
-class procsm : public QObject
-{
-#ifdef USEDBUS
-    Q_OBJECT
-
-#endif
-public:
-    procsm(const char * strsmname,const unsigned int nBufSize,const unsigned int nMaxPacCount,const int nMode);
-    int writemsg(const char * str,const unsigned int nSize);
-    unsigned int getcurrentnext();
-    int readmsg(unsigned int index,char * str,unsigned int nMaxSize,unsigned int * nRead,QDateTime * pdt);
-
-    bool AttachMem();
-
-private:
-    int MoveMem(const unsigned int nSize);
-    QSharedMemory * mpASM;
-    unsigned int mnBufSize;
-    unsigned int mnMaxPacCount;
-    procsm_info * mpinfo;
-    procsm_head * mphead;
-
-    bool mbAttach;
-
-    char * mstrtem;
-
-public:
-    const static int ModeRead = 1;
-    const static int ModeWrite = 0;
-
-    iv::modulemsg_type mmodulemsg_type;
-
-#ifdef USEDBUS
-private slots:
-    void onQuery();
-
-#endif
-private:
-#ifdef USEDBUS
-    QDBusMessage mmsg;
-    QDBusMessage mmsgres;  //Response Message Query;
-
-#endif
-
-
-
-};
-
-#endif // PROCSM_H

src1/common/modulecomm_shm/procsm_if.cpp

@@ -1,310 +0,0 @@
-#include "procsm_if.h"
-#include <QTimer>
-
-#include <iostream>
-
-
-
-procsm_if_readthread::procsm_if_readthread(procsm *pPSM,SMCallBack pCall,const char * strsmname)
-{
-    mpPSM = pPSM;
-    mpCall = pCall;
-    strncpy(mstrsmname,strsmname,255);
-
-#ifdef USEDBUS
-    bool bconnect = QDBusConnection::sessionBus().connect(QString(),"/catarc/adc",  "adc.adciv.modulecomm", strsmname,this,SLOT(onNewMsg(int)));
-    if(bconnect == false)
-    {
-        std::cout<<"procsm_if_readthread::procsm_if_readthread bconect is false"<<std::endl;
-    }
-#endif
-}
-
-procsm_if_readthread::procsm_if_readthread(procsm *pPSM,ModuleFun xFun,const char * strsmname)
-{
-    mpPSM = pPSM;
-    mFun = xFun;
-    strncpy(mstrsmname,strsmname,255);
-    mbFunPlus = true;
-
-#ifdef USEDBUS
-    bool bconnect = QDBusConnection::sessionBus().connect(QString(),"/catarc/adc",  "adc.adciv.modulecomm", strsmname,this,SLOT(onNewMsg(int)));
-    if(bconnect == false)
-    {
-        std::cout<<"procsm_if_readthread::procsm_if_readthread bconect is false"<<std::endl;
-        mbDBUSOK = false;
-        QTimer * timer = new QTimer();
-        timer->setTimerType(Qt::PreciseTimer);
-        delete timer;
-    }
-#endif
-}
-
-
-#ifdef USELCM
-    void procsm_if_readthread::handlerMethod(const lcm::ReceiveBuffer *rbuf, const std::string &channel)
-    {
-        qDebug("lcm receiv data. ");
-        mxindex++;
-        QDateTime dt = QDateTime::currentDateTime();
-        if(mbFunPlus)
-        {
-            mFun((char *)rbuf->data,rbuf->data_size,mxindex,&dt,mstrsmname);
-        }
-        else
-        {
-           (*mpCall)((char *)rbuf->data,rbuf->data_size,mxindex,&dt,mstrsmname);
-        }
-    }
-#endif
-
-
-void procsm_if_readthread::puaseread()
-{
-    mbRun = false;
-}
-
-void procsm_if_readthread::continueread()
-{
-    mbRun = true;
-}
-
-void procsm_if_readthread::run()
-{
-#ifdef USELCM
-    mlcm.subscribe(mstrsmname,&procsm_if_readthread::handlerMethod,this);
-    while(!QThread::isInterruptionRequested())
-    {
-        mlcm.handle();
-    }
-    return;
-#endif
-    QTime xTime;
-    xTime.start();
-    unsigned int nBufLen = 1;
-    unsigned int nRead;
-    char * str = new char[nBufLen];
-    unsigned int index =0;
-
-
-    QDateTime *pdt = new QDateTime();
-
-    bool bAttach = false;
-    while(!QThread::isInterruptionRequested())
-    {
-        if(mbRun == false)
-        {
-            msleep(10);
-            continue;
-        }
-        if(bAttach == false)
-        {
-            bAttach = mpPSM->AttachMem();
-            if(bAttach == false)
-            {
-                msleep(1);
-                continue;
-            }
-            else
-            {
-                index = mpPSM->getcurrentnext();
-            }
-        }
-
-        int nRtn = mpPSM->readmsg(index,str,nBufLen,&nRead,pdt);
-        if(nRtn == 0)
-        {
-#ifdef USEDBUS
-            if(mbDBUSOK == true)
-            {
-                mWaitMutex.lock();
-                mwc.wait(&mWaitMutex,10);
-                mWaitMutex.unlock();
-            }
-            else
-            {
-                msleep(1);
-            }
-#else
-            msleep(1);
-#endif
-        }
-        else
-        {
-            if(nRtn == -1)
-            {
-                nBufLen = nRead;
-                delete str;
-                if(nBufLen < 1)nBufLen = 1;
-                str = new char[nBufLen];
-            }
-            else
-            {
-                if(nRtn == -2)
-                {
-                   index = mpPSM->getcurrentnext();
-                }
-                else
-                {
-                   if(nRtn >0)
-                   {
-                       if(mbFunPlus)
-                       {
-                           mFun(str,nRtn,index,pdt,mstrsmname);
-                       }
-                       else
-                       {
-                          (*mpCall)(str,nRtn,index,pdt,mstrsmname);
-                       }
-                       index++;
-                   }
-                   else
-                   {
-                       usleep(100);
-                   }
-                }
-            }
-        }
-
-    }
-    delete str;
-    delete pdt;
-//    qDebug("Thread finish.");
-}
-
-#ifdef USEDBUS
-
-void procsm_if_readthread::onNewMsg(int x)
-{
-    if(x == 100)std::cout<<x<<std::endl;
-    mwc.wakeAll();
-//    qDebug("wake");
-}
-
-#endif
-
-procsm_if::procsm_if(const char * strsmname,const unsigned int nBufSize,const unsigned int nMaxPacCount,const int nMode)
-{
-    strncpy(mstrsmname,strsmname,255);
-#ifdef USELCM
-    if(nMode == procsm::ModeWrite)
-    {
-
-    }
-    else
-    {
-
-    }
-    return;
-#endif
-    mpPSM = new procsm(strsmname,nBufSize,nMaxPacCount,nMode);
-    mnType = nMode;
-
-    mTimer.setTimerType(Qt::PreciseTimer);
-
-}
-
-procsm_if::~procsm_if()
-{
-    if(mnType == procsm::ModeRead)
-    {
-
-        mpReadThread->requestInterruption();
-        while(!mpReadThread->isFinished())
-        {
-
-        }
-        delete mpReadThread;
-    }
-    delete mpPSM;
-}
-
-
-
-int procsm_if::writemsg(const char *str, const unsigned int nSize)
-{
-    if(mbRun == false)return -2;
-#ifdef USELCM
-    int nres = mlcm.publish(mstrsmname,str,nSize);
-    qDebug("publish message. res = %d",nres);
-    return 0;
-#endif
-    if(mnType == procsm::ModeRead)return -1; //this is listen.
-    return mpPSM->writemsg(str,nSize);
-}
-
-#ifdef USELCM
-    void procsm_if::handlerMethod(const lcm::ReceiveBuffer *rbuf, const std::string &channel)
-    {
-        qDebug("receiv data. ");
-    }
-#endif
-int procsm_if::listenmsg(SMCallBack pCall)
-{
-//#ifdef USELCM
-////     mlcm.subscribe(mstrsmname,&handlerMethod2);
-//    mlcm.subscribe(mstrsmname,&procsm_if::handlerMethod,this);
-//    while(true)
-//    {
-//        mlcm.handle();
-//    }
-//    return 0;
-//#endif
-    if(mnType == procsm::ModeWrite)return -1; //listening.
-    mpReadThread = new procsm_if_readthread(mpPSM,pCall,mstrsmname);
-//    mpReadThread->setPriority(QThread::TimeCriticalPriority);
-//    mpReadThread->start();
-    mpReadThread->start(QThread::HighestPriority);
-//    mnType = 1;
-    return 0;
-}
-
-int procsm_if::listenmsg(ModuleFun xFun)
-{
-//#ifdef USELCM
-//    mlcm.subscribe(mstrsmname,&procsm_if::handlerMethod,this);
-//    while(true)
-//    {
-//        mlcm.handle();
-//    }
-//    return 0;
-//#endif
-    if(mnType == procsm::ModeWrite)return -1; //listening.
-    mpReadThread = new procsm_if_readthread(mpPSM,xFun,mstrsmname);
-//    mpReadThread->setPriority(QThread::TimeCriticalPriority);
-//    mpReadThread->start();
-    mpReadThread->start(QThread::HighestPriority);
-//    mnType = 1;
-    return 0;
-}
-
-void procsm_if::stoplisten()
-{
-    if(mnType != 1)return;
-    mpReadThread->requestInterruption();
-    while(!mpReadThread->isFinished());
-    mnType = 0;
-//    mpReadThread->deleteLater();
-    qDebug("stop listen ok");
-}
-
-void procsm_if::pausecomm()
-{
-    mbRun = false;
-    if(mnType == procsm::ModeRead)
-    {
-        mpReadThread->puaseread();
-    }
-}
-
-void procsm_if::continuecomm()
-{
-    mbRun = true;
-    if(mnType == procsm::ModeRead)
-    {
-        mpReadThread->continueread();
-    }
-}
-
-
-

src1/common/modulecomm_shm/procsm_if.h

@@ -1,99 +0,0 @@
-#ifndef PROCSM_IF_H
-#define PROCSM_IF_H
-
-#include <QThread>
-#include <QTimer>
-#include <QWaitCondition>
-#include <QMutex>
-#include <functional>
-
-#ifdef USEDBUS
-#include <QtDBus/QDBusMessage>
-#include <QtDBus/QDBusConnection>
-
-#endif
-
-#include "procsm.h"
-
-typedef void (* SMCallBack)(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname);
-using namespace std::placeholders;
-typedef std::function<void(const char * ,const unsigned int , const unsigned int , QDateTime * ,const char *)> ModuleFun;
-
-class procsm_if_readthread:public QThread
-{
-    Q_OBJECT
-public:
-#ifdef USELCM
-
-#endif
-    procsm_if_readthread(procsm * pPSM,SMCallBack pCall,const char * strsmname);
-    procsm_if_readthread(procsm * pPSM,ModuleFun xFun,const char * strsmname);
-
-    void puaseread();
-    void continueread();
-private slots:
-#ifdef USEDBUS
-    void onNewMsg(int x);
-#endif
-private:
-
-#ifdef USELCM
-    lcm::LCM mlcm;
-    void handlerMethod(const lcm::ReceiveBuffer *rbuf,const std::string &channel);
-    int mxindex = 0;
-#endif
-    void run();
-    procsm * mpPSM;
-    SMCallBack  mpCall;
-    ModuleFun mFun;
-    char mstrsmname[256];
-
-    QWaitCondition mwc;
-
-    QMutex mWaitMutex;
-    bool mbFunPlus = false;
-    bool mbRun = true;
-    bool mbDBUSOK = true;
-};
-
-class procsm_if
-{
-
-
-public:
-    procsm_if(const char * strsmname,const unsigned int nBufSize,const unsigned int nMaxPacCount,const int nMode);
-    ~procsm_if();
-
-    int writemsg(const char * str,const unsigned int nSize);
-    int listenmsg(SMCallBack pCall);
-    int listenmsg(ModuleFun xFun);
-    void stoplisten();
-
-    void pausecomm();
-    void continuecomm();
-
-
-private:
-    procsm * mpPSM;
-    int mnType;
-    procsm_if_readthread * mpReadThread;
-    QTimer mTimer;
-    char mstrsmname[256];
-
-    bool mbRun = true;
-
-
-
-#ifdef USELCM
-    lcm::LCM mlcm;
-    void handlerMethod(const lcm::ReceiveBuffer *rbuf,const std::string &channel);
-#endif
-
-
-
-
-
-
-};
-
-#endif // PROCSM_IF_H

src1/decision/common/adc_adapter/base_adapter.h

@@ -1,44 +0,0 @@
-#ifndef BASE_ADAPTER_H
-#define BASE_ADAPTER_H
-
-
-
-#include <gps_type.h>
-#include <decition_type.h>
-#include <common/obstacle_type.h>
-#include <vector>
-#include <gnss_coordinate_convert.h>
-
-namespace iv {
-namespace decition {
-
-        class BaseAdapter {
-        public:
-
-            int adapter_id;
-            std::string adapter_name;
-
-            BaseAdapter();
-            ~BaseAdapter();
-
-
-
-        virtual  iv::decition::Decition getAdapterDeciton(GPS_INS now_gps_ins, std::vector<Point2D>  trace , float dSpeed, float obsDistacne ,
-                                                          float  obsSpeed,float accAim,float accNow, bool changingDangWei,Decition *decition);
-
-
-        public:
-            float minDecelerate = -100;
-        private:
-
-
-        };
-
-    }
-}
-
-
-
-
-
-#endif // BASE_ADAPTER_H

src1/decision/common/adc_adapter/bus_adapter.cpp

@@ -1,330 +0,0 @@
-#include <adc_adapter/bus_adapter.h>
-#include <common/constants.h>
-#include <common/car_status.h>
-#include <math.h>
-#include <iostream>
-#include <fstream>
-
-
-using namespace std;
-
-
-iv::decition::BusAdapter::BusAdapter(){
-    adapter_id=4;
-    adapter_name="bus";
-}
-iv::decition::BusAdapter::~BusAdapter(){
-
-}
-
-
-
-iv::decition::Decition iv::decition::BusAdapter::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){
-            controlBrake= u*1.5;
-        }
-         if(abs(dSpeed-realSpeed)<3 &&(obsDistance>40 || obsDistance<0)&&ttc>8 && dSpeed>0 && turn_around
-                 && dSpeed>0 && lastBrake==0){
-            controlBrake=0;
-            controlSpeed=0;
-        }else if(abs(realSpeed)<15 &&(obsDistance>40 || obsDistance<0)&&ttc>8 && dSpeed>0 && turn_around){
-            controlBrake=min(controlBrake,0.5f);
-            controlSpeed=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(ServiceCarStatus.torque_st==0){
-            controlSpeed = (u*(-16)+((0-u)-ServiceCarStatus.mfCalAcc)*10)*0.7; //*1.0
-            controlSpeed =min( controlSpeed,2.0f);
-
-        }else{
-            controlSpeed= ServiceCarStatus.torque_st+((0-u)-ServiceCarStatus.mfCalAcc)*20;//1115    *10
-        }
-
-        //        if(((gpsIns.ins_pitch_angle>-2.5 && ServiceCarStatus.daocheMode)||
-        //            (gpsIns.ins_pitch_angle<2.5 && !ServiceCarStatus.daocheMode))){
-        //            if(realSpeed<5  ){
-        //                controlSpeed=min((float)25.0,controlSpeed); //youmenxianxiao  30
-        //            }else if(realSpeed<10){
-        //                controlSpeed=min((float)25.0,controlSpeed);  //40
-        //            }
-        //        }
-
-
-        //   controlSpeed = min((float)(ServiceCarStatus.torque_st+1.0),controlSpeed); //1115 5.0
-
-//        if(accAim>0 && ServiceCarStatus.mfCalAcc>0  && realSpeed>1 ){
-//            if(controlSpeed>0){
-//                controlSpeed = ServiceCarStatus.torque_st;
-//            }
-//        }else if(accAim<0 && ServiceCarStatus.mfCalAcc<0 && dSpeed>0){
-//            if(controlSpeed>0 ){
-//                controlSpeed = ServiceCarStatus.torque_st;
-//            }
-//        }
-
-
-        if(controlSpeed>ServiceCarStatus.torque_st){
-            controlSpeed = min(ServiceCarStatus.torque_st+1.0f,controlSpeed);
-        }else if(controlSpeed<ServiceCarStatus.torque_st){
-            controlSpeed = ServiceCarStatus.torque_st-1.0;
-        }
-
-
-
-
-        //Seizing
-        if((ServiceCarStatus.torque_st==controlSpeed-1)&&dSpeed>1){
-            seizingCount++;
-        }else{
-            seizingCount=0;
-        }
-        if(seizingCount>=10){
-            controlSpeed=ServiceCarStatus.torque_st+2;
-        }
-
-//        if(controlSpeed==2.0 && ServiceCarStatus.torque_st<2){
-//          controlSpeed=2.4;
-//        }
-        //seizing end
-
-
-
-
-
-
-        // 斜坡加大油门   0217 lsn
-
-        if(((now_gps_ins.ins_pitch_angle<-2.5 && ServiceCarStatus.daocheMode)||
-            (now_gps_ins.ins_pitch_angle>2.5 && !ServiceCarStatus.daocheMode))
-                && abs(realSpeed)<1.0){
-            controlSpeed=max((float)20.0,controlSpeed);
-            controlSpeed=min((float)40.0,controlSpeed);
-        }
-        // 斜坡加大油门  end
-
-
-        else if(((now_gps_ins.ins_pitch_angle<-2.5 && ServiceCarStatus.daocheMode)||
-                 (now_gps_ins.ins_pitch_angle>2.5 && !ServiceCarStatus.daocheMode))
-                && abs(realSpeed)<10.0){
-            controlSpeed=min((float)25.0,controlSpeed); //youmenxianxiao  30
-        }
-
-    }
-
-    if(controlSpeed>0){
-        controlSpeed=max(controlSpeed,2.4f);
-    }
-
-    //0227 10m nei xianzhi shache
-    if(obsDistance<10 &&obsDistance>0){
-        controlSpeed=0;
-        controlBrake=max(controlBrake,0.8f);
-    }
-
-
-//    if(DecideGps00::minDecelerate<0){
-//        controlBrake = max((0-DecideGps00::minDecelerate),controlBrake);
-//        controlSpeed=0;
-//    }
-
-    if(minDecelerate<0){
-        controlBrake = max((0-minDecelerate),controlBrake);
-        controlSpeed=0;
-    }
-
-    controlBrake=limitBrake(realSpeed,controlBrake,dSpeed,obsDistance,ttc);
-
-    controlSpeed = limitSpeed(realSpeed, controlBrake, dSpeed, controlSpeed);
-
-   // (*decition)->brake = controlBrake*10;
-      (*decition)->brake = controlBrake*9;
-
-    (*decition)->torque=controlSpeed;
-      lastBrake= (*decition)->brake;
-
-
-
-
-
-    lastTorque=(*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)->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 ;
-    return *decition;
-}
-
-
-
-float iv::decition::BusAdapter::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::BusAdapter::limitSpeed(float realSpeed, float controlBrake,float dSpeed,float controlSpeed ){
-
-    controlSpeed=min((float)100.0,controlSpeed);
-    controlSpeed=max((float)0.0,controlSpeed);
-    return controlSpeed;
-}
-
-

src1/decision/common/adc_adapter/bus_adapter.h

@@ -1,43 +0,0 @@
-#ifndef BUS_ADAPTER_H
-#define BUS_ADAPTER_H
-
-
-#include <gps_type.h>
-#include <decition_type.h>
-#include <common/obstacle_type.h>
-#include <vector>
-#include <gnss_coordinate_convert.h>
-#include <adc_adapter/base_adapter.h>
-#include <adc_tools/transfer.h>
-//#include <decide_gps_00.h>
-
-
-namespace iv {
-     namespace decition {
-         class BusAdapter: public BaseAdapter {
-                    public:
-
-                        float lastTorque;
-                        float lastBrake;
-                        int lastDangWei=0;
-
-
-                        BusAdapter();
-                        ~BusAdapter();
-
-                        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 // BUS_ADAPTER_H

src1/decision/common/adc_adapter/ge3_adapter.cpp

@@ -1,199 +0,0 @@
-#include <adc_adapter/ge3_adapter.h>
-#include <constants.h>
-#include <common/car_status.h>
-#include <math.h>
-#include <iostream>
-#include <fstream>
-
-
-using namespace std;
-
-
-iv::decition::Ge3Adapter::Ge3Adapter(){
-    adapter_id=0;
-    adapter_name="ge3";
-}
-iv::decition::Ge3Adapter::~Ge3Adapter(){
-
-}
-
-
-
-iv::decition::Decition iv::decition::Ge3Adapter::getAdapterDeciton(GPS_INS now_gps_ins, std::vector<Point2D>  trace , float dSpeed, float obsDistance ,
-                                                                   float  obsSpeed,float accAim,float accNow  ,bool changingDangWei,Decition *decition){
-
-
-    float controlSpeed=0;
-    float controlBrake=0;
-    float realSpeed = now_gps_ins.speed;
-    float ttc = 0-(obsDistance/obsSpeed);
-    if(ttc<0){
-        ttc=15;
-    }
-    if (accAim < 0)
-    {
-        if(obsDistance<10 && obsDistance>0 ){
-            if(ttc>3){
-                controlBrake = (int)((0-accAim) * 40.0) + 1;  //(u * 14.0) + 1;
-            }else{
-                controlBrake = (int)((0-accAim) * 60.0) + 1;  //(u * 14.0) + 1;
-            }
-        } else{
-            if(ttc<5){
-                controlBrake = (int)((0-accAim) * 40.0) + 1;  //(u * 14.0) + 1;
-            }else {
-                controlBrake = (int)((0-accAim) * 20.0) + 1;  //(u * 14.0) + 1;
-            }
-        }
-        controlSpeed = 0;
-        if(obsDistance>50 && ttc>8 &&abs(realSpeed-dSpeed)<3){
-            controlBrake=0;
-            controlSpeed=max(lastTorque-10.0,0.0);
-        }
-    }
-    else
-    {
-        controlBrake = 0;
-        if(lastTorque==0){
-            controlSpeed=100;
-        }else{
-            controlSpeed = lastTorque+(accAim-accNow)*500*0.1;
-        }
-    }
-
-
-
-    if(ServiceCarStatus.bocheMode){
-        if(dSpeed<6){
-            controlSpeed=min(1.0f,controlSpeed);
-            controlBrake=min(5.0f,controlBrake);
-            if(abs(dSpeed-realSpeed)<2 && controlBrake>0){
-                controlBrake=0;
-            }
-        }
-    }
-
-
-    controlBrake=limitBrake(realSpeed,controlBrake,dSpeed,obsDistance,ttc);
-
-    controlSpeed = limitSpeed(realSpeed, controlBrake, dSpeed, controlSpeed);
-
-//    if(DecideGps00::minDecelerate<0){
-//        controlBrake = max((0-DecideGps00::minDecelerate)*30,controlBrake);
-//        controlSpeed=0;
-//    }
-
-    if(minDecelerate<0){
-        controlBrake = max((0-minDecelerate)*30,controlBrake);
-        controlSpeed=0;
-    }
-
-    controlBrake=min(controlBrake,100.0f);
-    controlBrake=max(controlBrake,0.0f);
-
-    (*decition)->brake = controlBrake;
-
-    (*decition)->torque= controlSpeed;
-
-
-
-
-
-
-    lastTorque=(*decition)->torque;
-
-
-    (*decition)->grade=1;
-    (*decition)->mode=1;
-    (*decition)->speak=0;
-    (*decition)->handBrake=0;
-    (*decition)->bright=false;
-    (*decition)->engine=0;
-
-    if(ServiceCarStatus.bocheMode){
-        (*decition)->dangWei=2;
-    }else{
-        (*decition)->dangWei=1;
-    }
-
-
-    (*decition)->wheel_angle=max((float)-500.0,float((*decition)->wheel_angle+ServiceCarStatus.mfEPSOff));
-    (*decition)->wheel_angle=min((float)500.0,(*decition)->wheel_angle);
-
-    (*decition)->air_enable=true;
-
-
-
-    return *decition;
-}
-
-
-
-float iv::decition::Ge3Adapter::limitBrake(float realSpeed, float controlBrake,float dSpeed,float obsDistance , float ttc){
-
-    //刹车限制
-
-    int BrakeIntMax = 0;
-    if (realSpeed < 10.0 / 3.6) BrakeIntMax = 40;
-    else if (realSpeed < 20.0 / 3.6) BrakeIntMax = 60;
-    else BrakeIntMax = 100;
-
-    if (controlBrake > BrakeIntMax) controlBrake = BrakeIntMax;
-
-    if ((obsDistance>0 && obsDistance < 6) || dSpeed == 0)
-    {
-        controlBrake = max(30.0f, controlBrake);
-    }
-    if (obsDistance>0 && obsDistance<10&&obsDistance>0)
-    {
-        controlBrake = max(20.0f, controlBrake);
-    }
-    if (obsDistance<10&&obsDistance>0 &&ttc<8)
-    {
-        controlBrake = max(30.0f, controlBrake);
-    }
-    if (obsDistance<10&&obsDistance>0 &&ttc<5)
-    {
-        controlBrake = max(40.0f, controlBrake);
-    }
-    if (obsDistance<10&&obsDistance>0 &&ttc<1.6)
-    {
-        controlBrake = max(60.0f, controlBrake);
-    }
-    if(obsDistance<5 && obsDistance>0 ){
-        controlBrake = max(60.0f, controlBrake);
-    }
-    if(obsDistance<5 && obsDistance>0 &&ttc<8){
-        controlBrake = max(80.0f, controlBrake);
-    }
-    controlBrake=min(100.0f,controlBrake);
-    return controlBrake;
-
-}
-
-float iv::decition::Ge3Adapter::limitSpeed(float realSpeed, float controlBrake,float dSpeed,float controlSpeed ){
-    if(controlBrake>0){
-        controlSpeed=0;
-    }
-
-    if(realSpeed<10){
-        controlSpeed=min((float)400.0,controlSpeed);
-    }else if(realSpeed<30){
-        controlSpeed =min((float)600.0,controlSpeed);
-    }
-    if(controlSpeed>0){
-        if(controlSpeed>lastTorque){
-            controlSpeed=min(float(lastTorque+5.0),controlSpeed);
-        }
-    }
-    if(dSpeed <= 3)
-    {
-        controlSpeed = min(controlSpeed,20.0f);
-    }
-
-    controlSpeed=min((float)1200.0,controlSpeed);
-    controlSpeed=max((float)0.0,controlSpeed);
-    return controlSpeed;
-}
-
-

src1/decision/common/adc_adapter/ge3_adapter.h

@@ -1,60 +0,0 @@
-#ifndef GE3_ADAPTER_H
-#define GE3_ADAPTER_H
-
-
-
-#include <gps_type.h>
-#include <decition_type.h>
-#include <common/obstacle_type.h>
-#include <vector>
-#include <gnss_coordinate_convert.h>
-#include <adc_adapter/base_adapter.h>
-#include <adc_tools/transfer.h>
-//#include <decision/decide_gps_00.h>
-
-
-namespace iv {
-     namespace decition {
-         class Ge3Adapter: public BaseAdapter {
-                    public:
-
-                        float lastTorque;
-                        float lastBrake;
-
-                        Ge3Adapter();
-                        ~Ge3Adapter();
-
-                        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:
-
-            };
-
-    }
-}
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-#endif // GE3_ADAPTER_H

src1/decision/common/adc_adapter/hapo_adapter.cpp

@@ -1,367 +0,0 @@
-#include <adc_adapter/hapo_adapter.h>
-#include <common/constants.h>
-#include <common/car_status.h>
-#include <math.h>
-#include <iostream>
-#include <fstream>
-#include <QTime>
-
-QTime doorOpenTime;
-
-using namespace std;
-
-iv::decition::HapoAdapter::HapoAdapter(){
-    adapter_id=5;
-    adapter_name="hapo";
-}
-iv::decition::HapoAdapter::~HapoAdapter(){
-
-}
-
-#include "ivlog.h"
-extern iv::Ivlog * givlog;
-
-iv::decition::Decition iv::decition::HapoAdapter::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<6){
-            controlBrake= u*1.5;
-        }
-         if(abs(dSpeed-realSpeed)<3 &&(obsDistance>40 || obsDistance<0)&&ttc>5 && dSpeed>0 && turn_around
-                 && dSpeed>0 && lastBrake==0){
-            controlBrake=0;
-            controlSpeed=0;
-        }else if(abs(realSpeed)<15 &&(obsDistance>40 || obsDistance<0)&&ttc>5 && dSpeed>0 && turn_around){
-            controlBrake=min(controlBrake,0.5f);
-            controlSpeed=0;
-        }
-
-       else if(abs(dSpeed-realSpeed)>=5 &&abs(dSpeed-realSpeed)<10&&(obsDistance>40 || obsDistance<0)&&ttc>5
-                  && !turn_around ){
-            controlBrake=min(controlBrake,0.3f);
-            controlSpeed=0;
-        }
-        else if(abs(dSpeed-realSpeed)>=10 &&abs(dSpeed-realSpeed)<15&&(obsDistance>40 || obsDistance<0)&&ttc>5
-                && dSpeed>0 && !turn_around ){
-            controlBrake=min(controlBrake,0.5f);
-            controlSpeed=0;
-        }
-         else if(abs(dSpeed-realSpeed)<3 &&(obsDistance>40 || obsDistance<0)&&ttc>=15
-                 && dSpeed>0 && !turn_around ){
-             controlBrake=0;
-             controlSpeed= 2.0* (realSpeed/15.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(ServiceCarStatus.torque_st==0){
-            controlSpeed = (u*(-16)+((0-u)-ServiceCarStatus.mfCalAcc)*10)*0.7; //*1.0
-            controlSpeed =min( controlSpeed,2.0f);
-             controlSpeed =max( controlSpeed,1.0f);
-
-        }else{
-            controlSpeed= ServiceCarStatus.torque_st+((0-u)-ServiceCarStatus.mfCalAcc)*20;//1115    *10
-        }
-
-        //        if(((gpsIns.ins_pitch_angle>-2.5 && ServiceCarStatus.daocheMode)||
-        //            (gpsIns.ins_pitch_angle<2.5 && !ServiceCarStatus.daocheMode))){
-        //            if(realSpeed<5  ){
-        //                controlSpeed=min((float)25.0,controlSpeed); //youmenxianxiao  30
-        //            }else if(realSpeed<10){
-        //                controlSpeed=min((float)25.0,controlSpeed);  //40
-        //            }
-        //        }
-
-
-        //   controlSpeed = min((float)(ServiceCarStatus.torque_st+1.0),controlSpeed); //1115 5.0
-
-//        if(accAim>0 && ServiceCarStatus.mfCalAcc>0  && realSpeed>1 ){
-//            if(controlSpeed>0){
-//                controlSpeed = ServiceCarStatus.torque_st;
-//            }
-//        }else if(accAim<0 && ServiceCarStatus.mfCalAcc<0 && dSpeed>0){
-//            if(controlSpeed>0 ){
-//                controlSpeed = ServiceCarStatus.torque_st;
-//            }
-//        }
-
-
-        if(controlSpeed>ServiceCarStatus.torque_st){
-            controlSpeed = min(ServiceCarStatus.torque_st+2.0f,controlSpeed);
-        }else if(controlSpeed<ServiceCarStatus.torque_st){
-            controlSpeed = ServiceCarStatus.torque_st-1.0f;
-        }
-
-
-
-
-        //Seizing
-        if((ServiceCarStatus.torque_st==controlSpeed-1)&&dSpeed>1){
-            seizingCount++;
-        }else{
-            seizingCount=0;
-        }
-        if(seizingCount>=10){
-            controlSpeed=ServiceCarStatus.torque_st+2;
-        }
-
-       if(controlSpeed==2.0 && ServiceCarStatus.torque_st<2){
-          controlSpeed=2.4;
-        }
-        //seizing end
-
-
-
-
-
-
-        // 斜坡加大油门   0217 lsn
-
-//        if(((now_gps_ins.ins_pitch_angle<-2.5 && ServiceCarStatus.daocheMode)||
-//            (now_gps_ins.ins_pitch_angle>2.5 && !ServiceCarStatus.daocheMode))
-//                && abs(realSpeed)<1.0){
-//            controlSpeed=max((float)20.0,controlSpeed);
-//            controlSpeed=min((float)40.0,controlSpeed);
-//        }
-        // 斜坡加大油门  end
-
-
-//        else if(((now_gps_ins.ins_pitch_angle<-2.5 && ServiceCarStatus.daocheMode)||
-//                 (now_gps_ins.ins_pitch_angle>2.5 && !ServiceCarStatus.daocheMode))
-//                && abs(realSpeed)<10.0){
-//            controlSpeed=min((float)25.0,controlSpeed); //youmenxianxiao  30
-//        }
-
-    }
-//0125
-    if(controlSpeed>0){
-        controlSpeed=max(controlSpeed,2.4f);
-    }
-
-    //0227 10m nei xianzhi shache
-    if(obsDistance<6 &&obsDistance>0){
-        controlSpeed=0;
-        controlBrake=max(controlBrake,0.8f);
-    }
-
-
-//    if(DecideGps00::minDecelerate<0){
-//        controlBrake = max((0-DecideGps00::minDecelerate),controlBrake);
-//        controlSpeed=0;
-//    }
-
-    if(minDecelerate<0){
-        controlBrake = max((0-minDecelerate),controlBrake);
-        controlSpeed=0;
-    }
-
-    controlBrake=limitBrake(realSpeed,controlBrake,dSpeed,obsDistance,ttc);
-
-    controlSpeed = limitSpeed(realSpeed, controlBrake, dSpeed, controlSpeed);
-
-   // (*decition)->brake = controlBrake*10;
-      (*decition)->brake = controlBrake*6;
-
-    (*decition)->torque=controlSpeed;
-      lastBrake= (*decition)->brake;
-
-
-
-
-
-    lastTorque=(*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;
-
-     if(ServiceCarStatus.stationCmd.mode_manual_drive==true)
-     {
-          (*decition)->mode=0;
-     }
-
-
-     if(ServiceCarStatus.station_control_door==0)
-     {
-         (*decition)->door=false;      //CLOSE
-         doorOpenTime.start();
-         givlog->debug("DOOR","STATUS is: %d",5);
-
-     }else if((ServiceCarStatus.station_control_door==1)&&(ServiceCarStatus.station_control_door_option==false))
-     {
-         (*decition)->door=true;       //OPEN
-         (*decition)->brake =6;
-         (*decition)->torque=0;
-         if(doorOpenTime.elapsed()>6000)
-         {
-             ServiceCarStatus.station_control_door_option=true;
-             givlog->debug("DOOR","STATUS is: %d",8);
-         }
-     }
-
-givlog->debug("DOOR","door is: %d",(*decition)->door);
-givlog->debug("DOOR","station_control_door is: %d",ServiceCarStatus.station_control_door);
-
-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 ;
-
-
-givlog->debug("obs_distance","dSpeed: %f, realSpeed: %f, brake: %f",
-              dSpeed, realSpeed,(*decition)->brake);
-
-    return *decition;
-}
-
-
-
-float iv::decition::HapoAdapter::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::HapoAdapter::limitSpeed(float realSpeed, float controlBrake,float dSpeed,float controlSpeed ){
-
-    controlSpeed=min((float)100.0,controlSpeed);
-    controlSpeed=max((float)0.0,controlSpeed);
-    return controlSpeed;
-}
-
-

src1/decision/common/adc_adapter/hapo_adapter.h

@@ -1,43 +0,0 @@
-#ifndef HAPO_ADAPTER_H
-#define HAPO_ADAPTER_H
-
-
-#include <gps_type.h>
-#include <decition_type.h>
-#include <common/obstacle_type.h>
-#include <vector>
-#include <gnss_coordinate_convert.h>
-#include <adc_adapter/base_adapter.h>
-#include <adc_tools/transfer.h>
-//#include <decision/decide_gps_00.h>
-
-
-namespace iv {
-     namespace decition {
-         class HapoAdapter: public BaseAdapter {
-                    public:
-
-                        float lastTorque;
-                        float lastBrake;
-                        int lastDangWei=0;
-
-
-                        HapoAdapter();
-                        ~HapoAdapter();
-
-                        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 // HAPO_ADAPTER_H

src1/decision/common/adc_adapter/qingyuan_adapter.cpp

@@ -1,317 +0,0 @@
-#include <adc_adapter/qingyuan_adapter.h>
-#include <common/constants.h>
-#include <common/car_status.h>
-#include <math.h>
-#include <iostream>
-#include <fstream>
-
-
-using namespace std;
-
-
-iv::decition::QingYuanAdapter::QingYuanAdapter(){
-    adapter_id=1;
-    adapter_name="qingyuan";
-}
-iv::decition::QingYuanAdapter::~QingYuanAdapter(){
-
-}
-
-
-
-iv::decition::Decition iv::decition::QingYuanAdapter::getAdapterDeciton(GPS_INS now_gps_ins, std::vector<Point2D>  trace , float dSpeed, float obsDistance ,
-                                                                        float  obsSpeed,float accAim,float accNow  ,bool changingDangWei,Decition *decition){
-
-
-    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){
-            controlBrake= u*1.5;
-        }
-        if(abs(dSpeed-realSpeed)<3 &&(obsDistance>40 || obsDistance<0)&&ttc>8 && dSpeed>0 && !turn_around){
-            controlBrake=0;
-            controlSpeed=max(0.0,ServiceCarStatus.torque_st-10.0);
-        }else if(abs(dSpeed-realSpeed)<3 &&(obsDistance>40 || obsDistance<0)&&ttc>8 && dSpeed>0 && turn_around){
-            controlBrake=0;
-            controlSpeed=0;
-        }else if(abs(realSpeed)<15 &&(obsDistance>40 || obsDistance<0)&&ttc>8 && dSpeed>0 && turn_around){
-            controlBrake=min(controlBrake,0.5f);
-            controlSpeed=0;
-        }
-
-        else if(abs(dSpeed-realSpeed)>=3 &&abs(dSpeed-realSpeed)<5&&(obsDistance>40 || obsDistance<0)&&ttc>8
-                && dSpeed>0 && !turn_around ){
-            controlBrake=0;
-            controlSpeed=0;
-        }else if(abs(dSpeed-realSpeed)>=5 &&abs(dSpeed-realSpeed)<10&&(obsDistance>40 || obsDistance<0)&&ttc>8
-                 && dSpeed>0 && !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(ServiceCarStatus.torque_st==0){
-            controlSpeed = (u*(-16)+((0-u)-ServiceCarStatus.mfCalAcc)*10)*0.7; //*1.0
-            controlSpeed =min( controlSpeed,1.0f);
-
-        }else{
-            controlSpeed= ServiceCarStatus.torque_st+((0-u)-ServiceCarStatus.mfCalAcc)*10;//1115    *10
-        }
-
-        //        if(((gpsIns.ins_pitch_angle>-2.5 && ServiceCarStatus.daocheMode)||
-        //            (gpsIns.ins_pitch_angle<2.5 && !ServiceCarStatus.daocheMode))){
-        //            if(realSpeed<5  ){
-        //                controlSpeed=min((float)25.0,controlSpeed); //youmenxianxiao  30
-        //            }else if(realSpeed<10){
-        //                controlSpeed=min((float)25.0,controlSpeed);  //40
-        //            }
-        //        }
-
-
-        //   controlSpeed = min((float)(ServiceCarStatus.torque_st+1.0),controlSpeed); //1115 5.0
-        if(controlSpeed>ServiceCarStatus.torque_st){
-            controlSpeed = ServiceCarStatus.torque_st+1.0;
-        }else if(controlSpeed<ServiceCarStatus.torque_st){
-            controlSpeed = ServiceCarStatus.torque_st-1.0;
-        }
-
-
-
-
-
-
-        //Seizing
-        if((ServiceCarStatus.torque_st==controlSpeed-1)&&dSpeed>1&&realSpeed<0.5){
-            seizingCount++;
-        }else{
-            seizingCount=0;
-        }
-        if(seizingCount>=10){
-            controlSpeed=ServiceCarStatus.torque_st+2;
-        }
-        //seizing end
-
-
-
-
-        // 斜坡加大油门   0217 lsn
-
-        if(((now_gps_ins.ins_pitch_angle<-2.5 && ServiceCarStatus.daocheMode)||
-            (now_gps_ins.ins_pitch_angle>2.5 && !ServiceCarStatus.daocheMode))
-                && abs(realSpeed)<1.0){
-            controlSpeed=max((float)20.0,controlSpeed);
-            controlSpeed=min((float)40.0,controlSpeed);
-        }
-        // 斜坡加大油门  end
-
-
-        else if(((now_gps_ins.ins_pitch_angle<-2.5 && ServiceCarStatus.daocheMode)||
-                 (now_gps_ins.ins_pitch_angle>2.5 && !ServiceCarStatus.daocheMode))
-                && abs(realSpeed)<10.0){
-            controlSpeed=min((float)25.0,controlSpeed); //youmenxianxiao  30
-        }
-
-    }
-
-    //0227 10m nei xianzhi shache
-    if(obsDistance<10 &&obsDistance>0){
-        controlSpeed=0;
-        controlBrake=max(controlBrake,0.8f);
-    }
-
-
-//    if(DecideGps00::minDecelerate<0){
-//        controlBrake = max((0-DecideGps00::minDecelerate),controlBrake);
-//        controlSpeed=0;
-//    }
-
-    if(minDecelerate<0){
-        controlBrake = max((0-minDecelerate),controlBrake);
-        controlSpeed=0;
-    }
-
-    controlBrake=limitBrake(realSpeed,controlBrake,dSpeed,obsDistance,ttc);
-
-    controlSpeed = limitSpeed(realSpeed, controlBrake, dSpeed, controlSpeed);
-
-    (*decition)->brake = controlBrake;
-
-    (*decition)->torque=controlSpeed;
-
-
-
-
-
-
-    lastTorque=(*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)->driveMode=1;
-    (*decition)->brakeType=0;
-    (*decition)->angSpeed=60;
-    (*decition)->topLight=0;
-
-
-    if(ServiceCarStatus.bocheMode ||ServiceCarStatus.daocheMode){
-        (*decition)->dangWei=3;
-        (*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)->wheel_angle+=ServiceCarStatus.mfEPSOff;
-
-    (*decition)->wheel_angle=max((float)-570.0,(*decition)->wheel_angle);
-     (*decition)->wheel_angle=min((float)570.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
-    <<std::endl;
-std::cout<<"========================================="<<std::endl;
-
-    return *decition;
-}
-
-
-
-float iv::decition::QingYuanAdapter::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 if(ttc>1.6){
-        BrakeIntMax = 5;
-    }else if(ttc>0.8){
-        BrakeIntMax = 8;
-    }else{
-        BrakeIntMax = 10;
-    }
-    if(obsDistance>0 && obsDistance<10){
-        BrakeIntMax=max(BrakeIntMax,3);
-    }
-
-    if (controlBrake > BrakeIntMax) controlBrake = BrakeIntMax;
-
-
-
-
-    controlBrake=min(10.0f,controlBrake);
-    controlBrake=max(0.0f,controlBrake);
-    return controlBrake;
-
-}
-
-float iv::decition::QingYuanAdapter::limitSpeed(float realSpeed, float controlBrake,float dSpeed,float controlSpeed ){
-
-    controlSpeed=min((float)100.0,controlSpeed);
-    controlSpeed=max((float)0.0,controlSpeed);
-    return controlSpeed;
-}
-
-

src1/decision/common/adc_adapter/qingyuan_adapter.h

@@ -1,41 +0,0 @@
-#ifndef QINGYUAN_ADAPTER_H
-#define QINGYUAN_ADAPTER_H
-
-#include <gps_type.h>
-#include <decition_type.h>
-#include <common/obstacle_type.h>
-#include <vector>
-#include <gnss_coordinate_convert.h>
-#include <adc_adapter/base_adapter.h>
-#include <adc_tools/transfer.h>
-//#include <decide_gps_00.h>
-
-
-namespace iv {
-     namespace decition {
-         class QingYuanAdapter: public BaseAdapter {
-                    public:
-
-                        float lastTorque;
-                        float lastBrake;
-                        int lastDangWei=0;
-                        int seizingCount=0;
-
-                        QingYuanAdapter();
-                        ~QingYuanAdapter();
-
-                        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:
-
-            };
-
-    }
-}
-
-
-#endif // QINGYUAN_ADAPTER_H

src1/decision/common/adc_adapter/vv7_adapter.cpp

@@ -1,156 +0,0 @@
-#include <adc_adapter/vv7_adapter.h>
-#include <common/constants.h>
-#include <common/car_status.h>
-#include <math.h>
-#include <iostream>
-#include <fstream>
-
-
-using namespace std;
-
-
-iv::decition::VV7Adapter::VV7Adapter(){
-    adapter_id= 2;
-    adapter_name="vv7";
-}
-iv::decition::VV7Adapter::~VV7Adapter(){
-
-}
-
-
-
-iv::decition::Decition iv::decition::VV7Adapter::getAdapterDeciton(GPS_INS now_gps_ins, std::vector<Point2D>  trace , float dSpeed, float obsDistance ,
-                                                                        float  obsSpeed,float accAim,float accNow  ,bool changingDangWei,Decition *decition){
-
-
-    float controlSpeed=accAim;
-    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)
-    {
-        //0110
-        if(changingDangWei){
-            controlSpeed=min(-0.5f,controlSpeed);
-        }
-//        else if((obsDistance<0 ||obsDistance>50)&&(abs(dSpeed-realSpeed)<3)){
-//             controlSpeed=max(-0.2f,controlSpeed);
-//        }
-    }
-    else
-    {
-        controlSpeed=min(1.0f,controlSpeed);
-
-    }
-
-    //0227 10m nei xianzhi shache
-    if(obsDistance<10 &&obsDistance>0){
-        controlSpeed=min(controlSpeed,-0.8f);
-    }
-
-
-//    if(DecideGps00::minDecelerate<0){
-//        controlSpeed = min(DecideGps00::minDecelerate, controlSpeed);
-//    }
-
-    if(minDecelerate<0){
-        controlSpeed = min(minDecelerate, controlSpeed);
-    }
-
-
-    controlSpeed = limitSpeed(realSpeed, controlBrake, dSpeed, controlSpeed);
-
-    (*decition)->accelerator=controlSpeed;
-
-    if((*decition)->leftlamp){
-        (*decition)->directLight=1;
-    }else if((*decition)->rightlamp){
-         (*decition)->directLight=2;
-    }else{
-        (*decition)->directLight=0;
-    }
-
-
-    (*decition)->wheel_angle+=ServiceCarStatus.mfEPSOff;
-
-    (*decition)->wheel_angle=max((float)-500.0,(*decition)->wheel_angle);
-     (*decition)->wheel_angle=min((float)500.0,(*decition)->wheel_angle);
-
-    if((*decition)->accelerator>=0){
-        (*decition)->torque= (*decition)->accelerator;
-        (*decition)->brake=0;
-    }else{
-        (*decition)->torque=0;
-        (*decition)->brake=0-(*decition)->accelerator;
-    }
-
-
-
-
-std::cout<<"==========================================="<<std::endl;
-     std::cout<<"dSpeed:"<<dSpeed<<"   realSpeed:"<<realSpeed<<"   acc:"<<accAim
-    <<std::endl;
-std::cout<<"========================================="<<std::endl;
-
-    return *decition;
-}
-
-
-
-float iv::decition::VV7Adapter::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 if(ttc>1.6){
-        BrakeIntMax = 5;
-    }else if(ttc>0.8){
-        BrakeIntMax = 8;
-    }else{
-        BrakeIntMax = 10;
-    }
-    if(obsDistance>0 && obsDistance<10){
-        BrakeIntMax=max(BrakeIntMax,3);
-    }
-
-    if (controlBrake > BrakeIntMax) controlBrake = BrakeIntMax;
-
-
-
-
-    controlBrake=min(10.0f,controlBrake);
-    controlBrake=max(0.0f,controlBrake);
-    return controlBrake;
-
-}
-
-float iv::decition::VV7Adapter::limitSpeed(float realSpeed, float controlBrake,float dSpeed,float controlSpeed ){
-
-    controlSpeed=min((float)5.0,controlSpeed);
-    controlSpeed=max((float)-7.0,controlSpeed);
-    return controlSpeed;
-}
-
-

src1/decision/common/adc_adapter/vv7_adapter.h

@@ -1,44 +0,0 @@
-#ifndef VV7_ADAPTER_H
-#define VV7_ADAPTER_H
-
-#include <gps_type.h>
-#include <decition_type.h>
-#include <common/obstacle_type.h>
-#include <vector>
-#include <gnss_coordinate_convert.h>
-#include <adc_adapter/base_adapter.h>
-#include <adc_tools/transfer.h>
-//#include <decide_gps_00.h>
-
-
-namespace iv {
-     namespace decition {
-         class VV7Adapter: public BaseAdapter {
-                    public:
-
-                        float lastTorque;
-                        float lastBrake;
-                        int lastDangWei=0;
-                        int seizingCount=0;
-
-                        VV7Adapter();
-                        ~VV7Adapter();
-
-                        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:
-
-            };
-
-    }
-}
-
-
-
-
-
-#endif // VV7_ADAPTER_H

src1/decision/common/adc_adapter/yuhesen_adapter.cpp

@@ -1,157 +0,0 @@
-#include <adc_adapter/yuhesen_adapter.h>
-#include <common/constants.h>
-#include <common/car_status.h>
-#include <math.h>
-#include <iostream>
-#include <fstream>
-
-
-using namespace std;
-
-
-iv::decition::YuHeSenAdapter::YuHeSenAdapter(){
-    adapter_id= 7;
-    adapter_name="yuhesen";
-}
-iv::decition::YuHeSenAdapter::~YuHeSenAdapter(){
-
-}
-
-
-
-iv::decition::Decition iv::decition::YuHeSenAdapter::getAdapterDeciton(GPS_INS now_gps_ins, std::vector<Point2D>  trace , float dSpeed, float obsDistance ,
-                                                                        float  obsSpeed,float accAim,float accNow  ,bool changingDangWei,Decition *decition){
-
-
-    float controlSpeed=accAim;
-    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)
-    {
-        //0110
-        if(changingDangWei){
-            controlSpeed=min(-0.5f,controlSpeed);
-        }
-//        else if((obsDistance<0 ||obsDistance>50)&&(abs(dSpeed-realSpeed)<3)){
-//             controlSpeed=max(-0.2f,controlSpeed);
-//        }
-    }
-    else
-    {
-        controlSpeed=min(1.0f,controlSpeed);
-
-    }
-
-    //0227 10m nei xianzhi shache
-    if(obsDistance<10 &&obsDistance>0){
-        controlSpeed=min(controlSpeed,-0.8f);
-    }
-
-
-//    if(DecideGps00::minDecelerate<0){
-//        controlSpeed = min(DecideGps00::minDecelerate, controlSpeed);
-//    }
-
-    if(minDecelerate<0){
-        controlSpeed = min(minDecelerate, controlSpeed);
-    }
-    controlSpeed = limitSpeed(realSpeed, controlBrake, dSpeed, controlSpeed);
-
-    (*decition)->accelerator=controlSpeed;
-
-    if((*decition)->leftlamp){
-        (*decition)->directLight=1;
-    }else if((*decition)->rightlamp){
-         (*decition)->directLight=2;
-    }else{
-        (*decition)->directLight=0;
-    }
-
-
-    (*decition)->wheel_angle+=ServiceCarStatus.mfEPSOff;
-
-    (*decition)->wheel_angle=max((float)-40.0,(*decition)->wheel_angle);
-     (*decition)->wheel_angle=min((float)40.0,(*decition)->wheel_angle);
-
-
-        (*decition)->brake=0;
-
-
-
-
-std::cout<<"==========================================="<<std::endl;
-     std::cout<<"dSpeed:"<<dSpeed<<"   realSpeed:"<<realSpeed<<"   acc:"<<accAim
-    <<std::endl;
-std::cout<<"========================================="<<std::endl;
-(*decition)->speed = dSpeed/3.6;
-(*decition)->wheel_angle=(*decition)->wheel_angle*0.1;
-(*decition)->wheel_angle=max((float)-40.0,(*decition)->wheel_angle);
-(*decition)->wheel_angle=min((float)40.0,(*decition)->wheel_angle);
-(*decition)->mode=1;
-(*decition)->dangWei=1;
-if((*decition)->brake>0){
-    (*decition)->speed=0;
-}
-    return *decition;
-}
-
-
-
-float iv::decition::YuHeSenAdapter::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 if(ttc>1.6){
-        BrakeIntMax = 5;
-    }else if(ttc>0.8){
-        BrakeIntMax = 8;
-    }else{
-        BrakeIntMax = 10;
-    }
-    if(obsDistance>0 && obsDistance<10){
-        BrakeIntMax=max(BrakeIntMax,3);
-    }
-
-    if (controlBrake > BrakeIntMax) controlBrake = BrakeIntMax;
-
-
-
-
-    controlBrake=min(10.0f,controlBrake);
-    controlBrake=max(0.0f,controlBrake);
-    return controlBrake;
-
-}
-
-float iv::decition::YuHeSenAdapter::limitSpeed(float realSpeed, float controlBrake,float dSpeed,float controlSpeed ){
-
-    controlSpeed=min((float)5.0,controlSpeed);
-    controlSpeed=max((float)-7.0,controlSpeed);
-    return controlSpeed;
-}
-
-

src1/decision/common/adc_adapter/yuhesen_adapter.h

@@ -1,44 +0,0 @@
-#ifndef YUHESEN_ADAPTER_H
-#define YUHESEN_ADAPTER_H
-
-#include <gps_type.h>
-#include <decition_type.h>
-#include <common/obstacle_type.h>
-#include <vector>
-#include <gnss_coordinate_convert.h>
-#include <adc_adapter/base_adapter.h>
-#include <adc_tools/transfer.h>
-//#include <decision/decide_gps_00.h>
-
-
-namespace iv {
-     namespace decition {
-         class YuHeSenAdapter: public BaseAdapter {
-                    public:
-
-                        float lastTorque;
-                        float lastBrake;
-                        int lastDangWei=0;
-                        int seizingCount=0;
-
-                        YuHeSenAdapter();
-                        ~YuHeSenAdapter();
-
-                        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:
-
-            };
-
-    }
-}
-
-
-
-
-
-#endif // VV7_ADAPTER_H

src1/decision/common/adc_adapter/zhongche_adapter.cpp

@@ -1,194 +0,0 @@
-
-#include <adc_adapter/zhongche_adapter.h>
-#include <common/constants.h>
-#include <common/car_status.h>
-#include <math.h>
-#include <iostream>
-#include <fstream>
-
-using namespace std;
-
-
-iv::decition::ZhongcheAdapter::ZhongcheAdapter(){
-    adapter_id= 3;
-    adapter_name="zhongche";
-}
-iv::decition::ZhongcheAdapter::~ZhongcheAdapter(){
-
-}
-
-
-iv::decition::Decition iv::decition::ZhongcheAdapter::getAdapterDeciton(GPS_INS now_gps_ins, std::vector<Point2D>  trace , float dSpeed, float obsDistance ,
-                                                                        float  obsSpeed,float accAim,float accNow  ,bool changingDangWei,Decition *decition){
-
-
-    float controlSpeed=accAim;
-    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;
-    }
-
-
-
-
-
-     (*decition)->grade=1;
-     (*decition)->mode=1;
-     (*decition)->speak=0;
-     (*decition)->handBrake=0;
-     (*decition)->bright=false;
-     (*decition)->engine=0;
-     (*decition)->brakeLight=0;
-
-    if(ServiceCarStatus.bocheMode){
-        (*decition)->dangWei=2;
-    }else{
-        (*decition)->dangWei=1;
-    }
-
-
-    if(ttc>10 && (obsDistance>10||obsDistance<0)){
-
-
-
-
-
-//        if(DecideGps00::minDecelerate<0){
-//            (*decition)->torque = 0;
-//            (*decition)->brake = max((0-DecideGps00::minDecelerate)*90, (*decition)->brake);
-//             dSpeed=10.0;
-//        }else{
-//            (*decition)->torque=90;
-//            (*decition)->brake=0;
-//        }
-
-        if(minDecelerate<0){
-            (*decition)->torque = 0;
-            (*decition)->brake = max((0-minDecelerate)*90, (*decition)->brake);
-             dSpeed=10.0;
-        }else{
-            (*decition)->torque=90;
-            (*decition)->brake=0;
-        }
-
-
-        dSpeed=max(0.0f,dSpeed);
-        dSpeed=min(10.0f, dSpeed);
-        (*decition)->speed=(int)dSpeed*10+5;
-
-        (*decition)->torque=min((float)100.0,(*decition)->torque);
-        (*decition)->torque=max((float)0.0,(*decition)->torque);
-        (*decition)->brake=min((float)100.0,(*decition)->brake);
-        (*decition)->brake=max((float)0.0,(*decition)->brake);
-
-        lastTorque=(*decition)->torque;
-
-        (*decition)->wheel_angle+=ServiceCarStatus.mfEPSOff;
-
-        (*decition)->wheel_angle=max((float)-550.0,(*decition)->wheel_angle);
-         (*decition)->wheel_angle=min((float)550.0,(*decition)->wheel_angle);
-        return *decition;
-    }
-
-
-
-    if (accAim < 0)
-    {
-        if(obsDistance<10 && obsDistance>0 &&ttc>3){
-            controlBrake = (int)(u * 40.0) + 1;  //(u * 14.0) + 1;
-        } else{
-            if(ttc<3){
-                controlBrake = (int)(u * 60.0) + 1;  //(u * 14.0) + 1;
-            }
-            else if(ttc<5){
-                controlBrake = (int)(u * 40.0) + 1;  //(u * 14.0) + 1;
-            }else {
-                controlBrake = (int)(u * 20.0) + 1;  //(u * 14.0) + 1;
-            }
-        }
-          (*decition)->brake = controlBrake;
-        (*decition)->torque=0;
-
-    }
-    else
-    {
-        controlBrake = 0;
-        (*decition)->brake=0;
-
-        if(lastTorque==0){
-             (*decition)->torque = u*(-30)+((0-u)-ServiceCarStatus.mfCalAcc)*1+
-                    now_gps_ins.ins_pitch_angle*10;
-        }else{
-             (*decition)->torque = lastTorque+((0-u)-ServiceCarStatus.mfCalAcc)*10;
-        }
-        if(realSpeed<10){
-             (*decition)->torque=min((float)40.0,(*decition)->torque);
-        }else if(realSpeed<30){
-             (*decition)->torque=min((float)60.0,(*decition)->torque);
-        }
-
-         (*decition)->torque=min((float)99.0,(*decition)->torque);
-         (*decition)->torque=max((float)0.0,(*decition)->torque);
-
-    }
-
-    //0227 10m nei xianzhi shache
-    if(obsDistance<10 &&obsDistance>0){
-        (*decition)->torque=0;
-        (*decition)->brake=max((float)30.0,(*decition)->brake);
-    }
-
-
-//    if(DecideGps00::minDecelerate<0){
-//        dSpeed=10;
-//        (*decition)->torque = 0;
-//        (*decition)->brake = min((0-DecideGps00::minDecelerate)*90, (*decition)->brake);
-//    }
-
-    if(minDecelerate<0){
-        dSpeed=10;
-        (*decition)->torque = 0;
-        (*decition)->brake = min((0-minDecelerate)*90, (*decition)->brake);
-    }
-
-
-    (*decition)->torque=min((float)100.0,(*decition)->torque);
-    (*decition)->torque=max((float)0.0,(*decition)->torque);
-    (*decition)->brake=min((float)100.0,(*decition)->brake);
-    (*decition)->brake=max((float)0.0,(*decition)->brake);
-
-    lastTorque=(*decition)->torque;
-
-    (*decition)->wheel_angle+=ServiceCarStatus.mfEPSOff;
-
-    (*decition)->wheel_angle=max((float)-550.0,(*decition)->wheel_angle);
-     (*decition)->wheel_angle=min((float)550.0,(*decition)->wheel_angle);
-
-
-
-    dSpeed=max(0.0f,dSpeed);
-    dSpeed=min(10.0f, dSpeed);
-    (*decition)->speed=(int)dSpeed*10+5;
-
-
-std::cout<<"==========================================="<<std::endl;
-     std::cout<<"dSpeed:"<<dSpeed<<"   realSpeed:"<<realSpeed<<"   acc:"<<accAim
-    <<std::endl;
-std::cout<<"========================================="<<std::endl;
-
-    return *decition;
-}
-
-
-
-
-
-

src1/decision/common/adc_adapter/zhongche_adapter.h

@@ -1,42 +0,0 @@
-#ifndef ZHONGCHE_ADAPTER_H
-#define ZHONGCHE_ADAPTER_H
-
-#include <gps_type.h>
-#include <decition_type.h>
-#include <common/obstacle_type.h>
-#include <vector>
-#include <gnss_coordinate_convert.h>
-#include <adc_adapter/base_adapter.h>
-#include <adc_tools/transfer.h>
-//#include <decide_gps_00.h>
-
-namespace iv {
-     namespace decition {
-         class ZhongcheAdapter: public BaseAdapter {
-                    public:
-
-                        float lastTorque;
-                        float lastBrake;
-                        int lastDangWei=0;
-                        int seizingCount=0;
-
-                        ZhongcheAdapter();
-                        ~ZhongcheAdapter();
-
-                        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:
-
-            };
-
-    }
-}
-
-
-
-
-#endif // ZHONGCHE_ADAPTER_H

src1/decision/common/adc_controller/base_controller.h

@@ -1,67 +0,0 @@
-#pragma once
-#ifndef _IV_DECITION__BASE_CONTROLLER_
-#define _IV_DECITION__BASE_CONTROLLER_
-
-#include <gps_type.h>
-#include <decition_type.h>
-#include <common/obstacle_type.h>
-#include<vector>
-#include <gnss_coordinate_convert.h>
-
-namespace iv {
-namespace decition {
-//根据传感器传输来的信息作出决策
-class BaseController {
-public:
-
-    int controller_id;
-    std::string  controller_name;
-
-    BaseController();
-    ~BaseController();
-
-
-    /**
-      * @brief iv::decition::BaseController::getControlDecition
-      * 基类BaseController的抽象方法 用于计算横向控制的方向盘和纵向控制的加速度
-      *
-      * @param now_gps_ins          实时gps信息
-      * @param path                 目标路径
-      * @param dSpeed               决策速度
-      * @param obsDistacne          障碍物距离
-      * @param obsSpeed             障碍物速度
-      * @param computeAngle         是否要计算方向盘角度
-      * @param computeAcc           是否要计算纵向加速度
-      * @param decition             决策信息结构体的指针
-      * @return                     iv::decition::Decition
-      */
-    virtual  iv::decition::Decition getControlDecition(GPS_INS now_gps_ins, std::vector<Point2D>  path , float dSpeed, float obsDistacne , float  obsSpeed,
-                                                       bool computeAngle, bool computeAcc, Decition *decition);
-
-
-private:
-
-
-};
-
-}
-}
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-#endif // ADC_CONTROLLER_H

src1/decision/common/adc_controller/pid_controller.cpp

@@ -1,352 +0,0 @@
-#include <adc_controller/pid_controller.h>
-#include <common/constants.h>
-#include <common/car_status.h>
-#include <math.h>
-#include <iostream>
-#include <fstream>
-
-
-using namespace std;
-
-
-iv::decition::PIDController::PIDController(){
-    controller_id = 0;
-    controller_name="pid";
-}
-iv::decition::PIDController::~PIDController(){
-
-}
-
-
-
-/**
- * @brief getControlDecition
- * pid方式计算横向方向盘和纵向加速度
- *
- * @param now_gps_ins          实时gps信息
- * @param path                 目标路径
- * @param dSpeed               决策速度
- * @param ObsDistacne          障碍物距离
- * @param ObsSpeed             障碍物速度
- * @param computeAngle         是否要计算方向盘角度
- * @param computeAcc           是否要计算纵向加速度
- * @param decition             决策信息结构体的指针
- * @return                     iv::decition::Decition
- */
-
-iv::decition::Decition iv::decition::PIDController::getControlDecition(GPS_INS now_gps_ins, std::vector<Point2D> path, float dSpeed, float obsDistacne, float obsSpeed,
-                                                                       bool computeAngle, bool computeAcc, Decition *decition){
-
-    float realSpeed= now_gps_ins.speed;
-    int roadMode = now_gps_ins.roadMode;
-    if(computeAngle){
-        (*decition)->wheel_angle=getPidAngle( realSpeed,  path,roadMode);
-    }
-    if(computeAcc){
-        (*decition)->accelerator=getPidAcc( now_gps_ins, dSpeed,  obsDistacne,  obsSpeed);
-    }
-    return *decition;
-}
-
-
-
-
-float iv::decition::PIDController::getPidAngle(float realSpeed, std::vector<Point2D> trace, int roadMode){
-    double ang = 0;
-    double EPos = 0, EAng = 0;
-
-    double KEang = 14, KEPos = 10, DEang = 0, DEPos = 0,IEpos=0,IEang=0;
-
-    bool turning=false;
-    if(roadMode==14||roadMode==15||roadMode==16||roadMode==17){
-        turning=true;
-    }
-
-    double PreviewDistance;//预瞄距离
-
-    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 if(ServiceCarStatus.msysparam.mvehtype=="hapo"){
-        if(turning){
-           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);
-        }
-    } else{
-        realSpeed > 40 ? PreviewDistance = max(6.0, realSpeed *0.6) : PreviewDistance = max(6.0, realSpeed *0.5);
-    }
-
-    //    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);
-
-    //    }
-    //   if(realSpeed <15){
-    //    PreviewDistance = max(4.0, realSpeed *0.4) ;
-    //   }
-    //    if(realSpeed <10){
-    //        PreviewDistance = max(3.0, realSpeed *0.3) ;
-    //    }
-    double sumdis = 0;
-    int gpsIndex = 0;
-    std::vector<Point2D> farTrace;
-
-    int nsize = trace.size();
-    for (int i = 1; i < nsize-1; i++)
-    {
-        sumdis += GetDistance(trace[i - 1], trace[i]);
-        if (sumdis > PreviewDistance)
-        {
-            gpsIndex = i;
-            break;
-        }
-    }
-
-    EPos = trace[gpsIndex].x;
-
-    for (unsigned int i = max(0, gpsIndex - 3); i < min((size_t)(gpsIndex + 3), trace.size()); i++) {
-        farTrace.push_back(trace[gpsIndex]);
-    }
-    if (farTrace.size() == 0) {
-        EAng = 0;
-    }
-    else {
-        EAng = getAveDef(farTrace);
-    }
-    if(abs(realSpeed)<3){
-        eAngSum=0;
-        ePosSum=0;
-    }else{
-        eAngSum=eAngSum*0.8+EAng;
-        ePosSum=ePosSum*0.8+EPos;
-    }
-
-    ang = KEang * EAng + KEPos * EPos + DEang * (EAng - lastEA) + DEPos * (EPos - lastEP)
-            +IEang*eAngSum + IEpos*ePosSum;
-
-    lastEA = EAng;
-    lastEP = EPos;
-
-    //    if (ang > angleLimit) {
-    //        ang = angleLimit;
-    //    }
-    //    else if (ang < -angleLimit) {
-    //        ang = -angleLimit;
-    //    }
-    if (lastAng >-3000&&lastAng<3000) {
-        ang = 0.2 * lastAng + 0.8 * ang;
-    }
-    if(ang >-3000&&ang<3000){
-        lastAng = ang;
-    }else{
-        ang=lastAng;
-    }
-    return ang;
-
-}
-
-
-
-float iv::decition::PIDController::getAveDef(std::vector<Point2D> farTrace){
-    double sum_x = 0;
-    double sum_y = 0;
-    for (int i = 0; i < min(5, (int)farTrace.size()); i++)
-    {
-        sum_x += farTrace[i].x;
-        sum_y += abs(farTrace[i].y);
-    }
-    double average_y = sum_y / min(5, (int)farTrace.size());
-    double average_x = sum_x / min(5, (int)farTrace.size());
-    return atan(average_x / average_y) / M_PI * 180;
-}
-
-
-
-float iv::decition::PIDController::getPidAcc (GPS_INS gpsIns,float dSpeed, float obsDistance, float obsSpeed){
-
-    std::cout << "\n计算用obs速度:%f\n" << obsSpeed << std::endl;
-    std::cout << "\n计算用obs距离:%f\n" << obsDistance << std::endl;
-
-    if(ServiceCarStatus.msysparam.mvehtype=="hapo")
-    {
-            static int obs_line=0;
-            if(obsDistance<6 && obsDistance>0){
-                dSpeed=0;
-                obs_line=1;
-            }
-            if(obs_line==1)
-            {
-                if(obsDistance<8 && obsDistance>0){
-                    dSpeed=0;
-                }else{
-                    obs_line=0;
-                }
-            }
-    }else if(obsDistance<10 && obsDistance>0){
-        dSpeed=0;
-    }
-
-    float dSecSpeed = dSpeed / 3.6;
-    float realSpeed=gpsIns.speed;
-    float secSpeed =realSpeed/3.6;
-    double vt = dSecSpeed;
-    double vs = secSpeed;
-    if (abs(vs) < 0.05) vs = 0;
-    if (abs(obsSpeed) < 0.05) obsSpeed = 0;
-    double vl = vs + obsSpeed;
-    double vh = vs;
-    double dmax = 150;
-
-    //车距滤波
-    if (obsDistance < 0||obsDistance>100) {
-        obsDistance = 500;
-        obsSpeed=0;
-    }
-
-
-    if (obsDistance > 150) vl = 25; //25m/s
-
-
-    //TTC计算
-    double ds = 0.2566 * vl * vl + 5;
-    double ttcr = (vh - vl) / obsDistance;
-    double ttc = 15;
-    if (15 * (vh - vl) > obsDistance)
-        ttc = obsDistance / (vh - vl);
-
-
-    ServiceCarStatus.mfttc = ttc;
-
-
-    if (obsDistance <= dmax)
-    {
-        if (ttc > 10)
-        {
-            if (obsDistance > ds + 5)
-            {
-                double dds = min(30.0, obsDistance - (ds + 5));
-                vt = vt * dds / 30 + vl * (1 - dds / 30);
-            }
-            else
-            {
-                vt = min(vt, vl);
-            }
-        }
-        else
-        {
-            vt = min(vt, vl);
-        }
-    }else{
-        vt=dSecSpeed;
-    }
-
-    vt=min((float)vt,dSecSpeed);
-    std::cout << "\nvt:%f\n" << vt << std::endl;
-
-    //计算加速度
-    float u =computeU(obsDistance,ds,vs,vt,vh,vl,ttc,ttcr);
-
-    //u 限值
-    u=limiteU(u,ttc);
-
-    lastVt = vt;
-    lastU = u;
-    float acc=0-u;
-    return acc;
-}
-
-
-float iv::decition::PIDController::computeU(float obsDistance, float ds,float vs,float vt,float vh,float vl,float ttc,float ttcr){
-
-    double Id = 0;
-    double ed = ds - obsDistance;
-    if (obsDistance > 150) ed = 0;
-    double ev = vs - vt;
-    double u = 0;
-    if (ttc>10)
-    {
-        double kp = 0.5;        //double kp = 0.5;
-        double kd = 0.5;       //kd = 0.5
-        //      double k11 = 0.025;
-        //      double k12 = 0.000125;
-        double dev = (ev - lastEv) / 0.1;
-        Iv = 0.925 * Iv + ev;
-        Id = 0.85 * Id + ed;
-        if (abs(vh) < 2.0&& abs(vl) < 2.0)
-        {
-            Iv = 0.0; Id = 0.0;
-        }
-        //u = kp * ev + kd * dev + k11 * Iv + k12 * Id;
-        u = kp * ev + kd * dev;
-    }
-    else if (ttc < 1.6 || (ttc <= 10 && obsDistance <= 0.75 * ds))
-    {
-        //AEB控制
-        Id = 0; Iv = 0;
-        u = 0;
-        if (ttc < 0.8) u = 7;
-        else if (ttc < 1.6) u = 3 + 5 * (1.6 - ttc);
-        else
-        {
-            u = (10 - ttc) * (10 - ttc) / 23.52;
-        }
-    }
-    else
-    {
-        //制动控制
-        Id = 0; Iv = 0;
-        if (obsDistance > 1.25 * ds)
-        {
-            double rev_ed = 1 / ed;
-            u = -0.5 * obsDistance * obsDistance * rev_ed * ttcr * ttcr;
-        }
-        else
-        {
-            if (abs(vl) > 2.0)
-                u = 0;
-            else
-                u = max(lastU, 1.0f);
-        }
-    }
-    if (abs(vl) < 1.0 && abs(vh) < 1.0
-            && obsDistance < 2 * ds)
-    {
-        u = 0.5;
-    }
-    lastEv = ev;
-    return u;
-}
-
-
-float iv::decition::PIDController::limiteU(float u,float ttc){
-    if(ttc<3 && u<-0.2){
-        u=-0.2;
-    }
-    else
-    {
-        if (u < -1.5) u = -1.5;
-    }
-    if (u >= 0 && lastU <= 0)
-    {
-        if (u > 0.5) u = 0.5;
-    }
-    else if (u >= 0 && lastU >= 0)
-    {
-        if (u > lastU + 0.5) u = lastU + 0.5;
-    }
-    else if (u <= 0 && lastU >= 0)
-    {
-        if (u < -0.04) u = -0.04;
-    }
-    else if (u <= 0 && lastU <= 0)
-    {
-        if (u < lastU - 0.04) u = lastU - 0.04;
-    }
-    return u;
-}
-
-
-
-
-
-

src1/decision/common/adc_controller/pid_controller.h

@@ -1,76 +0,0 @@
-#ifndef _IV_DECITION__PID_CONTROLLER_
-#define _IV_DECITION__PID_CONTROLLER_
-
-#include <gps_type.h>
-#include <decition_type.h>
-#include <common/obstacle_type.h>
-#include<vector>
-#include <gnss_coordinate_convert.h>
-#include <adc_controller/base_controller.h>
-#include <adc_tools/transfer.h>
-
-namespace iv {
-        namespace decition {
-        //根据传感器传输来的信息作出决策
-                    class PIDController: public BaseController {
-                    public:
-
-                        float lastEA;
-                        float lastEP;
-                        float lastAng;
-                        float angleLimit=600;
-                        float lastU ;
-                        float lastEv ;
-                        float lastVt ;
-
-                        float Iv;
-                        float eAngSum=0;
-                        float ePosSum=0;
-
-                        PIDController();
-                        ~PIDController();
-
-                        /**
-                           * @brief getControlDecition
-                           * pid方式计算横向方向盘和纵向加速度
-                           *
-                           * @param now_gps_ins          实时gps信息
-                           * @param path                 目标路径
-                           * @param dSpeed               决策速度
-                           * @param obsDistacne          障碍物距离
-                           * @param obsSpeed             障碍物速度
-                           * @param computeAngle         是否要计算方向盘角度
-                           * @param computeAcc           是否要计算纵向加速度
-                           * @param decition             决策信息结构体的指针
-                           * @return                     iv::decition::Decition
-                           */
-
-
-                        iv::decition::Decition getControlDecition(GPS_INS now_gps_ins, std::vector<Point2D> path,float dSpeed, float obsDistacne,float  obsSpeed,
-                                                                  bool computeAngle, bool computeAcc, Decition *decition);
-                        float  getPidAngle (float realSpeed, std::vector<Point2D> trace,int roadMode);
-                        float  getAveDef (std::vector<Point2D> farTrace);
-                        float  getPidAcc (GPS_INS gpsIns,float dSpeed, float obsDistacne, float obsSpeed);
-                        float  computeU(float obsDistance, float ds,float vs,float vt,float vh,float vl,float ttc,float ttcr);
-                        float  limiteU(float u ,float ttc);
-
-
-                    private:
-
-                    };
-
-        }
-}
-
-
-
-
-
-
-
-
-
-
-
-
-#endif // PID_CONTROLLER_H

src1/decision/common/adc_planner/base_planner.cpp

@@ -1,40 +0,0 @@
-#include <gps_type.h>
-#include <decition_type.h>
-#include <common/obstacle_type.h>
-#include <vector>
-#include <gnss_coordinate_convert.h>
-#include <adc_tools/transfer.h>
-//#include "perception/perceptionoutput.h"
-#include <adc_planner/base_planner.h>
-
-
-
-
-
-
-iv::decition::BasePlanner::BasePlanner(){
-
-}
-iv::decition::BasePlanner::~BasePlanner(){
-
-}
-
-
-/**
- * @brief iv::decition::BasePlanner::getPath
- * 基类BasePlanner的抽象方法，用于计算车辆行驶的路径。路径都已转换成车辆坐标系下的路径。
- *
- * @param now_gps_ins          实时gps信息
- * @param gpsMapLine           地图数据点
- * @param PathPoint            地图路线中距离车辆位置最近的一个点的序号
- * @param offset               在车辆避障中，地图源路线所在车道 与 即将去往的车道 之间的距离差值。可以为负值。
- * @param lidarGridPtr         激光雷达信息网格
- * @return                     返回一条车辆坐标系下的路径
- */
-std::vector<iv::Point2D> iv::decition::BasePlanner::getPath(GPS_INS now_gps_ins,const std::vector<GPSData>& gpsMapLine, int PathPoint, double offset, iv::LidarGridPtr lidarGridPtr){
-
-}
-
-//std::vector<iv::Point2D> iv::decition::BasePlanner::getPath (GPS_INS now_gps_ins,  std::vector<GPSData> gpsMapLine,  LidarGridPtr lidarGridPtr, std::vector<iv::Perception::PerceptionOutput> lidar_per){
-
-//}

src1/decision/common/adc_planner/base_planner.h

@@ -1,48 +0,0 @@
-#ifndef _IV_DECITION__BASE_PLANNER_
-#define _IV_DECITION__BASE_PLANNER_
-
-#include <gps_type.h>
-#include <decition_type.h>
-#include <common/obstacle_type.h>
-#include<vector>
-#include <gnss_coordinate_convert.h>
-//#include "perception/perceptionoutput.h"
-
-namespace iv {
-namespace decition {
-//根据传感器传输来的信息作出决策
-    class BasePlanner {
-    public:
-        int planner_id;
-        std::string  planner_name;
-
-        BasePlanner();
-        virtual ~BasePlanner();
-
-        /**
-         * @brief iv::decition::BasePlanner::getPath
-         * 基类BasePlanner的抽象方法，用于计算车辆行驶的路径。路径都已转换成车辆坐标系下的路径。
-         *
-         * @param now_gps_ins          实时gps信息
-         * @param gpsMapLine           地图数据点
-         * @param PathPoint            地图路线中距离车辆位置最近的一个点的序号
-         * @param offset               在车辆避障中，地图源路线所在车道 与 即将去往的车道 之间的距离差值。可以为负值。
-         * @param lidarGridPtr         激光雷达信息网格
-         * @return                     返回一条车辆坐标系下的路径
-         */
-        virtual  std::vector<iv::Point2D> getPath (GPS_INS now_gps_ins, const std::vector<GPSData>& gpsMapLine, int PathPoint, double offset, iv::LidarGridPtr lidarGridPtr);
-//        virtual  std::vector<iv::Point2D> getPath (GPS_INS now_gps_ins,  std::vector<GPSData> gpsMapLine,  LidarGridPtr lidarGridPtr, std::vector<iv::Perception::PerceptionOutput> lidar_per);
-
-    private:
-
-    };
-
-}
-}
-
-
-#endif // ADC_PLANNER_H
-
-
-
-

src1/decision/common/adc_planner/dubins_planner.cpp

@@ -1,58 +0,0 @@
-#include <adc_planner/dubins_planner.h>
-#include <adc_tools/transfer.h>
-#include <adc_tools/parameter_status.h>
-#include <common/constants.h>
-
-#include <math.h>
-
-std::vector<iv::GPSData> iv::decition::DubinsPlanner::gpsMap;
-
-iv::decition::DubinsPlanner::DubinsPlanner(){
-    this->planner_id = 2;
-    this->planner_name = "Dubins";
-}
-
-iv::decition::DubinsPlanner::~DubinsPlanner(){
-}
-
-/**
- * @brief iv::decition::LaneChangePlanner::getPath
- * 采用的车道变换的局部路径生成方法。计算原始地图路径偏移offset距离之后的路径。
- *
- * @param now_gps_ins          实时gps信息
- * @param gpsMapLine           地图数据点
- * @param PathPoint            地图路线中距离车辆位置最近的一个点的序号
- * @param offset               在车辆避障中，地图源路线所在车道 与 即将去往的车道 之间的距离差值。可以为负值。
- * @param lidarGridPtr         激光雷达信息网格
- * @return                     返回一条车辆坐标系下的路径
- */
-std::vector<iv::Point2D> iv::decition::DubinsPlanner::getPath(GPS_INS now_gps_ins, const std::vector<GPSData>& gpsMapLine, int PathPoint, double offset, iv::LidarGridPtr lidarGridPtr){
-    std::vector<iv::Point2D> trace;
-
-    double L = 2.6;
-    double R = L/sin(10.0*M_PI/180.0);
-    std::cout<<" R = "<<R<<std::endl;
-//    return a.exec();
-//    double q0[] = { 0,0,1.57};
-//    double q1[] = { 4,4,3.142 };
-    gpsMap.clear();
-    double q0[] = { now_gps_ins.gps_x, now_gps_ins.gps_y, now_gps_ins.ins_heading_angle};
-    double q1[] = { gpsMapLine[PathPoint]->gps_x, gpsMapLine[PathPoint]->gps_y ,gpsMapLine[PathPoint]->ins_heading_angle };
-    double turning_radius = 10.0;
-    DubinsPath path;
-    dubins_shortest_path( &path, q0, q1, turning_radius);
-    dubins_path_sample_many( &path, 0.1,  printConfiguration, NULL);
-
-
-}
-
-int  iv::decition::DubinsPlanner::printConfiguration(double q[3], double x, void* user_data) {
-    GPSData gps;
-    gps->roadMode=5;
-    gps->gps_x=q[0];
-    gps->gps_y=q[1];
-    gps->ins_heading_angle=q[3];
-    gpsMap.push_back(gps);
-    return 0;
-}
-

src1/decision/common/adc_planner/dubins_planner.h

@@ -1,44 +0,0 @@
-#ifndef DUBINS_PLANNER_H
-#define DUBINS_PLANNER_H
-#include "base_planner.h"
-#include <adc_tools/dubins.h>
-#include <QCoreApplication>
-
-#include <math.h>
-#include <iostream>
-
-
-
-#include <stdio.h>
-#include <string.h>
-
-namespace iv{
-namespace decition{
-    class DubinsPlanner : public BasePlanner{
-    public:
-        DubinsPlanner();
-        ~DubinsPlanner();
-
-         static std::vector<iv::GPSData>  gpsMap;
-
-        /**
-         * @brief iv::decition::LaneChangePlanner::getPath
-         * 采用的车道变换的局部路径生成方法。计算原始地图路径偏移offset距离之后的路径。
-         *
-         * @param now_gps_ins          实时gps信息
-         * @param gpsMapLine           地图数据点
-         * @param PathPoint            地图路线中距离车辆位置最近的一个点的序号
-         * @param offset               在车辆避障中，地图源路线所在车道 与 即将去往的车道 之间的距离差值。可以为负值。
-         * @param lidarGridPtr         激光雷达信息网格
-         * @return                     返回一条车辆坐标系下的路径
-         */
-        std::vector<iv::Point2D> getPath(GPS_INS now_gps_ins, const std::vector<GPSData>& gpsMapLine, int PathPoint, double offset, iv::LidarGridPtr lidarGridPtr);
-
-        int  static printConfiguration(double q[3], double x, void* user_data);
-
-        };
-    }
-}
-
-
-#endif // DUBINS_PLANNER_H

src1/decision/common/adc_planner/frenet_planner.cpp

@@ -1,644 +0,0 @@
-#include "frenet_planner.h"
-
-#include <adc_tools/transfer.h>
-#include <common/car_status.h>
-#include <adc_tools/parameter_status.h>
-//#include <decide_gps_00.h>
-#include <adc_planner/lane_change_planner.h>
-
-#include<Eigen/Dense>
-
-using namespace std;
-using namespace Eigen;
-
-iv::decition::FrenetPlanner::FrenetPlanner(){
-    this->planner_id = 1;
-    this->planner_name = "Frenet";
-    this->lidarGridPtr = NULL;
-}
-
-iv::decition::FrenetPlanner::~FrenetPlanner(){
-    free(this->lidarGridPtr);
-}
-
-
-/**
- * @brief iv::decition::FrenetPlanner::getPath
- * 采用的基于frenet坐标系的最优局部路径生成方法，可实现从当前车道变换到另一车道之间的路径生成。还可实现车辆在路线旁起步的“有迹可循”。
- * 计算的是，在车辆到达 原始地图路径偏移offset距离的车道 之前，frenet下规划的路径。
- *
- * @param now_gps_ins          实时gps信息
- * @param gpsMapLine           地图数据点
- * @param PathPoint            地图路线中距离车辆位置最近的一个点的序号
- * @param offset               在车辆避障中，地图源路线所在车道 与 即将去往的车道 之间的距离差值。可以为负值。offset = -(roadNow - roadOri)*roadWidth
- * @param lidarGridPtr         激光雷达信息网格
- * @return                     返回一条车辆坐标系下的路径
- */
-std::vector<iv::Point2D> iv::decition::FrenetPlanner::getPath(GPS_INS now_gps_ins, const std::vector<GPSData>& gpsMapLine, int PathPoint, double offset, iv::LidarGridPtr lidarGridPtr){
-    this->roadNow = -1;
-    this->now_gps_ins = now_gps_ins;
-    this->gpsMapLine = gpsMapLine;
-    this->PathPoint = PathPoint;
-    this->lidarGridPtr = lidarGridPtr;
-    LaneChangePlanner *lcp = new LaneChangePlanner();
-    std::vector<iv::Point2D> gpsTrace = lcp->getPath(now_gps_ins,gpsMapLine,PathPoint,0,lidarGridPtr);
-    delete lcp;
-    double realSecSpeed = now_gps_ins.speed / 3.6;
-    leftWidth = offset;
-    rightWidth = offset;
-    std::vector<iv::Point2D> trace = getGpsTrace_AfterCalcFrenetTrace(gpsTrace,realSecSpeed);
-    return trace;
-}
-
-/**
- * @brief iv::decition::FrenetPlanner::chooseRoadByFrenet
- * 用frenet的方法对每条道路考察，选择一个最优的道路
- * @param now_gps_ins          实时gps信息
- * @param gpsMapLine           地图数据点
- * @param PathPoint            地图路线中距离车辆位置最近的一个点的序号
- * @param offsetL              在选择道路中，地图源路线左边的最大宽度。可以为负值。offsetL = -(roadSum - 1)*roadWidth
- * @param offsetR              在选择道路中，地图源路线右边的最大宽度。可以为非负值。offsetR = (roadOri - 0)*roadWidth
- * @param lidarGridPtr         激光雷达信息网格
- * @return
- */
-int iv::decition::FrenetPlanner::chooseRoadByFrenet(GPS_INS now_gps_ins, const std::vector<GPSData>& gpsMapLine, int PathPoint, double offsetL, double offsetR, iv::LidarGridPtr lidarGridPtr,int roadNow){
-    this->roadNow = roadNow;
-    this->aimRoadByFrenet = -1;
-    this->now_gps_ins = now_gps_ins;
-    this->gpsMapLine = gpsMapLine;
-    this->PathPoint = PathPoint;
-    this->lidarGridPtr = lidarGridPtr;
-
-    LaneChangePlanner *lcp = new LaneChangePlanner();
-    std::vector<iv::Point2D> gpsTrace = lcp->getPath(now_gps_ins,gpsMapLine,PathPoint,0,lidarGridPtr);
-    delete lcp;
-    double realSecSpeed = now_gps_ins.speed / 3.6;
-    leftWidth = offsetL;
-    rightWidth = offsetR;
-    std::vector<iv::Point2D> trace = getGpsTrace_AfterCalcFrenetTrace(gpsTrace,realSecSpeed);
-    return aimRoadByFrenet;
-}
-
-
-/**
- * @brief iv::decition::FrenetPlanner::getGpsTrace_AfterCalcFrenetTrace
- * 进行frenet规划的总函数，还包括了规划前的一些准备工作，比如frenet坐标系建立、车辆起始状态等。
- * @param gpsTrace             地图路线上从PathPoint开始的600个点
- * @param realsecSpeed         实时车速 [m/s]
- * @return                     返回一条frenet规划后并转换到车辆坐标系下的路径
- */
-std::vector<iv::Point2D> iv::decition::FrenetPlanner::getGpsTrace_AfterCalcFrenetTrace(const std::vector<iv::Point2D>& gpsTrace,double realsecSpeed){
-    vector<Point2D> trace;
-    vector<FrenetPoint> frenet_s;
-    double stp=0.0;
-    //把gpsTrace里的点全部转为frenet坐标系下的点，存储在frenet_s中。相当于frenet坐标系的S轴。
-    FrenetPoint tmp={.x=gpsTrace[0].x,.y=gpsTrace[0].y,.s=0.0,.d=0.0};
-    frenet_s.push_back(tmp);
-    for(int i=1; i<gpsTrace.size(); ++i){
-        stp+=iv::decition::GetDistance(gpsTrace[i-1],gpsTrace[i]);
-        tmp={.x=gpsTrace[i].x,.y=gpsTrace[i].y,.s=stp,.d=0.0};
-        frenet_s.push_back(tmp);
-    }
-
-    //求出车辆当前位置在frenet坐标系下的坐标
-//    FrenetPoint car_now_at_frenet = XY2Frenet(0,0,gpsTrace);
-    FrenetPoint car_now_at_frenet = getFrenetfromXY(0,0,gpsTrace,this->gpsMapLine,this->PathPoint,this->now_gps_ins);
-
-    double c_s_speed = realsecSpeed * cos(car_now_at_frenet.tangent_Ang-M_PI/2);
-    double c_d_speed = realsecSpeed * sin(car_now_at_frenet.tangent_Ang-M_PI/2);
-    double realAcc = ServiceCarStatus.mfCalAcc;
-    double c_s_dd = realAcc * cos(car_now_at_frenet.tangent_Ang-M_PI/2);
-    double c_d_dd = realAcc * sin(car_now_at_frenet.tangent_Ang-M_PI/2);
-
-    vector<Point2D> path=frenet_optimal_planning(car_now_at_frenet,frenet_s,c_s_speed,c_d_speed,c_d_dd);
-
-    return path;
-}
-
-
-/**
- * @brief iv::decition::FrenetPlanner::frenet_optimal_planning
- * 对frenet规划出的路径进行后续操作，并选出损失最小的一条路径作为最优路径
- * @param car_now_frenet_point 车辆当前位置在frenet坐标系下的坐标
- * @param frenet_s             frenet坐标系的s轴
- * @param c_speed              车辆的纵向速度。沿s轴方向的速度。
- * @param c_d_d                车辆沿d方向的速度。
- * @param c_d_dd               车辆沿d方向的加速度。
- * @return                     返回一条frenet规划的最优路径
- */
-vector<iv::Point2D> iv::decition::FrenetPlanner::frenet_optimal_planning(iv::decition::FrenetPoint car_now_frenet_point,
-                                                          const std::vector<FrenetPoint>& frenet_s, double c_speed, double c_d_d, double c_d_dd){
-    double s0 = car_now_frenet_point.s;
-    double c_d = car_now_frenet_point.d;
-    vector<iv::Point2D> gpsTrace;
-    vector<Frenet_path> fplist=calc_frenet_paths(c_speed, c_d, c_d_d, c_d_dd, s0);  //frenet规划
-
-    sort(fplist.begin(),fplist.end(),comp); //按损失度由小到大进行排序
-    for(int i=0; i<fplist.size(); ++i){
-        calc_global_single_path(fplist[i],frenet_s);
-        if(check_single_path(fplist[i])){
-            gpsTrace = frenet_path_to_gpsTrace(fplist[i]);
-            aimRoadByFrenet = fplist[i].roadflag;
-            return gpsTrace;
-        }
-    }
-    return gpsTrace;
-
-/*
-//    calc_global_paths(fplist, frenet_s);    //生成frenet路径中每个点在车辆坐标系下的坐标
-//    fplist = check_paths(fplist);  //检验计算出的每条路径
-//    //找到损失最小的轨迹
-//    double min_cost = (numeric_limits<double>::max)();
-//    Frenet_path bestpath;
-//    for(int i=0; i<fplist.size(); ++i){
-//        if(min_cost > fplist[i].cf){
-//            min_cost = fplist[i].cf;
-//            bestpath = fplist[i];
-//        }
-//    }
-//    gpsTrace = frenet_path_to_gpsTrace(bestpath);
-//    aimRoadByFrenet = bestpath.roadflag;
-//    return gpsTrace;
-*/
-}
-
-
-/**
- * @brief iv::decition::FrenetPlanner::calc_frenet_paths
- * 正式规划出不同的frenet路径，并统计相应的损失度。单从车辆行驶的平滑性计算而得，后续需检测是否有障碍物。
- * @param c_speed              车辆的纵向速度。沿s轴方向的速度
- * @param c_d                  车辆的横向（d方向）偏移距离
- * @param c_d_d                车辆沿d方向的速度
- * @param c_d_dd               车辆沿d方向的加速度
- * @param s0                   车辆沿s方向的坐标
- * @return                     返回多条frenet路径
- */
-vector<iv::decition::Frenet_path> iv::decition::FrenetPlanner::calc_frenet_paths(double c_speed, double c_d, double c_d_d, double c_d_dd, double s0){
-    vector<iv::decition::Frenet_path> frenet_paths;
-    int roadNum = round(abs((leftWidth-rightWidth)/ServiceParameterStatus.D_ROAD_W));   //roadNum为frenet算法的起始道路序号
-    //采样，并对每一个目标配置生成轨迹
-    for(double di=leftWidth; di<=rightWidth; di+=ServiceParameterStatus.D_ROAD_W,roadNum--){
-        if(roadNum == this->roadNow)
-            continue;
-        //横向动作规划
-        for(double Ti=ServiceParameterStatus.MINT; Ti<ServiceParameterStatus.MAXT; Ti+=ServiceParameterStatus.DT){
-            Frenet_path fp;
-            fp.roadflag = roadNum;
-            //计算出关于目标配置di，Ti的横向多项式
-            Quintic_polynomial lat_qp = Quintic_polynomial(c_d, c_d_d, c_d_dd, di, 0.0, 0.0, Ti);
-
-            for(double i=0; i<Ti; i+=ServiceParameterStatus.D_POINT_T)
-                fp.t.push_back(i);
-
-            for(int i=0; i<fp.t.size(); ++i){
-                fp.d.push_back(lat_qp.calc_point(fp.t[i]));
-                fp.d_d.push_back(lat_qp.calc_first_derivative(fp.t[i]));
-                fp.d_dd.push_back(lat_qp.calc_second_derivative(fp.t[i]));
-                fp.d_ddd.push_back(lat_qp.calc_third_derivative(fp.t[i]));
-            }
-
-            //纵向速度规划 (速度保持)
-            for(double tv=ServiceParameterStatus.TARGET_SPEED - ServiceParameterStatus.D_T_S * ServiceParameterStatus.N_S_SAMPLE;
-                tv<(ServiceParameterStatus.TARGET_SPEED + ServiceParameterStatus.D_T_S * ServiceParameterStatus.N_S_SAMPLE);
-                tv+=ServiceParameterStatus.D_T_S){
-                Frenet_path tfp = fp;
-                Quartic_polynomial lon_qp = Quartic_polynomial(s0, c_speed, 0.0, tv, 0.0, Ti);
-
-                for(int i=0; i<fp.t.size(); ++i){
-                    tfp.s.push_back(lon_qp.calc_point(fp.t[i]));
-                    tfp.s_d.push_back(lon_qp.calc_first_derivative(fp.t[i]));
-                    tfp.s_dd.push_back(lon_qp.calc_second_derivative(fp.t[i]));
-                    tfp.s_ddd.push_back(lon_qp.calc_third_derivative(fp.t[i]));
-                }
-
-                //square of jerk
-                double Jp = inner_product(tfp.d_ddd.begin(), tfp.d_ddd.end(), tfp.d_ddd.begin(), 0);
-                //square of jerk
-                double Js = inner_product(tfp.s_ddd.begin(), tfp.s_ddd.end(), tfp.s_ddd.begin(), 0);
-
-                //square of diff from target speed
-                double ds = pow((ServiceParameterStatus.TARGET_SPEED - tfp.s_d.back()),2);
-                //横向的损失函数
-                tfp.cd = ServiceParameterStatus.KJ * Jp + ServiceParameterStatus.KT * Ti + ServiceParameterStatus.KD * tfp.d.back() * tfp.d.back();
-                //纵向的损失函数
-                tfp.cv = ServiceParameterStatus.KJ * Js + ServiceParameterStatus.KT * Ti + ServiceParameterStatus.KD * ds;
-                //总的损失函数为d 和 s方向的损失函数乘对应的系数相加
-                tfp.cf = ServiceParameterStatus.KLAT * tfp.cd + ServiceParameterStatus.KLON * tfp.cv;
-
-                frenet_paths.push_back(tfp);
-            }
-        }
-    }
-    return frenet_paths;
-}
-
-
-/**
- * @brief iv::decition::FrenetPlanner::calc_global_paths
- * 计算出每条frenet路径中轨迹点的x，y坐标。x，y坐标是轨迹点在车辆坐标系下的横纵坐标。
- * 部分函数参数在第一种计算XY坐标的方法中没有使用，但在第二种方法中会用到。
- * @param fplist               多条frenet路径
- * @param frenet_s             frenet坐标系的s轴
- * @return                     通过引用传递返回带有x，y值的多条frenet路径
- */
-void iv::decition::FrenetPlanner::calc_global_paths(vector<Frenet_path>& fplist,const std::vector<FrenetPoint>& frenet_s){
-
-    for(auto& fp:fplist){
-        for(int i=0; i<fp.s.size(); ++i){
-            double ix,iy;
-//            Frenet2XY(&ix,&iy,fp.s[i],fp.d[i],frenet_s);  //第一种方法
-            getXYfromFrenet(&ix,&iy,fp.s[i],fp.d[i],frenet_s,this->gpsMapLine,this->PathPoint,this->now_gps_ins);   //第二种方法
-
-            fp.x.push_back(ix);
-            fp.y.push_back(iy);
-        }
-
-        for(int i=0; i<(fp.x.size()-1); ++i){
-            double dx = fp.x[i+1] - fp.x[i];
-            double dy = fp.y[i+1] - fp.y[i];
-            fp.yaw.push_back(atan2(dy,dx));
-            fp.ds.push_back(sqrt(dx*dx + dy*dy));
-        }
-
-        fp.ds.push_back(fp.ds.back());
-        fp.yaw.push_back(fp.yaw.back());
-
-        for(int i = 0; i < (fp.yaw.size() - 1); ++i){
-            fp.c.push_back((fp.yaw[i + 1] - fp.yaw[i]) / fp.ds[i]);
-        }
-    }
-}
-
-void iv::decition::FrenetPlanner::calc_global_single_path(Frenet_path &fp, const std::vector<FrenetPoint> &frenet_s){
-    for(int i=0; i<fp.s.size(); ++i){
-        double ix,iy;
-        //            Frenet2XY(&ix,&iy,fp.s[i],fp.d[i],frenet_s);  //第一种方法
-        getXYfromFrenet(&ix,&iy,fp.s[i],fp.d[i],frenet_s,this->gpsMapLine,this->PathPoint,this->now_gps_ins);   //第二种方法
-
-        fp.x.push_back(ix);
-        fp.y.push_back(iy);
-    }
-
-    for(int i=0; i<(fp.x.size()-1); ++i){
-        double dx = fp.x[i+1] - fp.x[i];
-        double dy = fp.y[i+1] - fp.y[i];
-        fp.yaw.push_back(atan2(dy,dx));
-        fp.ds.push_back(sqrt(dx*dx + dy*dy));
-    }
-
-    fp.ds.push_back(fp.ds.back());
-    fp.yaw.push_back(fp.yaw.back());
-
-    for(int i = 0; i < (fp.yaw.size() - 1); ++i){
-        fp.c.push_back((fp.yaw[i + 1] - fp.yaw[i]) / fp.ds[i]);
-    }
-}
-
-
-/**
- * @brief iv::decition::FrenetPlanner::check_paths
- * 对多条frenet路径进行检验，排除不符合要求的路径。
- * @param fplist               多条frenet路径
- * @return                     排除部分路径后的多条frenet路径
- */
-vector<iv::decition::Frenet_path> iv::decition::FrenetPlanner::check_paths(const vector<Frenet_path>& fplist){
-    vector<Frenet_path> okind;
-    int i=0;
-    int j=0;
-
-    for(i=0; i<fplist.size(); ++i){
-        cout<<"&&&&&&&&&&fplist[i].roadflag: "<<fplist[i].roadflag<<endl;
-        for(j=0; j<fplist[i].s_d.size(); ++j){
-            if(fplist[i].s_d[j]>ServiceParameterStatus.MAX_SPEED)       //最大速度检查
-                goto ContinueFlag;
-        }
-        for(j=0; j<fplist[i].s_dd.size(); ++j){
-            if(abs(fplist[i].s_dd[j])>ServiceParameterStatus.MAX_ACCEL) //最大加速度检查
-                goto ContinueFlag;
-        }
-        for(j=30; j<fplist[i].c.size()-30; ++j){
-            if(abs(fplist[i].c[j])>ServiceParameterStatus.MAX_CURVATURE)//最大曲率检查
-                goto ContinueFlag;
-        }
-        if(!check_collision(fplist[i]))
-            continue;
-        okind.push_back(fplist[i]);
-        ContinueFlag:continue;
-    }
-    return okind;
-}
-
-
-bool iv::decition::FrenetPlanner::check_single_path(const iv::decition::Frenet_path &fp){
-    int j=0;
-
-    cout<<"&&&&&&&&&&fp.roadflag: "<<fp.roadflag<<endl;
-    for(j=0; j<fp.s_d.size(); ++j){
-        if(fp.s_d[j]>ServiceParameterStatus.MAX_SPEED)       //最大速度检查
-            return false;
-    }
-    for(j=0; j<fp.s_dd.size(); ++j){
-        if(abs(fp.s_dd[j])>ServiceParameterStatus.MAX_ACCEL) //最大加速度检查
-            return false;
-    }
-    for(j=30; j<fp.c.size()-30; ++j){
-        if(abs(fp.c[j])>ServiceParameterStatus.MAX_CURVATURE)//最大曲率检查
-            return false;
-    }
-    if(!check_collision(fp))
-        return false;
-
-    return true;
-}
-
-
-/**
- * @brief iv::decition::FrenetPlanner::check_collision
- * 检验frenet路径上是否有障碍物，调用的是改进的函数computeObsOnRoadByFrenet。
- * @param frenet_path          一条frenet路径
- * @param car_now_frenet_point 车辆当前位置在frenet坐标系下的坐标
- * @return                     在路径上有障碍物返回false，否则返回true。
- */
-bool iv::decition::FrenetPlanner::check_collision(const iv::decition::Frenet_path &frenet_path){
-    std::vector<Point2D> gpsTrace = frenet_path_to_gpsTrace(frenet_path);
-    double obs=-1;
- //   iv::decition::DecideGps00().computeObsOnRoadByFrenet(this->lidarGridPtr,gpsTrace,obs,this->gpsMapLine,this->PathPoint,this->now_gps_ins);
-    if(obs > 0 && obs < 30)
-        return false;
-    else
-        return true;
-}
-
-
-/**
- * @brief iv::decition::FrenetPlanner::frenet_path_to_gpsTrace
- * 将frenet路径转换到车辆坐标系下。
- * @param frenet_path          一条frenet路径
- * @return                     一条车辆坐标系下的路径
- */
-vector<iv::Point2D> iv::decition::FrenetPlanner::frenet_path_to_gpsTrace(const Frenet_path& frenet_path){
-    vector<iv::Point2D> gpsTrace;
-    for (int i=0; i<frenet_path.x.size(); ++i) {
-        gpsTrace.push_back(Point2D(frenet_path.x[i],frenet_path.y[i]));
-        gpsTrace[i].speed = sqrt(frenet_path.d_d[i]*frenet_path.d_d[i]+frenet_path.s_d[i]*frenet_path.s_d[i])*3.6;
-    }
-    return gpsTrace;
-}
-
-
-double iv::decition::FrenetPlanner::distance(double x1, double y1, double x2, double y2)
-{
-    return sqrt((x2-x1)*(x2-x1)+(y2-y1)*(y2-y1));
-}
-
-
-//找出gpsTrace中距离（x，y）最近的一个点
-int iv::decition::FrenetPlanner::ClosestWaypoint(double x, double y, const std::vector<Point2D>& gpsTrace)
-{
-
-    double closestLen = 100000; //large number
-    int closestWaypoint = 0;
-
-    for(int i = 1; i < gpsTrace.size(); i++)
-    {
-        double map_x = gpsTrace[i].x;
-        double map_y = gpsTrace[i].y;
-        double dist = distance(x,y,map_x,map_y);
-        if(dist < closestLen)
-        {
-            closestLen = dist;
-            closestWaypoint = i;
-        }
-
-    }
-    return closestWaypoint;
-}
-
-
- /* |==========================================================|
-  * |         车辆坐标系与 frenet 坐标系互转的第一种方法。           |
-  * |==========================================================| */
- // 车体坐标转frenet坐标。frenet坐标原点是 gpsTrace 中序号为0的点。
- // Transform from Cartesian x,y coordinates to Frenet s,d coordinates
-iv::decition::FrenetPoint iv::decition::FrenetPlanner::XY2Frenet(double x, double y, const std::vector<iv::Point2D>& gpsTrace){
-    int next_wp=ClosestWaypoint( x,  y,  gpsTrace);
-    int prev_wp = next_wp-1;  //prev_wp代表在next_wp的前一个点计算坐标，能够保证frenet_s都是依次相加的。避免在next_wp计算frenet_s的话，可能出现减的情况。
-//    if(next_wp == 0){
-//        prev_wp  = gpsTrace.size()-1;
-//    }
-
-    double n_x = gpsTrace[next_wp].x-gpsTrace[prev_wp].x;
-    double n_y = gpsTrace[next_wp].y-gpsTrace[prev_wp].y;
-    double x_x = x - gpsTrace[prev_wp].x;
-    double x_y = y - gpsTrace[prev_wp].y;
-
-    // find the projection of x onto n
-    double proj_norm = (x_x*n_x+x_y*n_y)/(n_x*n_x+n_y*n_y);
-    double proj_x = proj_norm*n_x;
-    double proj_y = proj_norm*n_y;  //proj_x、proj_y应该为垂足的坐标
-
-    double frenet_d = sqrt((x_x - proj_x) * (x_x - proj_x) + (x_y - proj_y) * (x_y - proj_y));
-    //经手动推算frenet_d = abs(n_x * x_y - n_y * x_x) / sqrt(n_x * n_x + n_y * n_y)。
-    //即frenet_d，等于点(x,y)到 过prev_wp、next_wp两点的直线 的垂直距离。
-
-    //see if d value is positive or negative by comparing it to a center point
-    double center_x = 1000-gpsTrace[prev_wp].x;
-    double center_y = 2000-gpsTrace[prev_wp].y;
-    double centerToPos = distance(center_x,center_y,x_x,x_y);
-    double centerToRef = distance(center_x,center_y,proj_x,proj_y);
-    if(centerToPos <= centerToRef){
-        frenet_d *= -1;
-    }
-
-    // calculate s value
-    double frenet_s = 0;
-    for(int i = 0; i < prev_wp; i++){
-        frenet_s += distance(gpsTrace[i].x,gpsTrace[i].y,gpsTrace[i+1].x,gpsTrace[i+1].y);
-    }
-    if(prev_wp<=0){
-        frenet_s -= distance(0,0,proj_x,proj_y);
-    }else{
-        frenet_s += distance(0,0,proj_x,proj_y);
-    }
-
-    double tmp_Angle = atan2(n_y,n_x);
-
-    return {x:x,y:y,s:frenet_s,d:frenet_d,tangent_Ang:tmp_Angle};
-}
-
- // frenet坐标转车体坐标。
- // Transform from Frenet s,d coordinates to Cartesian x,y
-void iv::decition::FrenetPlanner::Frenet2XY(double *res_x, double *res_y, double s, double d, const vector<iv::decition::FrenetPoint>& frenetTrace){
-    int prev_wp = 0;
-    //退出循环时，prev_wp 最大是等于 frenetTrace.size()-2。
-    while(s > frenetTrace[prev_wp+1].s && (prev_wp < (int)(frenetTrace.size()-2) )){
-        prev_wp++;
-    }
-    int wp2 = prev_wp+1;
-    double heading = atan2((frenetTrace[wp2].y-frenetTrace[prev_wp].y),(frenetTrace[wp2].x-frenetTrace[prev_wp].x));
-
-    double seg_s = (s-frenetTrace[prev_wp].s);
-    double seg_x = frenetTrace[prev_wp].x+seg_s*cos(heading);
-    double seg_y = frenetTrace[prev_wp].y+seg_s*sin(heading);
-
-    double perp_heading = heading-M_PI*0.5;
-    *res_x = seg_x + d*cos(perp_heading);
-    *res_y = seg_y + d*sin(perp_heading);
-}
-
-/* |==========================================================|
- * |         车辆坐标系与 frenet 坐标系互转的第二种方法。           |
- * |==========================================================| */
-// 车体坐标转frenet坐标。frenet坐标原点是 gpsTrace 中序号为0的点。
-iv::decition::FrenetPoint iv::decition::FrenetPlanner::getFrenetfromXY(double x, double y, const std::vector<iv::Point2D>& gpsTrace,const std::vector<iv::GPSData>& gpsMap,int pathpoint,GPS_INS nowGps){
-    int next_wp=ClosestWaypoint( x,  y,  gpsTrace);
-    int prev_wp = next_wp-1;  //prev_wp代表在next_wp的前一个点计算坐标，能够保证frenet_s都是依次相加的。避免在next_wp计算frenet_s的话，可能出现减的情况。
-    //    if(next_wp == 0){
-    //        prev_wp  = gpsTrace.size()-1;
-    //    }
-
-    GPS_INS gps = Coordinate_UnTransfer(x,y,nowGps);
-    Point2D pt = Coordinate_Transfer(gps.gps_x,gps.gps_y, *gpsMap[(pathpoint+prev_wp)%gpsMap.size()]);
-
-    // calculate s value
-    double frenet_s = 0;
-    for(int i = 0; i < prev_wp; i++){
-        frenet_s += distance(gpsTrace[i].x,gpsTrace[i].y,gpsTrace[i+1].x,gpsTrace[i+1].y);
-    }
-    frenet_s += pt.y;
-
-    //theta为prev_wp处车的航向角 与 nowGps处的车辆坐标系下x轴之间的夹角，逆时针为正。
-    double theta = (nowGps.ins_heading_angle - gpsMap[(pathpoint+prev_wp)%gpsMap.size()]->ins_heading_angle+90);
-    double tmp_Angle = theta * M_PI / 180;
-
-    return {x:x,y:y,s:frenet_s,d:pt.x,tangent_Ang:tmp_Angle};
-}
-
-void iv::decition::FrenetPlanner::getXYfromFrenet(double *res_x, double *res_y, double s, double d, const vector<iv::decition::FrenetPoint>& frenetTrace,const std::vector<iv::GPSData>& gpsMap,int pathpoint,GPS_INS nowGps)
-{
-    int prev_wp = -1;
-
-    while((prev_wp < (int)(frenetTrace.size()-1) ) && s > frenetTrace[prev_wp+1].s)
-    {
-        prev_wp++;
-    }
-    if(prev_wp < 0){
-        prev_wp = 0;
-    }
-//    int wp2 =prev_wp+1;
-    GPS_INS gps=  Coordinate_UnTransfer( d, s-frenetTrace[prev_wp].s , *gpsMap[(pathpoint+prev_wp)%gpsMap.size()]);
-    Point2D pt =  Coordinate_Transfer( gps.gps_x, gps.gps_y, nowGps);
-
-    *res_x = pt.x;
-    *res_y = pt.y;
-}
-
-
-iv::decition::Quintic_polynomial::Quintic_polynomial(double xs, double vxs, double axs, double xe, double vxe, double axe, double T){
-    //计算五次多项式系数
-    this->xs = xs;
-    this->vxs = vxs;
-    this->axs = axs;
-    this->xe = xe;
-    this->vxe = vxe;
-    this->axe = axe;
-
-    this->a0 = xs;
-    this->a1 = vxs;
-    this->a2 = axs/ 2.0;
-
-    MatrixXd A(3, 3);
-    MatrixXd b(3, 1);
-    MatrixXd x(3, 1);
-
-    A << pow(T, 3),   pow(T, 4),   pow(T, 5),
-         3*pow(T, 2), 4*pow(T, 3), 5*pow(T, 4),
-         6*T,         12*T*T,      20*pow(T, 3);
-    b << xe - a0 - a1*T - a2*T*T,
-         vxe - a1 - 2*a2*T,
-         axe - 2*a2;
-    x=A.colPivHouseholderQr().solve(b);
-
-    this->a3 = x(0,0);
-    this->a4 = x(1,0);
-    this->a5 = x(2,0);
-}
-
-iv::decition::Quintic_polynomial::~Quintic_polynomial(){
-}
-
-double iv::decition::Quintic_polynomial::calc_point(double t){
-    double xt = this->a0 + this->a1 * t + this->a2 * t*t + this->a3 * t*t*t + this->a4 * t*t*t*t + this->a5 * t*t*t*t*t;
-    return xt;
-}
-
-double iv::decition::Quintic_polynomial::calc_first_derivative(double t){
-    double xt = this->a1 + 2*this->a2 * t + 3*this->a3 * t*t + 4*this->a4 * t*t*t + 5*this->a5 * t*t*t*t;
-    return xt;
-}
-
-double iv::decition::Quintic_polynomial::calc_second_derivative(double t){
-    double xt = 2*this->a2 + 6*this->a3 * t + 12*this->a4 * t*t + 20*this->a5 * t*t*t;
-    return xt;
-}
-
-double iv::decition::Quintic_polynomial::calc_third_derivative(double t){
-    double xt = 6*this->a3 + 24*this->a4 * t + 60*this->a5 * t*t;
-    return xt;
-}
-
-
-
-iv::decition::Quartic_polynomial::Quartic_polynomial(double xs, double vxs, double axs, double vxe, double axe, double T){
-    //计算四次多项式系数
-    this->xs = xs;
-    this->vxs = vxs;
-    this->axs = axs;
-    this->vxe = vxe;
-    this->axe = axe;
-
-    this->a0 = xs;
-    this->a1 = vxs;
-    this->a2 = axs / 2.0;
-
-    MatrixXd A(2, 2);
-    MatrixXd b(2, 1);
-    MatrixXd x(2, 1);
-
-    A << 3*pow(T, 2), 4*pow(T, 3),
-         6*T,         12*T*T ;
-    b << vxe - a1 - 2*a2*T,
-         axe - 2*a2;
-    x=A.colPivHouseholderQr().solve(b);
-
-    this->a3 = x(0,0);
-    this->a4 = x(1,0);
-}
-
-iv::decition::Quartic_polynomial::~Quartic_polynomial(){
-}
-
-double iv::decition::Quartic_polynomial::calc_point(double t){
-    double xt = this->a0 + this->a1*t + this->a2*t*t + this->a3*t*t*t + this->a4*t*t*t*t;
-    return xt;
-}
-
-double iv::decition::Quartic_polynomial::calc_first_derivative(double t){
-    double xt = this->a1 + 2*this->a2*t + 3*this->a3*t*t + 4*this->a4*t*t*t;
-    return xt;
-}
-
-double iv::decition::Quartic_polynomial::calc_second_derivative(double t){
-    double xt = 2*this->a2 + 6*this->a3*t + 12*this->a4*t*t;
-    return xt;
-}
-
-double iv::decition::Quartic_polynomial::calc_third_derivative(double t){
-    double xt = 6*this->a3 + 24*this->a4*t;
-    return xt;
-}
-
-bool iv::decition::FrenetPlanner::comp(const iv::decition::Frenet_path &a, const iv::decition::Frenet_path &b){
-    return a.cf < b.cf;
-}
-

src1/decision/common/adc_planner/frenet_planner.h

@@ -1,161 +0,0 @@
-#ifndef FRENET_PLANNER_H
-#define FRENET_PLANNER_H
-
-#include "base_planner.h"
-
-namespace iv{
-namespace decition{
-
-    //x，y分别为frenet轨迹点相对于车辆的x，y轴坐标
-    //s，d分别为frenet轨迹点相对于frenet坐标系的s，d轴坐标值
-    //tangent_Ang为轨迹点在frenet坐标系的s轴的投影处的 切线 与车辆坐标系x轴之间的夹角。
-    //tangent_Ang用于分解计算车辆分别在s轴、d轴方向上的运动速度、加速度等。
-    struct FrenetPoint{
-        double x;
-        double y;
-        double s;
-        double d;
-        double tangent_Ang;
-    };
-
-
-
-    class Quintic_polynomial
-    {
-    public:
-        Quintic_polynomial(double xs, double vxs, double axs, double xe, double vxe, double axe, double T);
-        ~Quintic_polynomial();
-    private:
-        // 计算五次多项式系数
-        double xs;    //d0
-        double vxs;   //d.0
-        double axs;   //d..0
-        double xe;    //d1
-        double vxe;   //d.1
-        double axe;   //d..1
-
-        double a0;
-        double a1;
-        double a2;
-        double a3;
-        double a4;
-        double a5;
-
-    public:
-        double calc_point(double t);
-        double calc_first_derivative(double t);
-        double calc_second_derivative(double t);
-        double calc_third_derivative(double t);
-    };
-
-
-    class Quartic_polynomial
-    {
-    public:
-        Quartic_polynomial(double xs, double vxs, double axs, double vxe, double axe, double T);
-        ~Quartic_polynomial();
-
-    private:
-        //计算四次多项式系数
-        double xs;
-        double vxs;
-        double axs;
-        double vxe;
-        double axe;
-
-        double a0;
-        double a1;
-        double a2;
-        double a3;
-        double a4;
-
-    public:
-        double calc_point(double t);
-        double calc_first_derivative(double t);
-        double calc_second_derivative(double t);
-        double calc_third_derivative(double t);
-    };
-
-
-    class Frenet_path
-    {
-    public:
-        std::vector<double> t;
-        std::vector<double> d;
-        std::vector<double> d_d;
-        std::vector<double> d_dd;
-        std::vector<double> d_ddd;
-        std::vector<double> s;
-        std::vector<double> s_d;
-        std::vector<double> s_dd;
-        std::vector<double> s_ddd;
-        int roadflag = -1;  //标记每一条frenet路径，是以哪一条道路为目标道路的
-        double cd = 0.0;
-        double cv = 0.0;
-        double cf = 0.0;
-
-        std::vector<double> x;
-        std::vector<double> y;
-        std::vector<double> yaw;
-        std::vector<double> ds;
-        std::vector<double> c;
-    };
-
-    class FrenetPlanner : public BasePlanner{
-    public:
-        int aimRoadByFrenet = -1;          //记录由frenet规划选择出来的道路序号
-        int roadNow = -1;                  //记录避障时车辆当前道路序号,规划路径时，避开此条道路
-        double leftWidth = 0.0;            //中心线左边偏移距离，为负值
-        double rightWidth = 0.0;           //中心线右边偏移距离，为正值
-        GPS_INS now_gps_ins;               //实时gps信息
-        std::vector<GPSData> gpsMapLine;   //地图数据点
-        int PathPoint = 0;                 //地图路线中距离车辆位置最近的一个点的序号
-        iv::LidarGridPtr lidarGridPtr;     //激光雷达信息网格
-
-    public:
-        FrenetPlanner();
-        ~FrenetPlanner();
-
-        /**
-         * @brief iv::decition::FrenetPlanner::getPath
-         * 采用的基于frenet坐标系的最优局部路径生成方法，可实现从当前车道变换到另一车道之间的路径生成。还可实现车辆在路线旁起步的“有迹可循”。
-         * 计算的是，在车辆到达 原始地图路径偏移offset距离的车道 之前，frenet下规划的路径。
-         *
-         * @param now_gps_ins          实时gps信息
-         * @param gpsMapLine           地图数据点
-         * @param PathPoint            地图路线中距离车辆位置最近的一个点的序号
-         * @param offset               在车辆避障中，地图源路线所在车道 与 即将去往的车道 之间的距离差值。可以为负值。
-         * @param lidarGridPtr         激光雷达信息网格
-         * @return                     返回一条车辆坐标系下的路径
-         */
-        std::vector<iv::Point2D> getPath(GPS_INS now_gps_ins, const std::vector<GPSData>& gpsMapLine, int PathPoint, double offset, iv::LidarGridPtr lidarGridPtr);
-        int chooseRoadByFrenet(GPS_INS now_gps_ins, const std::vector<GPSData>& gpsMapLine, int PathPoint, double offsetL, double offsetR, iv::LidarGridPtr lidarGridPtr,int roadNow);
-        static iv::decition::FrenetPoint XY2Frenet(double x, double y, const std::vector<Point2D>& gpsTracet);
-        static void Frenet2XY(double *x, double *y, double s, double d, const std::vector<iv::decition::FrenetPoint>& frenetTrace);
-        static double distance(double x1, double y1, double x2, double y2);
-        static int ClosestWaypoint(double x, double y, const std::vector<Point2D>& gpsTrace);
-        static FrenetPoint getFrenetfromXY(double x, double y, const std::vector<iv::Point2D>& gpsTrace,const std::vector<iv::GPSData>& gpsMap,int pathpoint,GPS_INS nowGps);
-        static void getXYfromFrenet(double *res_x, double *res_y, double s, double d, const std::vector<iv::decition::FrenetPoint>& frenetTrace,const std::vector<iv::GPSData>& gpsMap,int pathpoint,GPS_INS nowGps);
-        static bool comp(const iv::decition::Frenet_path &a,const iv::decition::Frenet_path &b);
-
-    private:
-        std::vector<iv::Point2D> getGpsTrace_AfterCalcFrenetTrace(const std::vector<iv::Point2D>& gpsTrace,double realsecSpeed);
-        std::vector<iv::Point2D> frenet_optimal_planning(FrenetPoint car_now_frenet_point,
-                                                         const std::vector<FrenetPoint>& frenet_s,
-                                                         double c_speed, double c_d_d, double c_d_dd);
-        std::vector<Frenet_path> calc_frenet_paths(double c_speed, double c_d, double c_d_d, double c_d_dd, double s0);
-        void calc_global_paths(std::vector<Frenet_path>& fplist,const std::vector<FrenetPoint>& frenet_s);
-        void calc_global_single_path(Frenet_path& fp,const std::vector<FrenetPoint>& frenet_s);
-        std::vector<Frenet_path> check_paths(const std::vector<Frenet_path>& fplist);
-        bool check_single_path(const Frenet_path &fp);
-        bool check_collision(const Frenet_path &frenet_path);
-
-        std::vector<Point2D> frenet_path_to_gpsTrace(const Frenet_path& frenet_path);
-
-
-    };
-
-}
-}
-
-#endif // FRENET_PLANNER_H

src1/decision/common/adc_planner/lane_change_planner.cpp

@@ -1,196 +0,0 @@
-#include <adc_planner/lane_change_planner.h>
-#include <adc_tools/transfer.h>
-#include <adc_tools/parameter_status.h>
-#include <common/constants.h>
-
-#include <math.h>
-
-iv::decition::LaneChangePlanner::LaneChangePlanner(){
-    this->planner_id = 0;
-    this->planner_name = "LaneChange";
-}
-
-iv::decition::LaneChangePlanner::~LaneChangePlanner(){
-}
-
-/**
- * @brief iv::decition::LaneChangePlanner::getPath
- * 采用的车道变换的局部路径生成方法。计算原始地图路径偏移offset距离之后的路径。
- *
- * @param now_gps_ins          实时gps信息
- * @param gpsMapLine           地图数据点
- * @param PathPoint            地图路线中距离车辆位置最近的一个点的序号
- * @param offset               在车辆避障中，地图源路线所在车道 与 即将去往的车道 之间的距离差值。可以为负值。
- * @param lidarGridPtr         激光雷达信息网格
- * @return                     返回一条车辆坐标系下的路径
- */
-std::vector<iv::Point2D> iv::decition::LaneChangePlanner::getPath(GPS_INS now_gps_ins, const std::vector<GPSData>& gpsMapLine, int PathPoint, double offset, iv::LidarGridPtr lidarGridPtr){
-    std::vector<iv::Point2D> trace;
-    trace = this->getGpsTrace(now_gps_ins,gpsMapLine,PathPoint);
-    if(offset!=0){
-        trace = this->getGpsTraceOffset(trace,offset);
-    }
-    return trace;
-}
-
-bool iv::decition::LaneChangePlanner::checkAvoidEnable(const std::vector<double>& obsDisVector, int roadAim){
-    int roadNow = ServiceParameterStatus.now_road_num;
-    if ((obsDisVector[roadAim]>0 && obsDisVector[roadAim]<obsDisVector[roadNow]+ServiceCarStatus.msysparam.vehLenth)||
-            (obsDisVector[roadAim]>0 && obsDisVector[roadAim]<15))
-    {
-        return false;
-    }
-
-    if (roadAim-roadNow>1)
-    {
-        for (int i = roadNow+1; i < roadAim; i++)
-        {
-            if (obsDisVector[i] < 8 + (i-roadNow) * 2 && obsDisVector[i]>0) {
-                return false;
-            }
-        }
-
-    }
-
-    else if (roadNow-roadAim>1)
-    {
-        for (int i = roadNow-1; i >roadAim; i--)
-        {
-            if (obsDisVector[i] < 8 + (roadNow-i) * 2 && obsDisVector[i]>0) {
-                return false;
-            }
-        }
-    }
-    return true;
-}
-
-std::vector<iv::Point2D> iv::decition::LaneChangePlanner::getGpsTrace(iv::GPS_INS now_gps_ins, const std::vector<GPSData>& gpsMapLine, int PathPoint){
-    std::vector<iv::Point2D> trace;
-
-    if (gpsMapLine.size() > 400 && PathPoint >= 0) {
-        for (int i = PathPoint; i < PathPoint + 600; 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 = 36;
-                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);
-        }
-    }
-    return trace;
-}
-
-std::vector<iv::Point2D> iv::decition::LaneChangePlanner::getGpsTraceOffset(const 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 = std::max(0, j - 4); k <= j; k++)
-        {
-            count1 = count1 + 1;
-            sumx1 += gpsTrace[k].x;
-            sumy1 += gpsTrace[k].y;
-        }
-        for (int k = j; k <= std::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 + M_PI / 2),
-                           carFronty + avoidLenth  * sin(anglevalue + M_PI / 2));
-            ptLeft.speed = gpsTrace[j].speed;
-            ptLeft.v1 = gpsTrace[j].v1;
-            ptLeft.v2 = gpsTrace[j].v2;
-            ptLeft.roadMode = gpsTrace[j].roadMode;
-            trace.push_back(ptLeft);
-        }
-        else
-        {
-            Point2D ptRight(carFrontx + avoidLenth  * cos(anglevalue - M_PI / 2),
-                            carFronty + avoidLenth  * sin(anglevalue - M_PI / 2));
-            ptRight.speed = gpsTrace[j].speed;
-            ptRight.v1 = gpsTrace[j].v1;
-            ptRight.v2 = gpsTrace[j].v2;
-            ptRight.roadMode = gpsTrace[j].roadMode;
-            trace.push_back(ptRight);
-        }
-    }
-    return trace;
-}
-
-

src1/decision/common/adc_planner/lane_change_planner.h

@@ -1,38 +0,0 @@
-#ifndef LANE_CHANGE_PLANNER_H
-#define LANE_CHANGE_PLANNER_H
-
-#include "base_planner.h"
-
-namespace iv{
-namespace decition{
-    class LaneChangePlanner : public BasePlanner{
-    public:
-        LaneChangePlanner();
-        ~LaneChangePlanner();
-
-
-        /**
-         * @brief iv::decition::LaneChangePlanner::getPath
-         * 采用的车道变换的局部路径生成方法。计算原始地图路径偏移offset距离之后的路径。
-         *
-         * @param now_gps_ins          实时gps信息
-         * @param gpsMapLine           地图数据点
-         * @param PathPoint            地图路线中距离车辆位置最近的一个点的序号
-         * @param offset               在车辆避障中，地图源路线所在车道 与 即将去往的车道 之间的距离差值。可以为负值。
-         * @param lidarGridPtr         激光雷达信息网格
-         * @return                     返回一条车辆坐标系下的路径
-         */
-        std::vector<iv::Point2D> getPath(GPS_INS now_gps_ins, const std::vector<GPSData>& gpsMapLine, int PathPoint, double offset, iv::LidarGridPtr lidarGridPtr);
-
-        bool checkAvoidEnable(const std::vector<double>& obsDisVector, int roadAim);
-
-    private:
-        std::vector<iv::Point2D> getGpsTrace(iv::GPS_INS now_gps_ins, const std::vector<GPSData>& gpsMapLine, int PathPoint);
-        std::vector<iv::Point2D> getGpsTraceOffset(const std::vector<Point2D>& gpsTrace, double offset);
-
-    };
-}
-}
-
-
-#endif // LANE_CHANGE_PLANNER_H

src1/decision/common/adc_tools/compute_00.cpp

@@ -1,2102 +0,0 @@
-#include <adc_tools/compute_00.h>
-//#include <decision/decide_gps_00.h>
-#include <adc_tools/gps_distance.h>
-#include <decition_type.h>
-#include <adc_tools/transfer.h>
-#include <constants.h>
-#include <math.h>
-#include <iostream>
-#include <fstream>
-//#include <control/can.h>
-#include "common/car_status.h"
-
-#include "adc_planner/frenet_planner.h"
-using namespace std;
-
-extern bool handPark;
-extern long handParkTime;
-extern bool rapidStop;
-extern int gpsMissCount;
-extern bool changeRoad;
-extern double avoidX;
-extern bool parkBesideRoad;
-extern double steerSpeed;
-extern bool transferPieriod;
-extern bool transferPieriod2;
-extern double traceDevi;
-
-#define PI (3.1415926535897932384626433832795)
-
-iv::decition::Compute00::Compute00() {
-
-}
-iv::decition::Compute00::~Compute00() {
-
-}
-
-
-double iv::decition::Compute00::angleLimit = 700;
-double iv::decition::Compute00::lastEA = 0;
-double iv::decition::Compute00::lastEP = 0;
-double iv::decition::Compute00::decision_Angle = 0;
-double iv::decition::Compute00::lastAng = 0;
-int iv::decition::Compute00::lastEsrID = -1;
-int  iv::decition::Compute00::lastEsrCount = 0;
-int iv::decition::Compute00::lastEsrIDAvoid = -1;
-int  iv::decition::Compute00::lastEsrCountAvoid = 0;
-
-iv::GPS_INS  iv::decition::Compute00::nearTpoint;
-iv::GPS_INS  iv::decition::Compute00::farTpoint;
-double  iv::decition::Compute00::bocheAngle;
-
-
-
-iv::GPS_INS  iv::decition::Compute00::dTpoint0;
-iv::GPS_INS  iv::decition::Compute00::dTpoint1;
-iv::GPS_INS  iv::decition::Compute00::dTpoint2;
-iv::GPS_INS  iv::decition::Compute00::dTpoint3;
-double  iv::decition::Compute00::dBocheAngle;
-
-
-std::vector<int> lastEsrIdVector;
-std::vector<int> lastEsrCountVector;
-
-
-
-int iv::decition::Compute00::getDesiredSpeed(std::vector<GPSData> gpsMap)
-{
-    int startIndex = 0;     // startIndex = 0 则每一次都是遍历整条地图路线
-    int endIndex = gpsMap.size() - 1;
-    static double FrontTotalFive=0,FrontAveFive=0,BackAveFive=0,BackTotalFive=0;
-    static int FrontCount=0,BackCount=0;
-    static int CurrentIndex=0,MarkingIndex=0,MarkingCount=0;
-    int MarkedIndex=0,CurveContinue=0;
-
-    for (int j = startIndex; j < endIndex; j++)
-    {
-        int i = (j + gpsMap.size()) % gpsMap.size();
-        if((*gpsMap[i]).roadMode!=6)
-                (*gpsMap[i]).roadMode=5;
-    }
-    for (int j = startIndex; j < (endIndex-40); j+=40)
-    {
-        int i = (j + gpsMap.size()) % gpsMap.size();
-        for(int front_k=i;front_k<i+20;front_k++)
-        {
-            if(fabs(((*gpsMap[front_k]).ins_heading_angle)-((*gpsMap[i]).ins_heading_angle))<10)
-            {
-                   FrontTotalFive+=(*gpsMap[front_k]).ins_heading_angle;
-                   FrontCount++;
-            }
-
-        }
-        i+=20;
-        FrontAveFive=FrontTotalFive/FrontCount;
-        FrontTotalFive=0;
-        FrontCount=0;
-        for(int back_k=i;back_k<i+20;back_k++)
-        {
-            if(fabs((*gpsMap[back_k]).ins_heading_angle-(*gpsMap[i]).ins_heading_angle)<10)
-            {
-                   BackTotalFive+=(*gpsMap[back_k]).ins_heading_angle;
-                   BackCount++;
-            }
-
-        }
-        i+=20;
-        CurrentIndex=i-1;
-        BackAveFive=BackTotalFive/BackCount;
-        BackTotalFive=0;
-        BackCount=0;
-        if(fabs(FrontAveFive-BackAveFive)<50)
-        {
-                   if(fabs(FrontAveFive-BackAveFive)>4)
-                   {
-                        CurveContinue+=1;
-                        if(CurveContinue>=1)
-                        {
-                            MarkingCount=0;
-                            for(MarkingIndex=CurrentIndex;MarkingIndex>=MarkedIndex;MarkingIndex--)
-                            {
-                                if((*gpsMap[MarkingIndex]).roadMode!=6)
-                                {
-                                if(MarkingCount<150)
-                                {
-                                     if((BackAveFive-FrontAveFive)<=3.5)
-                                     {
-                                           (*gpsMap[MarkingIndex]).roadMode=14;   //弯道，2米,14
-                                     }
-                                     else if((BackAveFive-FrontAveFive)>3.5)
-                                     {
-                                           (*gpsMap[MarkingIndex]).roadMode=15;
-                                     }
-
-                                } //else if((MarkingCount>=100)&&(MarkingCount<150)){(*gpsMap[MarkingIndex]).roadMode=18;   //超低速，10米,1}
-                                else if((MarkingCount>=150)&&(MarkingCount<320))
-                                {
-                                     (*gpsMap[MarkingIndex]).roadMode=5;   //低速，20米,5
-                                }
-                                else if((MarkingCount>=320)&&(MarkingCount<620))
-                                {
-                                     (*gpsMap[MarkingIndex]).roadMode=0;   //常速，60米
-                                }
-                                else if(MarkingCount>=620)
-                                {
-                                     (*gpsMap[MarkingIndex]).roadMode=11;   //高速/疯狂加速，大于60米
-                                }
-                                }
-
-                                MarkingCount++;
-                            }
-                            MarkedIndex=CurrentIndex;
-                        }
-                   }
-                   else if(fabs(FrontAveFive-BackAveFive)<=4)
-                   {
-                        CurveContinue=0;
-                   }
-        }
-        FrontAveFive=0;
-        BackAveFive=0;
-    }
-
-    if(MarkedIndex<endIndex)
-    {
-        MarkingCount=0;
-        for(MarkingIndex=endIndex;MarkingIndex>=MarkedIndex;MarkingIndex--)
-        {
-            if((*gpsMap[MarkingIndex]).roadMode!=6)
-            {
-            if(MarkingCount<100)
-            {
-                if((BackAveFive-FrontAveFive)<3)
-                {
-                      (*gpsMap[MarkingIndex]).roadMode=14;   //弯道，2米,14
-                }
-                else if((BackAveFive-FrontAveFive)>3)
-                {
-                      (*gpsMap[MarkingIndex]).roadMode=15;
-                }
-            }
-            else if((MarkingCount>=100)&&(MarkingCount<150))
-            {
-                 (*gpsMap[MarkingIndex]).roadMode=18;   //超低速，10米
-            }
-            else if((MarkingCount>=150)&&(MarkingCount<320))
-            {
-                 (*gpsMap[MarkingIndex]).roadMode=5;   //低速，30米
-            }
-            else if((MarkingCount>=320)&&(MarkingCount<620))
-            {
-                 (*gpsMap[MarkingIndex]).roadMode=0;   //常速，60米
-            }
-            else if(MarkingCount>=620)
-            {
-                 (*gpsMap[MarkingIndex]).roadMode=11;   //高速/疯狂加速，大于60米
-            }
-             }
-            MarkingCount++;
-        }
-    }
-
-
-    return 1;
-}
-
-
-
-//首次找点
-
-int iv::decition::Compute00::getFirstNearestPointIndex(GPS_INS rp, std::vector<GPSData> gpsMap, int lastIndex, double mindis, double maxAngle)
-{
-    int index = -1;
-    //	DecideGps00().minDis = iv::MaxValue;
-    float minDis = 10;
-    double maxAng = iv::MaxValue;
-
-    int startIndex = 0;     // startIndex = 0 则每一次都是遍历整条地图路线
-    int endIndex = gpsMap.size() - 1;
-
-    for (int j = startIndex; j < endIndex; j++)
-    {
-        int i = (j + gpsMap.size()) % gpsMap.size();
-        double tmpdis = GetDistance(rp, (*gpsMap[i]));
-
-        if (tmpdis < minDis && (abs(rp.ins_heading_angle - (*gpsMap[i]).ins_heading_angle) < 80
-                                              || abs(rp.ins_heading_angle - (*gpsMap[i]).ins_heading_angle - 360) < 80
-                                              || abs(rp.ins_heading_angle - (*gpsMap[i]).ins_heading_angle + 360) < 80)
-                )
-        {
-            index = i;
-            minDis = tmpdis;
-            maxAng = min(abs(rp.ins_heading_angle - (*gpsMap[i]).ins_heading_angle), abs(rp.ins_heading_angle - (*gpsMap[i]).ins_heading_angle - 360));
-            maxAng = min(maxAng, abs(rp.ins_heading_angle - (*gpsMap[i]).ins_heading_angle + 360));
-        }
-    }
-
-//    DecideGps00().maxAngle=maxAng;
-//    DecideGps00().minDis=minDis;
-    return index;
-}
-
-//search pathpoint
-int iv::decition::Compute00::getNearestPointIndex(GPS_INS rp, const std::vector<GPSData> gpsMap, int lastIndex, double mindis, double maxAngle)
-{
-    int index = -1;
-    float minDis = 10;
-    double maxAng = iv::MaxValue;
-    int map_size=gpsMap.size();
-    int preDistance=max(100,(int)(rp.speed*10));
-        preDistance=min(500,preDistance);
-
-    int startIndex = max((int)(lastIndex - 100),(int)(lastIndex-map_size));     // startIndex = 0 则每一次都是遍历整条地图路线
-    int endIndex = min((int)(lastIndex + preDistance ),(int)(lastIndex+map_size));
-
-
-    for (int j = startIndex; j < endIndex; j++)
-    {
-        int i = (j + map_size) % map_size;
-        double tmpdis = GetDistance(rp, (*gpsMap[i]));
-
-        if (tmpdis < minDis && (abs(rp.ins_heading_angle - (*gpsMap[i]).ins_heading_angle) < 80
-            || abs(rp.ins_heading_angle - (*gpsMap[i]).ins_heading_angle - 360) < 80
-            || abs(rp.ins_heading_angle - (*gpsMap[i]).ins_heading_angle + 360) < 80)
-            )
-        {
-            index = i;
-            minDis = tmpdis;
-            maxAng = min(abs(rp.ins_heading_angle - (*gpsMap[i]).ins_heading_angle), abs(rp.ins_heading_angle - (*gpsMap[i]).ins_heading_angle - 360));
-            maxAng = min(maxAngle, abs(rp.ins_heading_angle - (*gpsMap[i]).ins_heading_angle + 360));
-        }
-    }
-
-
-//    DecideGps00().maxAngle=maxAng;
-//    DecideGps00().minDis=minDis;
-    return index;
-}
-
-
-double iv::decition::Compute00::getAveDef(std::vector<Point2D> farTrace)
-{
-    double sum_x = 0;
-    double sum_y = 0;
-
-    for (int i = 0; i < min(5, (int)farTrace.size()); i++)
-    {
-        sum_x += farTrace[i].x;
-        sum_y += abs(farTrace[i].y);
-    }
-    double average_y = sum_y / min(5, (int)farTrace.size());
-    double average_x = sum_x / min(5, (int)farTrace.size());
-
-
-    return atan(average_x / average_y) / PI * 180;
-}
-
-
-
-double iv::decition::Compute00::getAvoidAveDef(std::vector<Point2D> farTrace, double avoidX)
-{
-    double sum_x = 0;
-    double sum_y = 0;
-
-    for (int i = 0; i < min(5, (int)farTrace.size()); i++)
-    {
-        sum_x += farTrace[i].x;
-        sum_y += abs(farTrace[i].y);
-    }
-    double average_y = sum_y / min(5, (int)farTrace.size());
-    double average_x = sum_x / min(5, (int)farTrace.size());
-
-
-    return atan(average_x + avoidX / average_y) / PI * 180;
-}
-
-
-
-
-
-
-double iv::decition::Compute00::getDecideAngle(std::vector<Point2D>  gpsTrace, double realSpeed) {
-
-    double ang = 0;
-    double EPos = 0, EAng = 0;
-
-    //   double KEang = 14, KEPos = 10, DEang = 3, DEPos = 1;  // double KEang = 14, KEPos = 10, DEang = 10, DEPos = 10;
-    double KEang = 14, KEPos = 10, DEang = 0, DEPos = 0;
-
-    if(transferPieriod&& !transferPieriod2){
-        DEang = 200;
-        DEPos = 150;
-    }
-
-    //   double PreviewDistance = max(6.0, realSpeed / 3.6 * 1.8);//预瞄距离
-
-    double PreviewDistance;//预瞄距离
-    realSpeed > 40 ? PreviewDistance = max(6.0, realSpeed *0.6) : PreviewDistance = max(6.0, realSpeed *0.5);
-    if(changeRoad ||transferPieriod){
-        PreviewDistance=PreviewDistance+avoidX;
-    }
-    if(realSpeed <15){
-        PreviewDistance = max(4.0, realSpeed *0.4) ;
-    }
-
-    if (gpsTrace[0].v1 == 1)
-    {
-        KEang = 14; KEPos = 10;
-    }
-    else if (gpsTrace[0].v1 == 2 || gpsTrace[0].v1 == 3)
-    {
-        KEang = 14; KEPos = 10;
-    }
-    else if (gpsTrace[0].v1 == 4 || gpsTrace[0].v1 == 5)
-    {
-        KEang = 14; KEPos = 10;
-    }
-    else if (gpsTrace[0].v1 == 7 && (gpsTrace[0].v2 == 23 || gpsTrace[0].v2 == 24))
-    {
-        KEang = 18; KEPos = 50; PreviewDistance = 3;
-    }
-    else if (gpsTrace[0].v1 == 7)
-    {
-        KEang = 20; KEPos = 50; PreviewDistance = 4;
-    }
-
-
-    if (realSpeed > 40)	KEang = 10; KEPos = 8;
-    if (realSpeed > 50) KEang = 5;
-
-    double sumdis = 0;
-    int gpsIndex = 0;
-    std::vector<Point2D> farTrace;
-
-
-
-
-
-
-    for (int i = 1; i < gpsTrace.size() - 1; i++)
-    {
-        sumdis += GetDistance(gpsTrace[i - 1], gpsTrace[i]);
-        if (sumdis > PreviewDistance)
-        {
-            gpsIndex = i;
-            break;
-        }
-    }
-
-
-
-
-    EPos = gpsTrace[gpsIndex].x;
-
-    for (unsigned int i = max(0, gpsIndex - 3); i < min((size_t)(gpsIndex + 3), gpsTrace.size()); i++) {
-        farTrace.push_back(gpsTrace[gpsIndex]);
-    }
-    if (farTrace.size() == 0) {
-        EAng = 0;
-    }
-    else {
-        EAng = getAveDef(farTrace);
-    }
-
-    ang = KEang * EAng + KEPos * EPos + DEang * (EAng - lastEA) + DEPos * (EPos - lastEP);
-
-    lastEA = EAng;
-    lastEP = EPos;
-
-    if (ang > angleLimit) {
-        ang = angleLimit;
-    }
-    else if (ang < -angleLimit) {
-        ang = -angleLimit;
-    }
-    if (lastAng != iv::MaxValue) {
-        ang = 0.2 * lastAng + 0.8 * ang;
-        //ODS("lastAng：%d\n", lastAng);
-    }
-    lastAng = ang;
-    return ang;
-}
-
-
-
-
-int  iv::decition::Compute00::getSpeedPointIndex(std::vector<Point2D> gpsTrace, double realSpeed)
-{
-    int index = 1;
-    double sumdis = 0;
-    while (index < gpsTrace.size() && sumdis < realSpeed)
-        sumdis += GetDistance(gpsTrace[index - 1], gpsTrace[index++]);
-
-    if (index == gpsTrace.size())
-        return index - 1;
-
-    if (abs(sumdis - realSpeed) > abs(sumdis - GetDistance(gpsTrace[index - 1], gpsTrace[index]) - realSpeed))
-        index--;
-    return index;
-}
-
-iv::Point2D iv::decition::Compute00::getLidarObsPoint(std::vector<Point2D> gpsTrace, iv::LidarGridPtr lidarGridPtr) {
-
-    iv::Point2D obsPoint(-1, -1);
-    vector<Point2D> gpsTraceLeft;
-    vector<Point2D> gpsTraceRight;
-    float xiuzheng=0;
-    if(!ServiceCarStatus.useMobileEye){
-        xiuzheng=0-ServiceCarStatus.msysparam.lidarGpsXiuzheng;
-    }
-
-    ServiceCarStatus.obsTraceLeft.clear();
-    ServiceCarStatus.obsTraceRight.clear();
-
-    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));
-
-        gpsTraceLeft.push_back(ptLeft);
-        gpsTraceRight.push_back(ptRight);
-
-
-        TracePoint obsptleft(ptLeft.x,ptLeft.y);
-        ServiceCarStatus.obsTraceLeft.push_back(obsptleft);
-
-        TracePoint obsptright(ptRight.x,ptRight.y);
-        ServiceCarStatus.obsTraceLeft.push_back(obsptright);
-
-
-    }
-
-
-    bool isRemove = false;
-
-    for (int j = 1; j < gpsTrace.size() - 1 && !isRemove; j++)
-    {
-
-        if (!isRemove && gpsTrace[j].y>ServiceCarStatus.msysparam.lidarGpsXiuzheng)
-        {
-            int count = 0;
-
-            for (double length = 0; length <= ServiceCarStatus.msysparam.vehWidth; length += 0.4)
-            {
-                double ptx = gpsTraceLeft[j].x + (gpsTraceRight[j].x - gpsTraceLeft[j].x) / ServiceCarStatus.msysparam.vehWidth * length;
-                double pty = gpsTraceLeft[j].y + (gpsTraceRight[j].y - gpsTraceLeft[j].y) / ServiceCarStatus.msysparam.vehWidth * length;
-
-                //				int dx = (int)(ptx / gridwide * 2 + (ptx / abs(ptx))) / 2 + centerx;  //+(ptx / abs(ptx))) / 2左右多出一半的车宽（1米）
-                //				int dy = (int)(pty / gridwide * 2 + (pty / abs(pty))) / 2 + centery;
-                int dx = (ptx + gridwide*(double)centerx)/gridwide;
-                int dy = (pty + gridwide*(double)centery+xiuzheng)/gridwide;
-
-                if (dx >= 0 && dx <grx && dy >= 0 && dy < gry)
-                {
-                    //					if (lidarGridPtr[dx * (iv::gry + 1) + dy].ob != 0)
-                    if (lidarGridPtr[dx * (iv::gry) + dy].ob != 0)
-                    {
-                        count++; obsPoint.x = ptx; obsPoint.y = pty;
-                    }
-                }
-            }
-
-            j++;
-
-            for (double length = 0; length <= ServiceCarStatus.msysparam.vehWidth; length += 0.4)
-            {
-                double ptx = gpsTraceLeft[j].x + (gpsTraceRight[j].x - gpsTraceLeft[j].x) / ServiceCarStatus.msysparam.vehWidth * length;
-                double pty = gpsTraceLeft[j].y + (gpsTraceRight[j].y - gpsTraceLeft[j].y) / ServiceCarStatus.msysparam.vehWidth * length;
-
-                //				int dx = (int)(ptx / gridwide * 2 + (ptx / abs(ptx))) / 2 + centerx;
-                //				int dy = (int)(pty / gridwide * 2 + (pty / abs(pty))) / 2 + centery;
-                int dx = (ptx + gridwide*(double)centerx)/gridwide;
-                int dy = (pty + gridwide*(double)centery+xiuzheng)/gridwide;
-
-                if (dx >= 0 && dx <grx && dy >= 0 && dy < gry)
-                {
-                    //					if (lidarGridPtr[dx * (iv::gry + 1) + dy].ob != 0)
-                    if (lidarGridPtr[dx * (iv::gry) + dy].ob != 0)
-                    {
-                        count++; obsPoint.x = ptx; obsPoint.y = pty;
-                    }
-                }
-            }
-
-            if (count >= 2)
-            {
-                obsPoint.x = gpsTrace[j].x;
-                obsPoint.y = gpsTrace[j].y;
-
-                isRemove = true;
-                //		DecideGps00().lidarDistance = obsPoint.y;
-                return obsPoint;
-            }
-        }
-    }
-    //	DecideGps00().lidarDistance = obsPoint.y;
-    return obsPoint;
-}
-
-//1220
-iv::Point2D iv::decition::Compute00::getLidarRearObsPoint(std::vector<Point2D> gpsTrace, iv::LidarGridPtr lidarGridPtr) {
-
-    iv::Point2D obsPoint(-1, -1);
-    vector<Point2D> gpsTraceLeft;
-    vector<Point2D> gpsTraceRight;
-    float xiuzheng=0;
-    if(!ServiceCarStatus.useMobileEye){
-        xiuzheng=0-ServiceCarStatus.msysparam.rearLidarGpsXiuzheng;
-    }
-    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-0.3) / 2 * cos(anglevalue + PI / 2),
-                       carFronty + (ServiceCarStatus.msysparam.vehWidth-0.3) / 2 * sin(anglevalue + PI / 2));
-
-        Point2D ptRight(carFrontx + (ServiceCarStatus.msysparam.vehWidth-0.3) / 2 * cos(anglevalue - PI / 2),
-                        carFronty + (ServiceCarStatus.msysparam.vehWidth-0.3) / 2 * sin(anglevalue - PI / 2));
-
-        gpsTraceLeft.push_back(ptLeft);
-        gpsTraceRight.push_back(ptRight);
-
-    }
-
-
-    bool isRemove = false;
-
-    for (int j = 1; j < gpsTrace.size() - 1 && !isRemove; j++)
-    {
-
-        if (!isRemove && gpsTrace[j].y<(0-ServiceCarStatus.msysparam.rearGpsXiuzheng) )
-        {
-            int count = 0;
-
-            for (double length = 0; length <= ServiceCarStatus.msysparam.vehWidth; length += 0.4)
-            {
-                double ptx = gpsTraceLeft[j].x + (gpsTraceRight[j].x - gpsTraceLeft[j].x) / ServiceCarStatus.msysparam.vehWidth * length;
-                double pty = gpsTraceLeft[j].y + (gpsTraceRight[j].y - gpsTraceLeft[j].y) / ServiceCarStatus.msysparam.vehWidth * length;
-
-                //				int dx = (int)(ptx / gridwide * 2 + (ptx / abs(ptx))) / 2 + centerx;  //+(ptx / abs(ptx))) / 2左右多出一半的车宽（1米）
-                //				int dy = (int)(pty / gridwide * 2 + (pty / abs(pty))) / 2 + centery;
-                int dx = (ptx + gridwide*(double)centerx)/gridwide;
-
-                dx=grx-dx;//1227
-
-                int dy = (pty + gridwide*(double)centery+xiuzheng)/gridwide;
-
-                if (dx >= 0 && dx <grx && dy >= 0 && dy < gry)
-                {
-                    //					if (lidarGridPtr[dx * (iv::gry + 1) + dy].ob != 0)
-                    if (lidarGridPtr[dx * (iv::gry) + dy].ob != 0)
-                    {
-                        count++; obsPoint.x = ptx; obsPoint.y = pty;
-                    }
-                }
-            }
-
-            j++;
-
-            for (double length = 0; length <= ServiceCarStatus.msysparam.vehWidth; length += 0.4)
-            {
-                double ptx = gpsTraceLeft[j].x + (gpsTraceRight[j].x - gpsTraceLeft[j].x) / ServiceCarStatus.msysparam.vehWidth * length;
-                double pty = gpsTraceLeft[j].y + (gpsTraceRight[j].y - gpsTraceLeft[j].y) / ServiceCarStatus.msysparam.vehWidth * length;
-
-                //				int dx = (int)(ptx / gridwide * 2 + (ptx / abs(ptx))) / 2 + centerx;
-                //				int dy = (int)(pty / gridwide * 2 + (pty / abs(pty))) / 2 + centery;
-                int dx = (ptx + gridwide*(double)centerx)/gridwide;
-
-                dx=grx-dx;//1227
-                int dy = (pty + gridwide*(double)centery+xiuzheng)/gridwide;
-
-                if (dx >= 0 && dx <grx && dy >= 0 && dy < gry)
-                {
-                    //					if (lidarGridPtr[dx * (iv::gry + 1) + dy].ob != 0)
-                    if (lidarGridPtr[dx * (iv::gry) + dy].ob != 0)
-                    {
-                        count++; obsPoint.x = ptx; obsPoint.y = pty;
-                    }
-                }
-            }
-
-            if (count >= 2)
-            {
-                obsPoint.x = gpsTrace[j].x;
-                obsPoint.y = gpsTrace[j].y;
-
-                isRemove = true;
-                //		DecideGps00().lidarDistance = obsPoint.y;
-                return obsPoint;
-            }
-        }
-    }
-    //	DecideGps00().lidarDistance = obsPoint.y;
-    return obsPoint;
-}
-
-
-iv::Point2D iv::decition::Compute00::getLidarObsPointAvoid(std::vector<Point2D> gpsTrace, iv::LidarGridPtr lidarGridPtr,double & lidarDistanceAvoid) {
-
-    iv::Point2D obsPoint(-1, -1);
-    vector<Point2D> gpsTraceLeft;
-    vector<Point2D> gpsTraceRight;
-    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);
-
-        //1127 fanwei xiuzheng
-        float buchang=0;
-        Point2D ptLeft(carFrontx + (ServiceCarStatus.msysparam.vehWidth+(buchang)*2) / 2 * cos(anglevalue + PI / 2),
-                       carFronty +  (ServiceCarStatus.msysparam.vehWidth+(buchang)*2) / 2 * sin(anglevalue + PI / 2));
-
-        Point2D ptRight(carFrontx +  (ServiceCarStatus.msysparam.vehWidth+(buchang)*2) / 2 * cos(anglevalue - PI / 2),
-                        carFronty +  (ServiceCarStatus.msysparam.vehWidth+(buchang)*2) / 2 * sin(anglevalue - PI / 2));
-
-        gpsTraceLeft.push_back(ptLeft);
-        gpsTraceRight.push_back(ptRight);
-
-    }
-
-
-    bool isRemove = false;
-
-    for (int j = 1; j < gpsTrace.size() - 1 && !isRemove; j++)
-    {
-
-        if (!isRemove && gpsTrace[j].y>2.5 && gpsTraceLeft[j].y>2.5 && gpsTraceRight[j].y>2.5)
-        {
-            int count = 0;
-
-            for (double length = 0; length <= ServiceCarStatus.msysparam.vehWidth; length += 0.4)
-            {
-                double ptx = gpsTraceLeft[j].x + (gpsTraceRight[j].x - gpsTraceLeft[j].x) / ServiceCarStatus.msysparam.vehWidth * length;
-                double pty = gpsTraceLeft[j].y + (gpsTraceRight[j].y - gpsTraceLeft[j].y) / ServiceCarStatus.msysparam.vehWidth * length;
-
-                int dx = (int)(ptx / gridwide * 2 + (ptx / abs(ptx))) / 2 + centerx;  //*2左右多出一半的车宽（1米）
-                int dy = (int)(pty / gridwide * 2 + (pty / abs(pty))) / 2 + centery;
-
-                if (dx >= 0 && dx <grx && dy >= 0 && dy < gry)
-                {
-                    if (lidarGridPtr[dx * (iv::gry + 1) + dy].ob != 0)
-                    {
-                        count++; obsPoint.x = ptx; obsPoint.y = pty;
-                    }
-                }
-            }
-
-            j++;
-
-            for (double length = 0; length <= ServiceCarStatus.msysparam.vehWidth; length += 0.4)
-            {
-                double ptx = gpsTraceLeft[j].x + (gpsTraceRight[j].x - gpsTraceLeft[j].x) / ServiceCarStatus.msysparam.vehWidth * length;
-                double pty = gpsTraceLeft[j].y + (gpsTraceRight[j].y - gpsTraceLeft[j].y) / ServiceCarStatus.msysparam.vehWidth * length;
-
-                int dx = (int)(ptx / gridwide * 2 + (ptx / abs(ptx))) / 2 + centerx;
-                int dy = (int)(pty / gridwide * 2 + (pty / abs(pty))) / 2 + centery;
-
-                if (dx >= 0 && dx <grx && dy >= 0 && dy < gry)
-                {
-                    if (lidarGridPtr[dx * (iv::gry + 1) + dy].ob != 0)
-                    {
-                        count++; obsPoint.x = ptx; obsPoint.y = pty;
-                    }
-                }
-            }
-
-            if (count >= 2)
-            {
-                obsPoint.x = gpsTrace[j].x;
-                obsPoint.y = gpsTrace[j].y;
-
-                isRemove = true;
- //               DecideGps00().lidarDistanceAvoid = obsPoint.y;
-                lidarDistanceAvoid = obsPoint.y;
-                return obsPoint;
-            }
-        }
-    }
-    //	DecideGps00().lidarDistanceAvoid = obsPoint.y;
-
-    return obsPoint;
-}
-
-
-
-
-//int  iv::decition::Compute00::getEsrIndex(std::vector<Point2D> gpsTrace, std::vector<ObstacleBasic> esrRadars) {
-//	bool isRemove = false;
-//
-//	for (int j = 1; j < gpsTrace.size() - 1 && !isRemove; j++)
-//	{
-//
-//		for (int i = 0; i < esrRadars.size(); i++)
-//			if ((esrRadars[i].nomal_y) != 0)
-//			{
-//				double xxx = esrRadars[i].nomal_x + Esr_Offset;
-//				double yyy = esrRadars[i].nomal_y;
-//
-//				if (abs(xxx - gpsTrace[j].x) <= 3.0*ServiceCarStatus.msysparam.vehWidth / 4.0 && abs(yyy - (gpsTrace[j].y)) <= 1)
-//				{
-//
-//					if (lastEsrID == (esrRadars[i]).esr_ID)
-//					{
-//						lastEsrCount++;
-//					}
-//					else
-//					{
-//						lastEsrCount = 0;
-//					}
-//
-//					if (lastEsrCount >= 3)
-//					{
-//						return i;
-//					}
-//
-//					lastEsrID = (esrRadars[i]).esr_ID;
-//				}
-//			}
-//	}
-//	return -1;
-//}
-
-
-
-
-int  iv::decition::Compute00::getEsrIndex(std::vector<Point2D> gpsTrace,int roadNum,int *esrPathpoint,const double xiuzhengCs) {
-    bool isRemove = false;
-
-    float xiuzheng=0;
-    if(!ServiceCarStatus.useMobileEye){
-        xiuzheng=ServiceCarStatus.msysparam.radarGpsXiuzheng;
-    }
-
-//    float fxiuzhengCs = DecideGps00().xiuzhengCs;
-    float fxiuzhengCs = xiuzhengCs;
-    int nsize = gpsTrace.size();
-    for (int j = 1; j < nsize - 1 && !isRemove; j++)
-    {
-
-        for (int i = 0; i < 64; i++)
-            if ((ServiceCarStatus.obs_radar[i].nomal_y) != 0 && (ServiceCarStatus.obs_radar[i].valid))
-            {
-                double xxx = ServiceCarStatus.obs_radar[i].nomal_x + Esr_Offset;
-                double yyy = ServiceCarStatus.obs_radar[i].nomal_y+ xiuzheng;
-
-                /*ODS("\nESR毫米波检测物体X距离:%f\n", xxx);
-                ODS("\nESR毫米波检测物体Y距离:%f\n", yyy);*/
-
-//优化
-//                if(sqrt((xxx - gpsTrace[j].x)*(xxx - gpsTrace[j].x) + (yyy - gpsTrace[j].y)*(yyy - gpsTrace[j].y)) < (1.0*ServiceCarStatus.msysparam.vehWidth / 2.0+DecideGps00().xiuzhengCs)){
-//                    *esrPathpoint = j;
-//                    return i;
-//                }
-
-
-                if (abs(xxx - gpsTrace[j].x) <= (3.0*ServiceCarStatus.msysparam.vehWidth / 4.0+fxiuzhengCs) && abs(yyy - (gpsTrace[j].y)) <= 1)
-                {
-                    return i;
-
-
-                    if (lastEsrID == i)
-                    {
-                        lastEsrCount++;
-                    }
-                    else
-                    {
-                        lastEsrCount = 0;
-                    }
-
-                    if(yyy>50 ){
-                        if (lastEsrCount >=200)
-                        {
-                            return i;
-                        }
-                    }
-                    else if (lastEsrCount >= 1)
-                    {
-                        return i;
-                    }
-
-                    lastEsrID = i;
-                }
-            }
-    }
-    return -1;
-}
-
-
-
-
-int  iv::decition::Compute00::getRearEsrIndex(std::vector<Point2D> gpsTrace,int roadNum,const double xiuzhengCs) {
-    bool isRemove = false;
-
-    float xiuzheng = ServiceCarStatus.msysparam.rearRadarGpsXiuzheng;
-    for (int j = 1; j < gpsTrace.size() - 1 && !isRemove; j++)
-    {
-
-        for (int i = 0; i < 64; i++)
-            if ((ServiceCarStatus.obs_rear_radar[i].nomal_y) != 0 && (ServiceCarStatus.obs_rear_radar[i].valid))
-            {
-                double xxx = 0-(ServiceCarStatus.obs_rear_radar[i].nomal_x + Esr_Offset);
-                double yyy = 0-(ServiceCarStatus.obs_rear_radar[i].nomal_y+ xiuzheng);
-
-                if(ServiceCarStatus.msysparam.mvehtype=="qingyuan"){
-                    xxx=0-xxx;
-                }
-
-                /*ODS("\nESR毫米波检测物体X距离:%f\n", xxx);
-                ODS("\nESR毫米波检测物体Y距离:%f\n", yyy);*/
-
-
-//                if (abs(xxx - gpsTrace[j].x) <= (3.0*ServiceCarStatus.msysparam.vehWidth / 4.0+DecideGps00().xiuzhengCs) && abs(yyy - (gpsTrace[j].y)) <= 1)
-                if (abs(xxx - gpsTrace[j].x) <= (3.0*ServiceCarStatus.msysparam.vehWidth / 4.0+xiuzhengCs) && abs(yyy - (gpsTrace[j].y)) <= 1)
-
-                {
-
-                    if (lastEsrID == i)
-                    {
-                        lastEsrCount++;
-                    }
-                    else
-                    {
-                        lastEsrCount = 0;
-                    }
-
-                    if(yyy>50 ){
-                        if (lastEsrCount >=200)
-                        {
-                            return i;
-                        }
-                    }
-                    else if (lastEsrCount >= 1)
-                    {
-                        return i;
-                    }
-
-                    lastEsrID = i;
-                }
-            }
-    }
-    return -1;
-}
-
-
-
-
-
-//int  iv::decition::Compute00::getEsrIndex(std::vector<Point2D> gpsTrace,int roadNum) {
-//    bool isRemove = false;
-
-//    for (int j = 1; j < gpsTrace.size() - 1 && !isRemove; j++)
-//    {
-
-//        for (int i = 0; i < 64; i++)
-//            if ((examed_obs_radar[i].nomal_y) != 0 && (examed_obs_radar[i].valid))
-//            {
-//                double xxx = examed_obs_radar[i].nomal_x + Esr_Offset;
-//                double yyy = examed_obs_radar[i].nomal_y+ Esr_Y_Offset;
-
-//                /*ODS("\nESR毫米波检测物体X距离:%f\n", xxx);
-//                ODS("\nESR毫米波检测物体Y距离:%f\n", yyy);*/
-
-
-//                if (abs(xxx - gpsTrace[j].x) <= 3.0*ServiceCarStatus.msysparam.vehWidth / 4.0 && abs(yyy - (gpsTrace[j].y)) <= 1)
-//                {
-
-//                    if (lastEsrID == i)
-//                    {
-//                        lastEsrCount++;
-//                    }
-//                    else
-//                    {
-//                        lastEsrCount = 0;
-//                    }
-
-//                    if(yyy>50 ){
-//                        if (lastEsrCount >=200)
-//                        {
-//                            return i;
-//                        }
-//                    }
-//                    else if (lastEsrCount >= 3)
-//                    {
-//                        return i;
-//                    }
-
-//                    lastEsrID = i;
-//                }
-//            }
-//    }
-//    return -1;
-//}
-
-
-
-int  iv::decition::Compute00::getEsrIndexAvoid(std::vector<Point2D> gpsTrace) {
-    bool isRemove = false;
-
-    for (int j = 1; j < gpsTrace.size() - 1 && !isRemove; j++)
-    {
-
-        for (int i = 0; i < 64; i++)
-            if ((ServiceCarStatus.obs_radar[i].nomal_y) != 0 && (ServiceCarStatus.obs_radar[i].valid))
-            {
-                double xxx = ServiceCarStatus.obs_radar[i].nomal_x + Esr_Offset;
-                double yyy = ServiceCarStatus.obs_radar[i].nomal_y;
-
-                if (abs(xxx - gpsTrace[j].x) <= 3.0*ServiceCarStatus.msysparam.vehWidth / 4.0 && abs(yyy - (gpsTrace[j].y)) <= 1)
-                {
-
-                    if (lastEsrIDAvoid == i)
-                    {
-                        lastEsrCountAvoid++;
-                    }
-                    else
-                    {
-                        lastEsrCountAvoid = 0;
-                    }
-
-                    if (lastEsrCountAvoid >= 6)
-                    {
-                        return i;
-                    }
-
-                    lastEsrIDAvoid = i;
-                }
-            }
-    }
-    return -1;
-}
-
-
-
-
-
-
-
-//double iv::decition::Compute00::getObsSpeed(Point2D obsPoint, std::vector<ObstacleBasic> esrRadars,double realSecSpeed) {
-//	double obsSpeed = 0 - realSecSpeed;
-//	double minDis = iv::MaxValue;
-//	for (int i = 0; i < esrRadars.size(); i++)
-//		if ((esrRadars[i].nomal_y) != 0)
-//		{
-//			double xxx = esrRadars[i].nomal_x + Esr_Offset;
-//			double yyy = esrRadars[i].nomal_y;
-//
-//			if (abs(xxx - obsPoint.x) < 4 && abs(yyy - obsPoint.y) < 2)
-//			{
-//				double tmpDis =sqrt((xxx - obsPoint.x) * (xxx - obsPoint.x) + (yyy - obsPoint.y) * (yyy - obsPoint.y));
-//				if (tmpDis < minDis)
-//				{
-//					minDis = tmpDis;
-//					obsSpeed = esrRadars[i].speed_y;
-//				}
-//			}
-//		}
-//
-//	return obsSpeed;
-//
-//
-//}
-
-
-
-
-double iv::decition::Compute00::getObsSpeed(Point2D obsPoint, double realSecSpeed) {
-    double obsSpeed = 0 - realSecSpeed;
-    double minDis = iv::MaxValue;
-    for (int i = 0; i < 64; i++)
-        if ((ServiceCarStatus.obs_radar[i].nomal_y) != 0 && ServiceCarStatus.obs_radar[i].valid)
-        {
-            double xxx = ServiceCarStatus.obs_radar[i].nomal_x + Esr_Offset;
-            double yyy = ServiceCarStatus.obs_radar[i].nomal_y + Esr_Y_Offset;
-
-            if (abs(xxx - obsPoint.x) < 4 && abs(yyy - obsPoint.y) < 2)
-            {
-                double tmpDis = sqrt((xxx - obsPoint.x) * (xxx - obsPoint.x) + (yyy - obsPoint.y) * (yyy - obsPoint.y));
-                if (tmpDis < minDis)
-                {
-                    minDis = tmpDis;
-                    obsSpeed = ServiceCarStatus.obs_radar[i].speed_y;
-                }
-            }
-        }
-
-    return obsSpeed;
-}
-
-
-
-
-double iv::decition::Compute00::getDecideAvoidAngle(std::vector<Point2D>  gpsTrace, double realSpeed, float avoidX
-                                                    ,const bool readyParkMode,const int gpsLineParkIndex) {
-
-    double ang = 0;
-    double EPos = 0, EAng = 0;
-
-    double KEang = 14, KEPos = 10, DEang = 0, DEPos = 0;
-
-    double PreviewDistance = max(6.0, realSpeed / 3.6 * 1.8);//预瞄距离
-
-
-    if (gpsTrace[0].v1 == 1)
-    {
-        KEang = 10; KEPos = 8;
-        if (realSpeed > 60) KEang = 5;
-    }
-    else if (gpsTrace[0].v1 == 2 || gpsTrace[0].v1 == 3)
-    {
-        KEang = 14; KEPos = 10;
-    }
-    else if (gpsTrace[0].v1 == 4 || gpsTrace[0].v1 == 5)
-    {
-        KEang = 14; KEPos = 10;
-    }
-    else if (gpsTrace[0].v1 == 7 && (gpsTrace[0].v2 == 23 || gpsTrace[0].v2 == 24))
-    {
-        KEang = 18; KEPos = 50; PreviewDistance = 3;
-    }
-    else if (gpsTrace[0].v1 == 7)
-    {
-        KEang = 20; KEPos = 50; PreviewDistance = 4;
-    }
-
-
-    double sumdis = 0;
-    int gpsIndex = 0;
-    std::vector<Point2D> farTrace;
-
-
-
-
-
-
-    for (int i = 1; i < gpsTrace.size() - 1; i++)
-    {
-        sumdis += GetDistance(gpsTrace[i - 1], gpsTrace[i]);
-        if (sumdis > PreviewDistance)
-        {
-            gpsIndex = i;
-            break;
-        }
-    }
-
-//    if ((DecideGps00().readyParkMode) && (gpsIndex + 10>DecideGps00().gpsLineParkIndex))
-//    {
-//        gpsIndex = DecideGps00().gpsLineParkIndex;
-//    }
-
-    if ((readyParkMode) && (gpsIndex + 10>gpsLineParkIndex))
-    {
-        gpsIndex = gpsLineParkIndex;
-    }
-
-
-
-    EPos = gpsTrace[gpsIndex].x + avoidX;
-
-    for (unsigned int i = max(0, gpsIndex - 3); i < min((size_t)(gpsIndex + 3), gpsTrace.size()); i++) {
-        farTrace.push_back(gpsTrace[gpsIndex]);
-    }
-    if (farTrace.size() == 0) {
-        EAng = 0;
-    }
-    else {
-        EAng = getAvoidAveDef(farTrace, avoidX);
-    }
-
-    ang = KEang * EAng + KEPos * EPos + DEang * (EAng - lastEA) + DEPos * (EPos - lastEP);
-
-    lastEA = EAng;
-    lastEP = EPos;
-
-    if (ang > angleLimit) {
-        ang = angleLimit;
-    }
-    else if (ang < -angleLimit) {
-        ang = -angleLimit;
-    }
-    if (lastAng != iv::MaxValue) {
-        ang = 0.2 * lastAng + 0.8 * ang;
-        //ODS("lastAng：%d\n", lastAng);
-    }
-    lastAng = ang;
-
-    return ang;
-}
-
-
-std::vector<iv::GPSData>   iv::decition::Compute00::getBesideGpsMapLine(iv::GPS_INS now_gps_ins, vector<iv::GPSData>gpsMapLine, float avoidX) {
-
-    vector<vector<iv::GPSData>> maps;
-    vector<iv::GPSData> gpsMapLineBeside;
-    int sizeN = gpsMapLine.size();
-    for (int i = 1; i < sizeN; i++)
-    {
-        iv::GPSData gpsData(new GPS_INS);
-        double xx = gpsMapLine[i]->gps_x - now_gps_ins.gps_x;
-        double yy = gpsMapLine[i]->gps_y - now_gps_ins.gps_y;
-        double lng = ServiceCarStatus.location->ins_heading_angle;
-
-
-        double x0 = xx * cos(lng * PI / 180) - yy * sin(lng * PI / 180);
-        double y0 = xx * sin(lng * PI / 180) + yy * cos(lng * PI / 180);
-        double k1 = sin((90 + (gpsMapLine[i]->ins_heading_angle - lng)) * PI / 180);
-        double k2 = cos((90 + (gpsMapLine[i]->ins_heading_angle - lng)) * PI / 180);
-
-        //	memcpy(&gpsData, &gpsMapLine[i], sizeof(gpsData));
-
-        gpsData->speed_mode = gpsMapLine[i]->speed_mode;
-        gpsData->gps_x = x0 + k1 * avoidX;
-        gpsData->gps_y = y0 + k2 * avoidX;
-        gpsMapLineBeside.push_back(gpsData);
-
-    }
-    return gpsMapLineBeside;
-
-}
-
-
-
-//double iv::decition::Compute00::getDecideAngleByLane(double realSpeed) {
-
-//    double ang = 0;
-//    double EPos = 0, EAng = 0;
-
-// //   double KEang = 14, KEpos = 10, DEang = 0, DEpos = 0;
-//       double KEang = 5, KEPos = 30, DEang = 0, DEPos = 0;
-
-// //   double PreviewDistance = max(6.0, realSpeed / 3.6 * 1.8);//预瞄距离
-
-//    double PreviewDistance;//预瞄距离
-//    realSpeed > 40 ? PreviewDistance = max(6.0, realSpeed *0.6) : PreviewDistance = max(6.0, realSpeed *0.5);
-
-
-
-
-
-////    if (realSpeed > 40)	KEang = 10; KEpos = 8;
-////        if (realSpeed > 50) KEang = 5;
-
-
-
-
-
-//double c1 = ServiceCarStatus.aftermarketLane.dist_to_lane_l;
-//double c2 = ServiceCarStatus.aftermarketLane.dist_to_lane_r;
-//double a = ServiceCarStatus.Lane.curvature;
-//double b = ServiceCarStatus.Lane.heading;
-//double c = (c1+c2)*0.5;
-//double x= PreviewDistance;
-//double y;
-
-
-//y=a*x*x+b*x+c;
-
-//   // EPos = y;
-//EPos=c;
-
-
-//  //  EAng=atan(2*a*x+b) / PI * 180;
-//    EAng=ServiceCarStatus.Lane.yaw;
-//        ang = KEang * EAng + KEPos * EPos + DEang * (EAng - lastEA) + DEPos * (EPos - lastEP);
-
-//        lastEA = EAng;
-//        lastEP = EPos;
-
-
-//           std::cout << "\nEPos:%f\n" << EPos << std::endl;
-//            std::cout << "\nEAng:%f\n" << EAng << std::endl;
-
-
-//        if (ang > angleLimit) {
-//            ang = angleLimit;
-//        }
-//        else if (ang < -angleLimit) {
-//            ang = -angleLimit;
-//        }
-//        if (lastAng != iv::MaxValue) {
-//            ang = 0.2 * lastAng + 0.8 * ang;
-//            //ODS("lastAng：%d\n", lastAng);
-//        }
-//        lastAng = ang;
-//        return ang;
-//    }
-
-
-
-double IEPos = 0, IEang = 0;
-
-
-double iv::decition::Compute00::getDecideAngleByLanePID(double realSpeed) {
-
-    double ang = 0;
-    double EPos = 0, EAng = 0;
-    double Curve=0;
-    double KEang = 14, KEPos = 10, DEang = 0, DEPos = 0;
-    double KCurve=120;
-    double KIEPos = 0, KIEang = 0;
-    //   double PreviewDistance = max(6.0, realSpeed / 3.6 * 1.8);//预瞄距离
-
-    double PreviewDistance;//预瞄距离
-
-    int confL=ServiceCarStatus.aftermarketLane.lane_conf_left;
-    int confR=ServiceCarStatus.aftermarketLane.lane_conf_right;
-    int conf =min(confL,confR);
-
-    realSpeed > 40 ? PreviewDistance = max(6.0, realSpeed *0.6) : PreviewDistance = max(6.0, realSpeed *0.5);
-
-
-
-
-
-    if (realSpeed > 40)	KEang = 10; KEPos = 8;
-    if (realSpeed > 50) KEang = 5;
-
-    KEPos = 20;
-    KEang = 200;
-    //KEang = 15;
-
-    double c1 = ServiceCarStatus.aftermarketLane.dist_to_lane_l;
-    double c2 = ServiceCarStatus.aftermarketLane.dist_to_lane_r;
-    double a = ServiceCarStatus.Lane.curvature;
-    double b = ServiceCarStatus.Lane.heading;
-    double c = (c1+c2)*0.5;
-    double yaw= ServiceCarStatus.Lane.yaw;
-
-    double x= PreviewDistance;
-    double y;
-
-
-    y=c-(a*x*x+b*x);
-    double difa=0-(atan(2*a*x+b) / PI * 180);
-    Curve=0-a;
-
-    //EAng=difa;
-    //EPos=y;
-    EAng= 0-b;
-    EPos = c;
-    DEang = 10;
-    DEPos = 20;
-    //DEang = 20;
-    //DEPos = 10;
-
-    IEang = EAng+0.7*IEang;
-    IEPos = EPos+0.7*IEPos;
-    KIEang = 0;
-    //KIEang = 0.5;
-    KIEPos =2;
-
-
-
-    if(abs(confL)>=2&&abs(confR)>=2){
-        //ang = KEang * EAng + KEPos * EPos + DEang * (EAng - lastEA) + DEPos * (EPos - lastEP)+ KIEang * IEang + KIEPos * IEPos;
-        ang = KEang * EAng + KEPos * EPos +KCurve*Curve+ DEang * (EAng - lastEA) + DEPos * (EPos - lastEP)+ KIEang * IEang + KIEPos * IEPos;
-    }else{
-        ang=lastAng;
-    }
-    //if(lastAng!=0&&abs(ang-lastAng)>20)ang=lastAng;
-
-    lastEA = EAng;
-    lastEP = EPos;
-
-    if (ang > angleLimit) {
-        ang = angleLimit;
-    }
-    else if (ang < -angleLimit) {
-        ang = -angleLimit;
-    }
-    if (lastAng != iv::MaxValue) {
-        ang = 0.2 * lastAng + 0.8 * ang;
-        //ODS("lastAng：%d\n", lastAng);
-    }
-    lastAng = ang;
-    return ang;
-}
-
-
-
-double iv::decition::Compute00::bocheCompute(GPS_INS nowGps, GPS_INS aimGps) {
-
-    GaussProjCal(aimGps.gps_lng, aimGps.gps_lat, &aimGps.gps_x, &aimGps.gps_y);
-    Point2D pt = Coordinate_Transfer(nowGps.gps_x, nowGps.gps_y, aimGps);
-
-
-
-    double x_1 = pt.x;
-    double y_1 = pt.y;
-    double angle_1 = getQieXianAngle(nowGps,aimGps);
-    double x_2 = 0.0, y_2 = 0.0;
-    double steering_angle;
-    double l = 2.950;
-    double r =6;
-    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的切点
-    double x_t_3, y_t_3, x_t_4, y_t_4;//圆形2的切点
-    double g_1 = tan(angle_1);
-    double car_pos[3] = { x_1,y_1,g_1 };
-    double parking_pos[2] = { x_2,y_2 };
-    double g_3;
-    double t[4][2];
-    double p[4];
-    double  s1, s2; //切点与车起始位置的距离
-    double  min;
-    int  min_i;
-
-    //g_3 = 0 - 0.5775;
-    g_3 = pingfenxian_xielv(x_1, y_1, x_2, y_2, angle_1);
-    //交点
-    x_3 = 0.0;//(y_1 - y_2 + g_2*x_2 - g_1*x_1) / (g_2 - g_1);
-    y_3 = y_1 - g_1 * x_1;
-    //圆心1
-    x_o_1 = r;
-    y_o_1 = g_3 * r + y_3;
-    //圆形1的切点1
-    x_t_1 = 0.0;
-    y_t_1 = g_3 * r + y_3;
-    //圆形1的切点2
-    if (g_1 == 0)
-    {
-        x_t_2 = r;
-        y_t_2 = y_1 - g_1 * x_1;
-    }
-    else
-    {
-        y_t_2 = (y_1 + g_1 * x_o_1 + y_o_1 * g_1*g_1 - g_1 * x_1) / (1 + g_1 * g_1);
-        x_t_2 = (y_t_2 + g_1 * x_1 - y_1) / g_1;
-
-    }
-    //圆心2
-    x_o_2 = 0 - r;
-    y_o_2 = y_3 - g_3 * r;
-    //圆形2的切点1
-    x_t_3 = 0;
-    y_t_3 = y_3 - g_3 * r;
-    //圆形2的切点2
-    if (g_1 == 0)
-    {
-        x_t_4 = 0 - r;
-        y_t_4 = y_1 - g_1 * x_1;
-    }
-    else
-    {
-        y_t_4 = (y_1 + g_1 * x_o_2 + y_o_2 * g_1*g_1 - g_1 * x_1) / (1 + g_1 * g_1);
-        x_t_4 = (y_t_4 + g_1 * x_1 - y_1) / g_1;
-
-    }
-    t[0][0] = x_t_1;
-    t[0][1] = y_t_1;
-    t[1][0] = x_t_2;
-    t[1][1] = y_t_2;
-    t[2][0] = x_t_3;
-    t[2][1] = y_t_3;
-    t[3][0] = x_t_4;
-    t[3][1] = y_t_4;
-    for (int i = 0; i < 4; i++)
-    {
-
-        p[i] = (t[i][0] - parking_pos[0])*(t[i][0] - parking_pos[0]) + (t[i][1] - parking_pos[1])*(t[i][1] - parking_pos[1]);
-
-    }
-    min = p[0];
-    min_i = 0;
-    for (int i = 1; i < 4; i++)
-    {
-
-        if (p[i] < min)
-        {
-            min = p[i]; min_i = i;
-        }
-    }
-    if (min_i < 2)
-    {
-        x_o = x_o_1;
-        y_o = y_o_1;
-        s1 = (x_t_1 - x_1)*(x_t_1 - x_1) + (y_t_1 - y_1)*(y_t_1 - y_1);
-        s2 = (x_t_2 - x_1)*(x_t_2 - x_1) + (y_t_2 - y_1)*(y_t_2 - y_1);
-        if (s1 < s2)
-        {
-            x_t_n = x_t_1;
-            y_t_n = y_t_1;
-            x_t_f = x_t_2;
-            y_t_f = y_t_2;
-        }
-        else
-        {
-            x_t_n = x_t_2;
-            y_t_n = y_t_2;
-            x_t_f = x_t_1;
-            y_t_f = y_t_1;
-
-        }
-    }
-    else
-    {
-        x_o = x_o_2;
-        y_o = y_o_2;
-        s1 = (x_t_3 - x_1)*(x_t_3 - x_1) + (y_t_3 - y_1)*(y_t_3 - y_1);
-        s2 = (x_t_4 - x_1)*(x_t_4 - x_1) + (y_t_4 - y_1)*(y_t_4 - y_1);
-
-        if (s1 < s2)
-        {
-
-            x_t_n = x_t_3;
-            y_t_n = y_t_3;
-            x_t_f = x_t_4;
-            y_t_f = y_t_4;
-        }
-        else
-        {
-            x_t_n = x_t_4;
-            y_t_n = y_t_4;
-            x_t_f = x_t_3;
-            y_t_f = y_t_3;
-        }
-
-
-
-
-    }
-    steering_angle = atan2(l, r);
-
-    if (x_t_n < 0)
-    {
-        steering_angle = 0 - steering_angle;
-    }
-
-    nearTpoint=Coordinate_UnTransfer(x_t_n, y_t_n, aimGps);
-    farTpoint = Coordinate_UnTransfer(x_t_f, y_t_f, aimGps);
-    bocheAngle = steering_angle*180/PI;
-
-    cout << "近切点：x_t_n=" << x_t_n << endl;
-    cout << "近切点：y_t_n=" << y_t_n << endl;
-    cout << "远切点：x_t_f=" << x_t_f << endl;
-    cout << "远切点：y_t_f=" << y_t_f << endl;
-    cout << "航向角：" << steering_angle << endl;
-
-
-    //    if (x_1 < 0 && y_1 > 0 && x_1 < x_t_n &&y_t_f > 0.1) {
-    //        return 1;
-    //    }
-    Point2D ptN = Coordinate_Transfer(nearTpoint.gps_x, nearTpoint.gps_y, nowGps);
-    double disA = GetDistance(aimGps,nowGps);
-    if(y_t_n>0 && ptN.y<0 && y_t_f>0.1 && disA<40){
-        return 1;
-    }
-
-    return 0;
-
-}
-
-
-
-//返回垂直平分线的斜率
-double iv::decition::Compute00::pingfenxian_xielv(double x_1, double y_1, double x_2, double y_2, double angle_1) {
-    double angl, x_3, angle_3;
-    if (tan(angle_1 == 0))
-    {
-        if ((x_1 - x_2) > 0 && ((y_1 - y_2) > 0))
-        {
-            angle_3 = 0 - 1;
-        }
-        else
-        {
-            angle_3 = 1;
-        }
-    }
-    else
-    {
-        x_3 = (tan(angle_1)*x_1 - y_1) / tan(angle_1);//车所在直线与x轴交点
-        angl = tan(angle_1);//车所在直线的斜率
-        if ((x_1 - x_2)>0 && ((y_1 - y_2)>0))//第一象限
-        {
-            if ((angl *x_3)<0)//车斜率与车直线的x轴交点异号
-            {
-                if (angl < 0)
-                {
-                    angle_3 = tan(PI*0.5 + (PI*0.5 - atan(fabs(angl))) *0.5);//垂直平分线斜率
-                }
-                else
-                {
-                    angle_3 = tan(PI*0.5 + (PI*0.5 + atan(fabs(angl))) *0.5);//垂直平分线斜率
-                }
-
-
-
-            }
-
-        }
-        else//第二象限
-        {
-            if ((angl*x_3)<0)//车斜率与车直线的x轴交点异号
-            {
-                if (angl < 0)
-                {
-                    angle_3 = tan(PI*0.5 - (PI*0.5 + atan(fabs(angl))) *0.5);//垂直平分线斜率
-                }
-                else
-                {
-                    angle_3 = tan(atan(fabs(angl)) + (PI*0.5 - atan(fabs(angl))) *0.5);//垂直平分线斜率
-                }
-
-            }
-        }
-    }
-
-    return angle_3;
-
-}
-
-
-
-double iv::decition::Compute00::getQieXianAngle(GPS_INS nowGps, GPS_INS aimGps) {
-    double heading = nowGps.ins_heading_angle *PI/180;
-    double x1 = nowGps.gps_x;
-    double y1 = nowGps.gps_y;
-    if (heading<=PI*0.5)
-    {
-        heading = 0.5*PI - heading;
-    }
-    else if (heading>PI*0.5 && heading<=PI*1.5) {
-        heading = 1.5*PI - heading;
-    }
-    else if (heading>PI*1.5) {
-        heading = 2.5*PI - heading;
-    }
-    double k1 = tan(heading);
-    double x = x1+10;
-    double y = k1 * x + y1 - (k1 * x1);
-    Point2D pt1 = Coordinate_Transfer(nowGps.gps_x, nowGps.gps_y, aimGps);
-    Point2D pt2 = Coordinate_Transfer(x, y, aimGps);
-    double xielv = (pt1.y - pt2.y) / (pt1.x - pt2.x);
-    double angle = atan(abs(xielv));
-    if (xielv<0)
-    {
-        angle = PI - angle;
-    }
-    return angle;
-
-}
-
-
-/*
-  chuizhicheweiboche
-  */
-
-
-int iv::decition::Compute00::bocheDirectCompute(GPS_INS nowGps, GPS_INS aimGps)
-{
-
-
-
-    double l=2.95;//轴距
-    double x_0 = 0, y_0 = 0.5;
-    double x_1, y_1;//车起点坐标
-    double ange1;//车航向角弧度
-    double x_2, y_2;//另一条与车直线在angle2和R_M 固定情况下过坐标点，第二个近切点
-    double real_rad;;//另一条直线的航向角弧度
-    double angle_3;//垂直平分线弧度
-    double x_3, y_3;//垂直平分线交点
-    double x_4, y_4;//另一条直线的远切点坐标，第二个远切点，已知
-    double x_o_1, y_o_1;//圆形1坐标
-    double x_o_2, y_o_2;//圆形2坐标
-    double x_t_n, y_t_n, x_t_f, y_t_f;//近切点和远切点
-    double min_rad;
-    double R_M; //后轴中点的转弯半径
-    double steering_angle;
-
-
-
-
-    GaussProjCal(aimGps.gps_lng, aimGps.gps_lat, &aimGps.gps_x, &aimGps.gps_y);
-    Point2D pt = Coordinate_Transfer(nowGps.gps_x, nowGps.gps_y, aimGps);
-    x_1=pt.x;
-    y_1=pt.y;
-    ange1=getQieXianAngle(nowGps,aimGps);
-
-    min_rad_zhuanxiang(&R_M , &min_rad);
-    qiedian_n(x_1,y_1,R_M,min_rad,&x_2 , &y_2, &real_rad);//计算另一条与车直线在angle2和R_M 固定情况下近切点：x_2, y_2
-    liangzhixian_jiaodian( x_1, y_1,  x_2, y_2,ange1,real_rad,&x_3 , &y_3);
-    chuizhipingfenxian_xielv( x_1, y_1, ange1, real_rad, min_rad,&angle_3);
-    yuanxin( x_2, y_2, x_3, y_3, real_rad, angle_3, R_M,&x_o_1,&y_o_1,&x_o_2,&y_o_2);
-    yuanxin_qiedian( ange1, x_o_1, y_o_1,  x_o_2, y_o_2,
-                     x_1, y_1, x_2, y_2, x_3, y_3, real_rad, angle_3, R_M,&x_t_n,&y_t_n,&x_t_f, &y_t_f);
-    steering_angle = atan2(l, R_M);
-    x_4 = 0.5;
-    y_4 = 0;
-    //for (int i = 0; i < 4; i++)
-    //{
-    //for (int j = 0; j < 2; j++)
-    //{
-    //	cout << t[i][j] << endl;
-    //}
-    //}
-    //cout << "min_rad:" << min_rad<< endl;
-    //cout << "jiaodian：x=" << x_3 << endl;
-    //cout << "jiaodian：y=" << y_3 << endl;
-    // cout << "R-M：" << R_M << endl;
-    cout << "x_0:" << x_0 << endl;
-    cout << "y_0:" << y_0 << endl;
-    cout << "x_2:" << x_2 << endl;
-    cout << "y_2:" << y_2 << endl;
-    cout << "近切点：x_t_n="<< x_t_n << endl;
-    cout << "近切点：y_t_n=" << y_t_n << endl;
-    cout << "远切点：x_t_f=" << x_t_f << endl;
-    cout << "远切点：y_t_f=" << y_t_f << endl;
-    //cout << "航向角：" << steering_angle << endl;
-    //cout << "圆心1横坐标=" << x_o_1 << endl;
-    //cout << "圆心1纵坐标=" << y_o_1 << endl;
-    //cout << "圆心2横坐标=" << x_o_2 << endl;
-    //cout << "圆心2纵坐标=" << y_o_2 << endl;
-    //cout << "平分线弧度=" << angle_3 << endl;
-    //cout << " min_rad=" << min_rad << endl;
-    //cout << " real_rad=" << real_rad << endl;
-    //   system("PAUSE");
-
-
-
-    dTpoint0=Coordinate_UnTransfer(x_t_n, y_t_n, aimGps);
-    dTpoint1 = Coordinate_UnTransfer(x_t_f, y_t_f, aimGps);
-    dTpoint2 = Coordinate_UnTransfer(x_2, y_2, aimGps);
-    dTpoint3 = Coordinate_UnTransfer(x_0, y_0, aimGps);
-    dBocheAngle = steering_angle*180/PI;
-
-    double disA = GetDistance(aimGps,nowGps);
-
-    if(pt.y>y_t_n && x_t_f<x_2 && y_t_f>y_2&&disA<40){
-        return 1;
-    }
-    return 0;
-
-}
-
-
-double iv::decition::Compute00::min_rad_zhuanxiang(double *R_M, double *min_rad) {
-    double L_c = 4.749;//车长
-    double rad_1;
-    double rad_2;
-    double L_k = 1.931;//车宽
-    double L = 2.95;//轴距
-    double L_f =1.2 ;//前悬
-    double L_r =0.7 ;//后悬
-    double R_min =6.5 ;//最小转弯半径
-    *R_M = fabs(sqrt(R_min*R_min - (L + L_f)*(L + L_f))) - L_k*0.5;//double	R_M  ;//后轴中点的转弯半径
-    //rad_1 = atan2(sqrt(R_min*R_min - (R_M - L_k*0.5)*(R_M - L_k*0.5)), R_M - L_k*0.5);
-    //rad_2 = atan2(L + L_f, R_M + L_k*0.5);
-    *min_rad = 45 * PI / 180;// rad_1 - rad_2;
-    return 0;
-}
-
-
-double iv::decition::Compute00::qiedian_n(double x_1, double y_1, double R_M,double min_rad, double *x_2, double *y_2, double *real_rad ) {
-
-    if (x_1 > 0 && y_1 > 0)
-    {
-        *real_rad = PI*0.5 - min_rad;
-        *x_2 = R_M - R_M*cos(min_rad);
-        *y_2 = R_M*sin(min_rad) + 0.5;
-    }
-    else
-    {
-        *real_rad = PI*0.5 + min_rad;
-        *x_2 = R_M*cos(min_rad) - R_M;
-        *y_2 = R_M*sin(min_rad) + 0.5;
-    }
-    return 0;
-
-}
-
-
-double iv::decition::Compute00::liangzhixian_jiaodian(double x_1,double y_1,double x_2,double y_2,double ange1,double real_rad,double *x_3,double *y_3) {
-    double b1, b2;
-    double k1, k2;
-    if (ange1!=(PI*0.5))
-    {
-        k1 = tan(ange1);
-        b1 = y_1 - k1*x_1;
-        k2 = tan(real_rad);
-        b2 = y_2 - k2*x_2;
-        *x_3 = (b2 - b1) / (k1 - k2);
-        *y_3 = k2*(*x_3) + b2;
-    }
-    else
-    {
-        k2 = tan(real_rad);
-        b2 = y_2 - k2*x_2;
-        *x_3 = x_1;
-        *y_3 = k2*(*x_3) + b2;
-    }
-    return 0;
-}
-
-
-double iv::decition::Compute00::chuizhipingfenxian_xielv(double x_1,double y_1,double ange1,double real_rad,double min_rad,double *angle_3) {
-    double k1, k2;
-    double  angle_j;
-    k2 = tan(real_rad);
-    if (ange1 != (PI*0.5))
-    {
-        k1 = tan(ange1);
-        angle_j = atan(fabs((k2 - k1) / (1 + k2*k1)));//两直线夹角
-
-        if (x_1 > 0 && y_1 > 0)
-        {
-            *angle_3 = angle_j*0.5 - min_rad + PI;
-        }
-        else
-        {
-            *angle_3 = min_rad - angle_j*0.5;
-        }
-    }
-    else
-    {
-        angle_j = min_rad;//两直线夹角
-        if (x_1 > 0 && y_1 > 0)
-        {
-            *angle_3 = angle_j*0.5 - min_rad + PI;
-        }
-        else
-        {
-            *angle_3 = min_rad - angle_j*0.5;
-        }
-    }
-    return 0;
-}
-
-
-double iv::decition::Compute00::yuanxin(double x_2,double y_2,double x_3,double y_3,double real_rad,double angle_3,double R_M,
-                                        double *x_o_1,double *y_o_1,double *x_o_2,double *y_o_2) {
-    double b2, b3, k2, k3;
-    b2 = y_2 - tan(real_rad)*x_2;
-    b3 = y_3 - tan(angle_3)*x_3;
-    k2 = tan(real_rad);
-    k3 = tan(angle_3);
-    *x_o_1 = (sqrt(k2*k2 + 1)*R_M + b3 - b2) / (k2 - k3);
-    *y_o_1 = k3*(*x_o_1) + b3;
-
-    *x_o_2 = (b3 - b2 - (sqrt(k2*k2 + 1)*R_M)) / (k2 - k3);
-    *y_o_2 = k3*(*x_o_2) + b3;
-    return 0;
-}
-
-
-double iv::decition::Compute00::yuanxin_qiedian(double ange1,double x_o_1,double y_o_1, double x_o_2,double y_o_2,
-                                                double x_1,double y_1,double x_2,double y_2,double x_3,double y_3,double real_rad,double angle_3,double R_M,
-                                                double *x_t_n, double *y_t_n, double *x_t_f, double *y_t_f)
-{
-    double x_o, y_o;
-    double b2, b3, k1, k2, k3;
-
-    //double car_pos[3] = { x_1,y_1,k1 };
-    double parking_pos[2] = { x_2,y_2 };
-    //double t[4][2];
-    double p[4];
-    double  s1, s2; //切点与车起始位置的距离
-    double  min;
-    int  min_i;
-    double x_t_1, y_t_1, x_t_2, y_t_2;//圆形1的切点
-    double x_t_3, y_t_3, x_t_4, y_t_4;//圆形2的切点
-    double t[4][2];
-    k1 = tan(ange1);
-    b2 = y_2 - tan(real_rad)*x_2;
-    b3 = y_3 - tan(real_rad)*x_3;
-    k2 = tan(real_rad);//另一条直线斜率
-    k3 = tan(angle_3);//垂直平分线斜率
-    //圆心1和2切点*********************************************
-    if (x_1 > 0 && y_1 > 0)//第一象限
-    {
-        if (ange1 == (PI*0.5))
-        {
-            x_t_1 = x_1;
-            y_t_1 = y_o_1;
-            y_t_2 = (y_2 + k2 *x_o_1 + y_o_1*k2*k2 - k2*x_2) / (1 + k2*k2);
-            x_t_2 = (y_t_2 + k2*x_2 - y_2) / k2;
-
-            x_t_3 = x_1;
-            y_t_3 = y_o_2;
-            y_t_4 = (y_2 + k2 *x_o_2 + y_o_2*k2*k2 - k2*x_2) / (1 + k2*k2);
-            x_t_4 = (y_t_4 + k2*x_2 - y_2) / k2;
-
-
-        }
-        else
-        {
-            y_t_1 = (y_1 + k1 *x_o_1 + y_o_1*k1*k1 - k1*x_1) / (1 + k1*k1);
-            x_t_1 = (y_t_1 + k1*x_1 - y_1) / k1;
-            y_t_2 = (y_2 + k2 *x_o_1 + y_o_1*k2*k2 - k2*x_2) / (1 + k2*k2);
-            x_t_2 = (y_t_2 + k2*x_2 - y_2) / k2;
-
-            y_t_3 = (y_1 + k1 *x_o_2 + y_o_2*k1*k1 - k1*x_1) / (1 + k1*k1);
-            x_t_3 = (y_t_3 + k1*x_1 - y_1) / k1;
-            y_t_4 = (y_2 + k2 *x_o_2 + y_o_2*k2*k2 - k2*x_2) / (1 + k2*k2);
-            x_t_4 = (y_t_4 + k2*x_2 - y_2) / k2;
-
-        }
-    }
-    else
-    {
-        if (ange1 == 0)
-        {
-            x_t_1 = 0 - x_1;
-            y_t_1 = y_o_1;
-            y_t_2 = (y_2 + k2 *x_o_1 + y_o_1*k2*k2 - k2*x_2) / (1 + k2*k2);
-            x_t_2 = (y_t_2 + k2*x_2 - y_2) / k2;
-
-            x_t_3 = 0 - x_1;
-            y_t_3 = y_o_2;
-            y_t_4 = (y_2 + k2 *x_o_2 + y_o_2*k2*k2 - k2*x_2) / (1 + k2*k2);
-            x_t_4 = (y_t_4 + k2*x_2 - y_2) / k2;
-        }
-        else
-        {
-            y_t_1 = (y_1 + k1 *x_o_1 + y_o_1*k1*k1 - k1*x_1) / (1 + k1*k1);
-            x_t_1 = (y_t_1 + k1*x_1 - y_1) / k1;
-            y_t_2 = (y_2 + k2 *x_o_1 + y_o_1*k2*k2 - k2*x_2) / (1 + k2*k2);
-            x_t_2 = (y_t_2 + k2*x_2 - y_2) / k2;
-
-            y_t_3 = (y_1 + k1 *x_o_2 + y_o_2*k1*k1 - k1*x_1) / (1 + k1*k1);
-            x_t_3 = (y_t_3 + k1*x_1 - y_1) / k1;
-            y_t_4 = (y_2 + k2 *x_o_2 + y_o_2*k2*k2 - k2*x_2) / (1 + k2*k2);
-            x_t_4 = (y_t_4 + k2*x_2 - y_2) / k2;
-
-        }
-
-    }
-
-    //圆心1和2切点*********************************************
-
-    t[0][0] = x_t_1;
-    t[0][1] = y_t_1;
-    t[1][0] = x_t_2;
-    t[1][1] = y_t_2;
-    t[2][0] = x_t_3;
-    t[2][1] = y_t_3;
-    t[3][0] = x_t_4;
-    t[3][1] = y_t_4;
-    for (int i = 0; i < 4; i++)
-    {
-
-        p[i] = (t[i][0] - parking_pos[0])*(t[i][0] - parking_pos[0]) + (t[i][1] - parking_pos[1])*(t[i][1] - parking_pos[1]);
-
-    }
-    min = p[0];
-    min_i = 0;
-    for (int i = 1; i < 4; i++)
-    {
-
-        if (p[i] < min)
-        {
-            min = p[i]; min_i = i;
-        }
-    }
-    if (min_i < 2)
-    {
-        x_o = x_o_1;
-        y_o = y_o_1;
-        s1 = (x_t_1 - x_1)*(x_t_1 - x_1) + (y_t_1 - y_1)*(y_t_1 - y_1);
-        s2 = (x_t_2 - x_1)*(x_t_2 - x_1) + (y_t_2 - y_1)*(y_t_2 - y_1);
-        if (s1 < s2)
-        {
-            *x_t_n = x_t_1;
-            *y_t_n = y_t_1;
-            *x_t_f = x_t_2;
-            *y_t_f = y_t_2;
-        }
-        else
-        {
-            *x_t_n = x_t_2;
-            *y_t_n = y_t_2;
-            *x_t_f = x_t_1;
-            *y_t_f = y_t_1;
-
-        }
-    }
-    else
-    {
-        x_o = x_o_2;
-        y_o = y_o_2;
-        s1 = (x_t_3 - x_1)*(x_t_3 - x_1) + (y_t_3 - y_1)*(y_t_3 - y_1);
-        s2 = (x_t_4 - x_1)*(x_t_4 - x_1) + (y_t_4 - y_1)*(y_t_4 - y_1);
-
-        if (s1 < s2)
-        {
-
-            *x_t_n = x_t_3;
-            *y_t_n = y_t_3;
-            *x_t_f = x_t_4;
-            *y_t_f = y_t_4;
-        }
-        else
-        {
-            *x_t_n = x_t_4;
-            *y_t_n = y_t_4;
-            *x_t_f = x_t_3;
-            *y_t_f = y_t_3;
-        }
-
-
-
-    }
-
-    return 0;
-
-}
-
-
-int iv::decition::Compute00::getNoAngleNearestPointIndex(GPS_INS rp, const std::vector<GPSData> gpsMap)
-{
-    int index = -1;
-    int startIndex = 0;     // startIndex = 0 则每一次都是遍历整条地图路线
-    int endIndex = gpsMap.size() - 1;
-    float minDis=20;
-
-    for (int j = startIndex; j < endIndex; j++)
-    {
-        int i = (j + gpsMap.size()) % gpsMap.size();
-        double tmpdis = GetDistance(rp, (*gpsMap[i]));
-
-        if (tmpdis < minDis)
-        {
-            index = i;
-            minDis=tmpdis;
-        }
-    }
-    return index;
-}
-
-
-double iv::decition::Compute00::getObsSpeedByFrenet(Point2D obsPoint, double realSecSpeed,const vector<Point2D> gpsTrace, std::vector<iv::GPSData> gpsMap,int pathpoint,GPS_INS nowGps) {
-    double obsSpeed = 0 - realSecSpeed;
-    double minDis = iv::MaxValue;
-    FrenetPoint esr_obs_F_point;
-    for (int i = 0; i < 64; i++)
-        if ((ServiceCarStatus.obs_radar[i].nomal_y) != 0 && ServiceCarStatus.obs_radar[i].valid)
-        {
-            double xxx = ServiceCarStatus.obs_radar[i].nomal_x + Esr_Offset;
-            double yyy = ServiceCarStatus.obs_radar[i].nomal_y + Esr_Y_Offset;
-
-            if (abs(xxx - obsPoint.x) < 4 && abs(yyy - obsPoint.y) < 2)
-            {
-                double tmpDis = sqrt((xxx - obsPoint.x) * (xxx - obsPoint.x) + (yyy - obsPoint.y) * (yyy - obsPoint.y));
-                if (tmpDis < minDis)
-                {
-                    minDis = tmpDis;
-//                    esr_obs_F_point = iv::decition::FrenetPlanner::XY2Frenet(xxx, yyy, gpsTrace);
-                    esr_obs_F_point = iv::decition::FrenetPlanner::getFrenetfromXY(xxx, yyy, gpsTrace,gpsMap,pathpoint,nowGps);
-//                    obsSpeed = ServiceCarStatus.obs_radar[i].speed_y;
-                    double speedx=ServiceCarStatus.obs_radar[i].speed_x;  //障碍物相对于车辆x轴的速度
-                    double speedy=ServiceCarStatus.obs_radar[i].speed_y;  //障碍物相对于车辆y轴的速度
-                    double speed_combine = sqrt(speedx*speedx+speedy*speedy);    //将x、y轴两个方向的速度求矢量和
-                    //障碍物的速度方向与道路方向的夹角。用于将速度分解到s方向和d方向。
-                    //所谓道路方向是指，道路上离障碍物的最近的点沿道路弧线的切线方向。
-                    double Etheta = esr_obs_F_point.tangent_Ang - atan2(speedy,speedx);
-
-                    obsSpeed = speed_combine*cos(Etheta);  //由speed_combine分解的s轴方向上的速度
-                }
-            }
-        }
-
-    return obsSpeed;
-}
-
-
-int iv::decition::Compute00::getEsrIndexByFrenet(std::vector<Point2D> gpsTrace, FrenetPoint &esrObsPoint, std::vector<iv::GPSData> gpsMap,int pathpoint,GPS_INS nowGps
-                                                 ,const double xiuzhengCs){
-    double minDistance = numeric_limits<double>::max();
-    int minDis_index=-1;
-
-    for(int i=0; i<64; ++i){
-        if ((ServiceCarStatus.obs_radar[i].nomal_y) != 0 && (ServiceCarStatus.obs_radar[i].valid)){
-            //毫米波在车头，故要加上毫米波与惯导的相对距离。(xxx,yyy)才是障碍物在 车辆坐标系下的坐标。
-            double xxx = ServiceCarStatus.obs_radar[i].nomal_x + Esr_Offset;
-            double yyy = ServiceCarStatus.obs_radar[i].nomal_y+ Esr_Y_Offset;
-
-            //将毫米波障碍物位置转换到frenet坐标系下
-//            esrObsPoint = iv::decition::FrenetPlanner::XY2Frenet(xxx,yyy,gpsTrace);
-            esrObsPoint = iv::decition::FrenetPlanner::getFrenetfromXY(xxx,yyy,gpsTrace,gpsMap,pathpoint,nowGps);
-
-            //如果障碍物与道路的横向距离d<=3.0*ServiceCarStatus.msysparam.vehWidth / 4.0，则认为道路上有障碍物。
-            //s则可理解为障碍物距离。为障碍物与车辆沿着道路方向的距离，而不是空间上的x或y坐标或者直线距离。
-            //minDistance、minDis_index用来统计最近的障碍物信息。
- //           if(abs(esrObsPoint.d)<=(3.0*ServiceCarStatus.msysparam.vehWidth / 4.0+DecideGps00().xiuzhengCs)){
-            if(abs(esrObsPoint.d)<=(3.0*ServiceCarStatus.msysparam.vehWidth / 4.0+xiuzhengCs)){
-                if(esrObsPoint.s<minDistance){
-                    minDistance = esrObsPoint.s;
-                    minDis_index = i;
-                }
-            }
-        }
-    }
-    return minDis_index;
-}
-
-
-
-
-std::vector<std::vector<iv::GPSData>> gmapsL;
-std::vector<std::vector<iv::GPSData>> gmapsR;

src1/decision/common/adc_tools/compute_00.h

@@ -1,105 +0,0 @@
-#pragma once
-#ifndef _IV_DECITION_COMPUTE_00_
-#define _IV_DECITION_COMPUTE_00_
-
-#include <gps_type.h>
-#include <common/obstacle_type.h>
-#include <decition_type.h>
-#include <vector>
-//#include <decision/decide_gps_00.h>
-
-#include "adc_planner/frenet_planner.h"
-
-namespace iv {
-    namespace decition {
-    //根据传感器传输来的信息作出决策
-        class Compute00 {
-        public:
-            Compute00();
-            ~Compute00();
-
-            static   double maxAngle;
-            static	 double angleLimit;  //角度限制
-            static	 double lastEA;      //上一次角度误差
-            static	 double lastEP;      //上一次位置误差
-            static	 double decision_Angle;  //决策角度
-            static	 double lastAng;         //上次角度
-            static   int lastEsrID;          //上次毫米波障碍物ID
-            static   int  lastEsrCount;      //毫米波障碍物累计次数
-
-            static   int lastEsrIDAvoid;          //上次毫米波障碍物ID Avoid
-            static   int  lastEsrCountAvoid;      //毫米波障碍物累计次数 Avoid
-
-            static iv::GPS_INS nearTpoint, farTpoint,dTpoint0,dTpoint1,dTpoint2,dTpoint3;
-            static double bocheAngle,dBocheAngle;
-
-            static	int getNearestPointIndex(GPS_INS rp, const std::vector<GPSData> gpsMap, int lastIndex, double mindis, double maxAngle);
-            static	int getFirstNearestPointIndex(GPS_INS rp, std::vector<GPSData> gpsMap, int lastIndex, double mindis, double maxAngle);
-
-            static int getDesiredSpeed(std::vector<GPSData> gpsMap);
-
-            static double getDecideAngle(std::vector<Point2D>  gpsTrace, double realSpeed);
-            static double getAveDef(std::vector<Point2D> farTrace);
-            static int getSpeedPointIndex(std::vector<Point2D> gpsTrace, double realSpeed);
-            static Point2D getLidarObsPoint(std::vector<Point2D> gpsTrace, iv::LidarGridPtr lidarGridPtr);
-            static Point2D getLidarRearObsPoint(std::vector<Point2D> gpsTrace, iv::LidarGridPtr lidarGridPtr);
-
-
-            static Point2D getLidarObsPointAvoid(std::vector<Point2D> gpsTrace, iv::LidarGridPtr lidarGridPtr,double & lidarDistanceAvoid);
-            static int getEsrIndex(std::vector<Point2D> gpsTrace , int roadNum, int *esrPathpoint,const double xiuzhengCs);
-            static int getRearEsrIndex(std::vector<Point2D> gpsTrace , int roadNum,const double xiuzhengCs);
-            static int getEsrIndexAvoid(std::vector<Point2D> gpsTrace);
-
-            static double getObsSpeed(Point2D obsPoint, double realSecSpeed);
-
-            static double getAvoidAveDef(std::vector<Point2D> farTrace, double avoidX);
-            static double getDecideAvoidAngle(std::vector<Point2D>  gpsTrace, double realSpeed, float avoidX
-                                              ,const bool readyParkMode,const int gpsLineParkIndex);
-
-            static std::vector<GPSData>  getBesideGpsMapLine(iv::GPS_INS now_gps_ins, std::vector<iv::GPSData>gpsMapLine, float avoidX);
-
-            static double getDecideAngleByLane(double realSpeed);
-
-            static double getDecideAngleByLanePID(double realSpeed);
-
-            static double bocheCompute(GPS_INS nowGps, GPS_INS aimGps);
-
-            static	double pingfenxian_xielv(double x_1, double y_1, double x_2, double y_2, double angle_1);
-
-            static double getQieXianAngle(GPS_INS nowGps, GPS_INS aimGps);
-
-            static int bocheDirectCompute(GPS_INS nowGps, GPS_INS aimGps);
-
-            static double min_rad_zhuanxiang(double *R_M, double *min_rad);
-            static double qiedian_n(double x_1,double y_1,double R_M,double min_rad,
-                                    double *x_2, double *y_2, double *real_rad);
-            static double liangzhixian_jiaodian(double x_1,double y_1,double x_2,double y_2,
-                                                double ange1,double real_rad,double *x_3, double *y_3);
-            static double chuizhipingfenxian_xielv(double x_1,double y_1,double ange1,double real_rad,
-                                                   double min_rad,double *angle_3);
-            static double yuanxin(double x_2,double y_2,double x_3,double y_3,double real_rad,
-                                  double angle_3,double R_M,double *x_o_1, double *y_o_1, double *x_o_2, double *y_0_2);
-            static double yuanxin_qiedian(double angel,double x_o_1,double y_o_1, double x_o_2,double y_o_2,
-                                          double x_1,double y_1,double x_2,double y_2,double x_3,double y_3,
-                                          double real_rad,double angle_3,double R_M,
-                                          double *x_t_n, double *y_t_n, double *x_t_f, double *y_t_f);
-
-            static int getNoAngleNearestPointIndex(GPS_INS rp, const std::vector<GPSData> gpsMap);
-
-            static int getEsrIndexByFrenet(std::vector<Point2D> gpsTrace,FrenetPoint &esrObsPoint, std::vector<iv::GPSData> gpsMap,int pathpoint,GPS_INS nowGps
-                                           ,const double xiuzhengCs);
-            double getObsSpeedByFrenet(Point2D obsPoint, double realSecSpeed,const std::vector<Point2D> gpsTrace, std::vector<iv::GPSData> gpsMap,int pathpoint,GPS_INS nowGps);
-
-        private:
-
-        };
-    }
-}
-extern std::vector<std::vector<iv::GPSData>> gmapsL;
-extern std::vector<std::vector<iv::GPSData>> gmapsR;
-
-extern std::vector<int> lastEsrIdVector;
-extern std::vector<int> lastEsrCountVector;
-
-#endif // !_IV_DECITION_COMPUTE_00_
-

src1/decision/common/adc_tools/dubins.cpp

@@ -1,439 +0,0 @@
-/*
- * Copyright (c) 2008-2018, Andrew Walker
- *
- * Permission is hereby granted, free of charge, to any person obtaining a copy
- * of this software and associated documentation files (the "Software"), to deal
- * in the Software without restriction, including without limitation the rights
- * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
- * copies of the Software, and to permit persons to whom the Software is
- * furnished to do so, subject to the following conditions:
- *
- * The above copyright notice and this permission notice shall be included in
- * all copies or substantial portions of the Software.
- *
- * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
- * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
- * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
- * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
- * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
- * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
- * THE SOFTWARE.
- */
-#ifdef WIN32
-#define _USE_MATH_DEFINES
-#endif
-#include <math.h>
-#include "dubins.h"
-
-#define EPSILON (10e-10)
-
-typedef enum
-{
-    L_SEG = 0,
-    S_SEG = 1,
-    R_SEG = 2
-} SegmentType;
-
-/* The segment types for each of the Path types */
-const SegmentType DIRDATA[][3] = {
-    { L_SEG, S_SEG, L_SEG },
-    { L_SEG, S_SEG, R_SEG },
-    { R_SEG, S_SEG, L_SEG },
-    { R_SEG, S_SEG, R_SEG },
-    { R_SEG, L_SEG, R_SEG },
-    { L_SEG, R_SEG, L_SEG }
-};
-
-typedef struct
-{
-    double alpha;
-    double beta;
-    double d;
-    double sa;
-    double sb;
-    double ca;
-    double cb;
-    double c_ab;
-    double d_sq;
-} DubinsIntermediateResults;
-
-
-int dubins_word(DubinsIntermediateResults* in, DubinsPathType pathType, double out[3]);
-int dubins_intermediate_results(DubinsIntermediateResults* in, double q0[3], double q1[3], double rho);
-
-/**
- * Floating point modulus suitable for rings
- *
- * fmod doesn't behave correctly for angular quantities, this function does
- */
-double fmodr( double x, double y)
-{
-    return x - y*floor(x/y);
-}
-
-double mod2pi( double theta )
-{
-    return fmodr( theta, 2 * M_PI );
-}
-
-int dubins_shortest_path(DubinsPath* path, double q0[3], double q1[3], double rho)
-{
-    int i, errcode;
-    DubinsIntermediateResults in;
-    double params[3];
-    double cost;
-    double best_cost = INFINITY;
-    int best_word = -1;
-    errcode = dubins_intermediate_results(&in, q0, q1, rho);
-    if(errcode != EDUBOK) {
-        return errcode;
-    }
-
-
-    path->qi[0] = q0[0];
-    path->qi[1] = q0[1];
-    path->qi[2] = q0[2];
-    path->rho = rho;
-
-    for( i = 0; i < 6; i++ ) {
-        DubinsPathType pathType = (DubinsPathType)i;
-        errcode = dubins_word(&in, pathType, params);
-        if(errcode == EDUBOK) {
-            cost = params[0] + params[1] + params[2];
-            if(cost < best_cost) {
-                best_word = i;
-                best_cost = cost;
-                path->param[0] = params[0];
-                path->param[1] = params[1];
-                path->param[2] = params[2];
-                path->type = pathType;
-            }
-        }
-    }
-    if(best_word == -1) {
-        return EDUBNOPATH;
-    }
-    return EDUBOK;
-}
-
-int dubins_path(DubinsPath* path, double q0[3], double q1[3], double rho, DubinsPathType pathType)
-{
-    int errcode;
-    DubinsIntermediateResults in;
-    errcode = dubins_intermediate_results(&in, q0, q1, rho);
-    if(errcode == EDUBOK) {
-        double params[3];
-        errcode = dubins_word(&in, pathType, params);
-        if(errcode == EDUBOK) {
-            path->param[0] = params[0];
-            path->param[1] = params[1];
-            path->param[2] = params[2];
-            path->qi[0] = q0[0];
-            path->qi[1] = q0[1];
-            path->qi[2] = q0[2];
-            path->rho = rho;
-            path->type = pathType;
-        }
-    }
-    return errcode;
-}
-
-double dubins_path_length( DubinsPath* path )
-{
-    double length = 0.;
-    length += path->param[0];
-    length += path->param[1];
-    length += path->param[2];
-    length = length * path->rho;
-    return length;
-}
-
-double dubins_segment_length( DubinsPath* path, int i )
-{
-    if( (i < 0) || (i > 2) )
-    {
-        return INFINITY;
-    }
-    return path->param[i] * path->rho;
-}
-
-double dubins_segment_length_normalized( DubinsPath* path, int i )
-{
-    if( (i < 0) || (i > 2) )
-    {
-        return INFINITY;
-    }
-    return path->param[i];
-}
-
-DubinsPathType dubins_path_type( DubinsPath* path )
-{
-    return path->type;
-}
-
-void dubins_segment( double t, double qi[3], double qt[3], SegmentType type)
-{
-    double st = sin(qi[2]);
-    double ct = cos(qi[2]);
-    if( type == L_SEG ) {
-        qt[0] = +sin(qi[2]+t) - st;
-        qt[1] = -cos(qi[2]+t) + ct;
-        qt[2] = t;
-    }
-    else if( type == R_SEG ) {
-        qt[0] = -sin(qi[2]-t) + st;
-        qt[1] = +cos(qi[2]-t) - ct;
-        qt[2] = -t;
-    }
-    else if( type == S_SEG ) {
-        qt[0] = ct * t;
-        qt[1] = st * t;
-        qt[2] = 0.0;
-    }
-    qt[0] += qi[0];
-    qt[1] += qi[1];
-    qt[2] += qi[2];
-}
-
-int dubins_path_sample( DubinsPath* path, double t, double q[3] )
-{
-    /* tprime is the normalised variant of the parameter t */
-    double tprime = t / path->rho;
-    double qi[3]; /* The translated initial configuration */
-    double q1[3]; /* end-of segment 1 */
-    double q2[3]; /* end-of segment 2 */
-    const SegmentType* types = DIRDATA[path->type];
-    double p1, p2;
-
-    if( t < 0 || t > dubins_path_length(path) ) {
-        return EDUBPARAM;
-    }
-
-    /* initial configuration */
-    qi[0] = 0.0;
-    qi[1] = 0.0;
-    qi[2] = path->qi[2];
-
-    /* generate the target configuration */
-    p1 = path->param[0];
-    p2 = path->param[1];
-    dubins_segment( p1,      qi,    q1, types[0] );
-    dubins_segment( p2,      q1,    q2, types[1] );
-    if( tprime < p1 ) {
-        dubins_segment( tprime, qi, q, types[0] );
-    }
-    else if( tprime < (p1+p2) ) {
-        dubins_segment( tprime-p1, q1, q,  types[1] );
-    }
-    else {
-        dubins_segment( tprime-p1-p2, q2, q,  types[2] );
-    }
-
-    /* scale the target configuration, translate back to the original starting point */
-    q[0] = q[0] * path->rho + path->qi[0];
-    q[1] = q[1] * path->rho + path->qi[1];
-    q[2] = mod2pi(q[2]);
-
-    return EDUBOK;
-}
-
-int dubins_path_sample_many(DubinsPath* path, double stepSize,
-                            DubinsPathSamplingCallback cb, void* user_data)
-{
-    int retcode;
-    double q[3];
-    double x = 0.0;
-    double length = dubins_path_length(path);
-    while( x <  length ) {
-        dubins_path_sample( path, x, q );
-        retcode = cb(q, x, user_data);
-        if( retcode != 0 ) {
-            return retcode;
-        }
-        x += stepSize;
-    }
-    return 0;
-}
-
-int dubins_path_endpoint( DubinsPath* path, double q[3] )
-{
-    return dubins_path_sample( path, dubins_path_length(path) - EPSILON, q );
-}
-
-int dubins_extract_subpath( DubinsPath* path, double t, DubinsPath* newpath )
-{
-    /* calculate the true parameter */
-    double tprime = t / path->rho;
-
-    if((t < 0) || (t > dubins_path_length(path)))
-    {
-        return EDUBPARAM;
-    }
-
-    /* copy most of the data */
-    newpath->qi[0] = path->qi[0];
-    newpath->qi[1] = path->qi[1];
-    newpath->qi[2] = path->qi[2];
-    newpath->rho   = path->rho;
-    newpath->type  = path->type;
-
-    /* fix the parameters */
-    newpath->param[0] = fmin( path->param[0], tprime );
-    newpath->param[1] = fmin( path->param[1], tprime - newpath->param[0]);
-    newpath->param[2] = fmin( path->param[2], tprime - newpath->param[0] - newpath->param[1]);
-    return 0;
-}
-
-int dubins_intermediate_results(DubinsIntermediateResults* in, double q0[3], double q1[3], double rho)
-{
-    double dx, dy, D, d, theta, alpha, beta;
-    if( rho <= 0.0 ) {
-        return EDUBBADRHO;
-    }
-
-    dx = q1[0] - q0[0];
-    dy = q1[1] - q0[1];
-    D = sqrt( dx * dx + dy * dy );
-    d = D / rho;
-    theta = 0;
-
-    /* test required to prevent domain errors if dx=0 and dy=0 */
-    if(d > 0) {
-        theta = mod2pi(atan2( dy, dx ));
-    }
-    alpha = mod2pi(q0[2] - theta);
-    beta  = mod2pi(q1[2] - theta);
-
-    in->alpha = alpha;
-    in->beta  = beta;
-    in->d     = d;
-    in->sa    = sin(alpha);
-    in->sb    = sin(beta);
-    in->ca    = cos(alpha);
-    in->cb    = cos(beta);
-    in->c_ab  = cos(alpha - beta);
-    in->d_sq  = d * d;
-
-    return EDUBOK;
-}
-
-int dubins_LSL(DubinsIntermediateResults* in, double out[3])
-{
-    double tmp0, tmp1, p_sq;
-
-    tmp0 = in->d + in->sa - in->sb;
-    p_sq = 2 + in->d_sq - (2*in->c_ab) + (2 * in->d * (in->sa - in->sb));
-
-    if(p_sq >= 0) {
-        tmp1 = atan2( (in->cb - in->ca), tmp0 );
-        out[0] = mod2pi(tmp1 - in->alpha);
-        out[1] = sqrt(p_sq);
-        out[2] = mod2pi(in->beta - tmp1);
-        return EDUBOK;
-    }
-    return EDUBNOPATH;
-}
-
-
-int dubins_RSR(DubinsIntermediateResults* in, double out[3])
-{
-    double tmp0 = in->d - in->sa + in->sb;
-    double p_sq = 2 + in->d_sq - (2 * in->c_ab) + (2 * in->d * (in->sb - in->sa));
-    if( p_sq >= 0 ) {
-        double tmp1 = atan2( (in->ca - in->cb), tmp0 );
-        out[0] = mod2pi(in->alpha - tmp1);
-        out[1] = sqrt(p_sq);
-        out[2] = mod2pi(tmp1 -in->beta);
-        return EDUBOK;
-    }
-    return EDUBNOPATH;
-}
-
-int dubins_LSR(DubinsIntermediateResults* in, double out[3])
-{
-    double p_sq = -2 + (in->d_sq) + (2 * in->c_ab) + (2 * in->d * (in->sa + in->sb));
-    if( p_sq >= 0 ) {
-        double p    = sqrt(p_sq);
-        double tmp0 = atan2( (-in->ca - in->cb), (in->d + in->sa + in->sb) ) - atan2(-2.0, p);
-        out[0] = mod2pi(tmp0 - in->alpha);
-        out[1] = p;
-        out[2] = mod2pi(tmp0 - mod2pi(in->beta));
-        return EDUBOK;
-    }
-    return EDUBNOPATH;
-}
-
-int dubins_RSL(DubinsIntermediateResults* in, double out[3])
-{
-    double p_sq = -2 + in->d_sq + (2 * in->c_ab) - (2 * in->d * (in->sa + in->sb));
-    if( p_sq >= 0 ) {
-        double p    = sqrt(p_sq);
-        double tmp0 = atan2( (in->ca + in->cb), (in->d - in->sa - in->sb) ) - atan2(2.0, p);
-        out[0] = mod2pi(in->alpha - tmp0);
-        out[1] = p;
-        out[2] = mod2pi(in->beta - tmp0);
-        return EDUBOK;
-    }
-    return EDUBNOPATH;
-}
-
-int dubins_RLR(DubinsIntermediateResults* in, double out[3])
-{
-    double tmp0 = (6. - in->d_sq + 2*in->c_ab + 2*in->d*(in->sa - in->sb)) / 8.;
-    double phi  = atan2( in->ca - in->cb, in->d - in->sa + in->sb );
-    if( fabs(tmp0) <= 1) {
-        double p = mod2pi((2*M_PI) - acos(tmp0) );
-        double t = mod2pi(in->alpha - phi + mod2pi(p/2.));
-        out[0] = t;
-        out[1] = p;
-        out[2] = mod2pi(in->alpha - in->beta - t + mod2pi(p));
-        return EDUBOK;
-    }
-    return EDUBNOPATH;
-}
-
-int dubins_LRL(DubinsIntermediateResults* in, double out[3])
-{
-    double tmp0 = (6. - in->d_sq + 2*in->c_ab + 2*in->d*(in->sb - in->sa)) / 8.;
-    double phi = atan2( in->ca - in->cb, in->d + in->sa - in->sb );
-    if( fabs(tmp0) <= 1) {
-        double p = mod2pi( 2*M_PI - acos( tmp0) );
-        double t = mod2pi(-in->alpha - phi + p/2.);
-        out[0] = t;
-        out[1] = p;
-        out[2] = mod2pi(mod2pi(in->beta) - in->alpha -t + mod2pi(p));
-        return EDUBOK;
-    }
-    return EDUBNOPATH;
-}
-
-int dubins_word(DubinsIntermediateResults* in, DubinsPathType pathType, double out[3])
-{
-    int result;
-    switch(pathType)
-    {
-    case LSL:
-        result = dubins_LSL(in, out);
-        break;
-    case RSL:
-        result = dubins_RSL(in, out);
-        break;
-    case LSR:
-        result = dubins_LSR(in, out);
-        break;
-    case RSR:
-        result = dubins_RSR(in, out);
-        break;
-    case LRL:
-        result = dubins_LRL(in, out);
-        break;
-    case RLR:
-        result = dubins_RLR(in, out);
-        break;
-    default:
-        result = EDUBNOPATH;
-    }
-    return result;
-}
-
-

src1/decision/common/adc_tools/dubins.h

@@ -1,149 +0,0 @@
-#ifndef DUBINS_H
-#define DUBINS_H
-
-typedef enum
-{
-    LSL = 0,
-    LSR = 1,
-    RSL = 2,
-    RSR = 3,
-    RLR = 4,
-    LRL = 5
-} DubinsPathType;
-
-typedef struct
-{
-    /* the initial configuration */
-    double qi[3];
-    /* the lengths of the three segments */
-    double param[3];
-    /* model forward velocity / model angular velocity */
-    double rho;
-    /* the path type described */
-    DubinsPathType type;
-} DubinsPath;
-
-#define EDUBOK        (0)   /* No error */
-#define EDUBCOCONFIGS (1)   /* Colocated configurations */
-#define EDUBPARAM     (2)   /* Path parameterisitation error */
-#define EDUBBADRHO    (3)   /* the rho value is invalid */
-#define EDUBNOPATH    (4)   /* no connection between configurations with this word */
-
-/**
- * Callback function for path sampling
- *
- * @note the q parameter is a configuration
- * @note the t parameter is the distance along the path
- * @note the user_data parameter is forwarded from the caller
- * @note return non-zero to denote sampling should be stopped
- */
-typedef int (*DubinsPathSamplingCallback)(double q[3], double t, void* user_data);
-
-/**
- * Generate a path from an initial configuration to
- * a target configuration, with a specified maximum turning
- * radii
- *
- * A configuration is (x, y, theta), where theta is in radians, with zero
- * along the line x = 0, and counter-clockwise is positive
- *
- * @param path  - the resultant path
- * @param q0    - a configuration specified as an array of x, y, theta
- * @param q1    - a configuration specified as an array of x, y, theta
- * @param rho   - turning radius of the vehicle (forward velocity divided by maximum angular velocity)
- * @return      - non-zero on error
- */
-int dubins_shortest_path(DubinsPath* path, double q0[3], double q1[3], double rho);
-
-/**
- * Generate a path with a specified word from an initial configuration to
- * a target configuration, with a specified turning radius
- *
- * @param path     - the resultant path
- * @param q0       - a configuration specified as an array of x, y, theta
- * @param q1       - a configuration specified as an array of x, y, theta
- * @param rho      - turning radius of the vehicle (forward velocity divided by maximum angular velocity)
- * @param pathType - the specific path type to use
- * @return         - non-zero on error
- */
-int dubins_path(DubinsPath* path, double q0[3], double q1[3], double rho, DubinsPathType pathType);
-
-/**
- * Calculate the length of an initialised path
- *
- * @param path - the path to find the length of
- */
-double dubins_path_length(DubinsPath* path);
-
-/**
- * Return the length of a specific segment in an initialized path
- *
- * @param path - the path to find the length of
- * @param i    - the segment you to get the length of (0-2)
- */
-double dubins_segment_length(DubinsPath* path, int i);
-
-/**
- * Return the normalized length of a specific segment in an initialized path
- *
- * @param path - the path to find the length of
- * @param i    - the segment you to get the length of (0-2)
- */
-double dubins_segment_length_normalized( DubinsPath* path, int i );
-
-/**
- * Extract an integer that represents which path type was used
- *
- * @param path    - an initialised path
- * @return        - one of LSL, LSR, RSL, RSR, RLR or LRL
- */
-DubinsPathType dubins_path_type(DubinsPath* path);
-
-/**
- * Calculate the configuration along the path, using the parameter t
- *
- * @param path - an initialised path
- * @param t    - a length measure, where 0 <= t < dubins_path_length(path)
- * @param q    - the configuration result
- * @returns    - non-zero if 't' is not in the correct range
- */
-int dubins_path_sample(DubinsPath* path, double t, double q[3]);
-
-/**
- * Walk along the path at a fixed sampling interval, calling the
- * callback function at each interval
- *
- * The sampling process continues until the whole path is sampled, or the callback returns a non-zero value
- *
- * @param path      - the path to sample
- * @param stepSize  - the distance along the path for subsequent samples
- * @param cb        - the callback function to call for each sample
- * @param user_data - optional information to pass on to the callback
- *
- * @returns - zero on successful completion, or the result of the callback
- */
-int dubins_path_sample_many(DubinsPath* path,
-                            double stepSize,
-                            DubinsPathSamplingCallback cb,
-                            void* user_data);
-
-/**
- * Convenience function to identify the endpoint of a path
- *
- * @param path - an initialised path
- * @param q    - the configuration result
- */
-int dubins_path_endpoint(DubinsPath* path, double q[3]);
-
-/**
- * Convenience function to extract a subset of a path
- *
- * @param path    - an initialised path
- * @param t       - a length measure, where 0 < t < dubins_path_length(path)
- * @param newpath - the resultant path
- */
-int dubins_extract_subpath(DubinsPath* path, double t, DubinsPath* newpath);
-
-
-
-#endif // DUBINS_H

src1/decision/common/adc_tools/gps_distance.cpp

@@ -1,45 +0,0 @@
-#include <adc_tools/gps_distance.h>
-#include <math.h>
-#define M_PI (3.1415926535897932384626433832795)
-
-// 计算弧度
-double iv::decition::rad(double d)
-{
-    const double PI = 3.1415926535898;
-    return d * PI / 180.0;
-}
-
-// 从两个gps坐标点（经纬度）获得两点的直线距离，单位是米
-double iv::decition::CalcDistance(double fLati1, double fLong1, double fLati2, double fLong2)
-{
-    const float EARTH_RADIUS = 6378.137;
-    double radLat1 = rad(fLati1);
-    double radLat2 = rad(fLati2);
-    double a = radLat1 - radLat2;
-
-    double b = rad(fLong1) - rad(fLong2);
-    double s = 2 * asin(sqrt(pow(sin(a / 2), 2) + cos(radLat1)*cos(radLat2)*pow(sin(b / 2), 2)));
-    s = s * EARTH_RADIUS;
-    //	s = (int)(s * 10000000) / 10000;
-    return s;
-}
-
-
-// 从直角坐标两点的直线距离，单位是米
-double iv::decition::DirectDistance(double x1, double y1, double x2, double y2)
-{
-    double s = sqrt(pow((x1-x2), 2) +pow((y1-y2), 2));
-    return s;
-}
-
-
-// 从直角坐标计算两点的夹角
-double iv::decition::DirectAngle(double x1, double y1, double x2, double y2)
-{
-    double angle = atan((y2 - y1) / (x2 - x1)) * 180 / M_PI;
-    return angle;
-}
-
-
-
-

src1/decision/common/adc_tools/gps_distance.h

@@ -1,26 +0,0 @@
-#pragma once
-#ifndef _IV_DECITION_GPS_DISTANCE_
-#define _IV_DECITION_GPS_DISTANCE_
-
-namespace iv {
-	namespace decition {
-        // 计算弧度
-		double rad(double d);
-
-		// 从两个gps坐标点（经纬度）获得两点的直线距离，单位是米
-		double CalcDistance(double , double , double , double );
-
-		//计算直角坐标距离
-		double DirectDistance(double, double, double, double);
-
-		//计算直角坐标角度
-		double DirectAngle(double, double, double, double);
-
-
-	}
-}
-
-
-
-
-#endif // !_IV_DECITION_GPS_DISTANCE_

src1/decision/common/adc_tools/parameter_status.h

@@ -1,109 +0,0 @@
-#ifndef PARAMETER_STATUS_H
-#define PARAMETER_STATUS_H
-
-
-
-
-#include <boost.h>
-#include <cstdint>
-#include <boost/serialization/singleton.hpp>
-#include <common/vv7.h>
-
-namespace iv {
-    namespace decition {
-        class ParameterStatus : public boost::noncopyable
-        {
-        public:
-            /*****************
-             * **** speed control *****
-             * ***************/
-
-
-
-
-                    float speed_kp=0.5;
-                    float speed_kd=0.3;
-                    float speed_ki=0;
-
-                    float speed_kp_t=10;
-                    float speed_kd_t=0;
-                    float speed_ki_t=0;
-
-                    float speed_increase_limite_switch=1;
-                    float speed_decline_limite_switch=1;
-                    float speed_max_increase=5;
-                    float speed_max_decline=10;
-
-
-
-
-            /*****************
-             * **** ttc control *****
-             * ***************/
-
-                float ttc_emc=0.8;
-                float ttc_urgent=1.6;
-                float ttc_early=10;
-                float brakeIntMax_emc=10;
-                float brakeIntMax_urgent=5;
-                float brakeIntMax_early=3;
-
-
-            /*****************
-             * **** wheel control *****
-             * ***************/
-
-                float wheel_p_eang=14;
-                float wheel_p_epos=10;
-                float wheel_d_eang=5;
-                float wheel_d_epos=5;
-                float wheel_i_eang=0;
-                float wheel_i_epos=0;
-
-                float  wheel_previewDistance_coefficient=0.6;
-                float  wheel_previewDistance_min=5;
-                float  wheel_change_limit_switch=0;
-                float  wheel_max_change=10;
-
-            /*****************
-             * **** path planning *****
-             * ***************/
-                float road_width = 3.5; //道路宽度
-                int now_road_num = 0;  //车辆当前所在道路编号
-
-
-            /*****************
-             * **** frenet path planning *****
-             * ***************/
-                double MAX_SPEED = 50.0 / 3.6;     // 最大速度 [m/s]
-                double MAX_ACCEL = 10;            // 最大加速度[m/ss]
-                double MAX_CURVATURE = 10;        // 最大曲率 [1/m]
-                double MIN_ROAD_OFFSET = -3.5;     // 最小道路偏移度 [m]。可以为负数。
-                double MAX_ROAD_WIDTH = 3.5;       // 最大道路宽度 [m]。过小可能不具有避障功能。
-                double D_ROAD_W = 3.5;             // 道路宽度采样间隔 [m]
-                double DT = 0.2;                   // Delta T [s]。总的预测时间的增量。
-                double MAXT = 4.0;                 // 最大预测时间 [s]
-                double MINT = 2.0;                 // 最小预测时间 [s]
-                double D_POINT_T = 0.04;           // 时间增量 [s]。用于控制每条轨迹生成轨迹点的密度。
-                double TARGET_SPEED = 30.0 / 3.6;  // 目标速度（即纵向的速度保持） [m/s]
-                double D_T_S = 5.0 / 3.6;          // 目标速度采样间隔 [m/s]
-                double N_S_SAMPLE = 1;             // sampling number of target speed
-                double ROBOT_RADIUS = 2.0;         // robot radius [m]
-
-                // 损失函数权重
-                double KJ = 0.1;
-                double KT = 0.1;
-                double KD = 1.0;
-                double KLAT = 1.0;
-                double KLON = 1.0;
-
-
-    };
-        typedef boost::serialization::singleton<iv::decition::ParameterStatus> ParameterStatusSingleton;
-}
-#define ServiceParameterStatus iv::decition::ParameterStatusSingleton::get_mutable_instance()
-
-}
-
-
-#endif // PARAMETER_STATUS_H

src1/decision/common/adc_tools/transfer.cpp

@@ -1,138 +0,0 @@
-#include <adc_tools/transfer.h>
-#include <decition_type.h>
-#include <math.h>
-#include <iostream>
-#include <fstream>
-using namespace std;
-#define PI (3.1415926535897932384626433832795)
-
-
-
-///大地转车体
-iv::Point2D iv::decition::Coordinate_Transfer(double x_path, double y_path, iv::GPS_INS pos)
-{
-    double x_vehicle, y_vehicle;
-    double theta = atan2(x_path - pos.gps_x, y_path - pos.gps_y);
-
-    double distance = sqrt((x_path - pos.gps_x) * (x_path - pos.gps_x) + (y_path - pos.gps_y) * (y_path - pos.gps_y));
-    double angle = theta - (-90 + pos.ins_heading_angle) / 180 * PI;
-
-    x_vehicle = -cos(angle) * distance;
-    y_vehicle = sin(angle) * distance;
-    return  iv::Point2D(x_vehicle, y_vehicle);
-}
-
-///车体转大地
-iv::GPS_INS iv::decition::Coordinate_UnTransfer(double x_path, double y_path, iv::GPS_INS pos)
-{
-    iv::GPS_INS gps_ins;
-    double x_vehicle, y_vehicle;
-    double theta = atan2(x_path, y_path);
-
-    double distance = sqrt(x_path * x_path + y_path * y_path);
-    double angle = (pos.ins_heading_angle / 180 * PI + theta);
-
-    x_vehicle = pos.gps_x + distance * sin(angle);
-    y_vehicle = pos.gps_y + distance * cos(angle);
-    gps_ins.gps_x = x_vehicle;
-    gps_ins.gps_y = y_vehicle;
-
-    return  gps_ins;
-}
-
-double iv::decition::GetDistance(iv::Point2D x1, iv::Point2D x2)
-{
-    return sqrt((x1.x - x2.x) * (x1.x - x2.x) + (x1.y - x2.y) * (x1.y - x2.y));
-}
-
-double iv::decition::GetDistance(iv::GPS_INS p1, iv::GPS_INS p2)
-{
-    return sqrt((p1.gps_x - p2.gps_x) * (p1.gps_x - p2.gps_x) + (p1.gps_y - p2.gps_y) * (p1.gps_y - p2.gps_y));
-}
-
-
-
-
-//GPS转大地坐标
-void iv::decition::BLH2XYZ(iv::GPS_INS gp)
-{
-    int nFlag = 2;
-
-    double B = gp.gps_lat;
-
-    double L = gp.gps_lng;
-
-
-
-    double N, E, h;
-    double L0 = GetL0InDegree(L);//根据经度求中央子午线经度
-
-    double a = 6378245.0;            //地球半径  北京6378245
-    double F = 298.257223563;        //地球扁率
-    double iPI = 0.0174532925199433; //2pi除以360，用于角度转换
-
-    double f = 1 / F;
-    double b = a * (1 - f);
-    double ee = (a * a - b * b) / (a * a);
-    double e2 = (a * a - b * b) / (b * b);
-    double n = (a - b) / (a + b), n2 = (n * n), n3 = (n2 * n), n4 = (n2 * n2), n5 = (n4 * n);
-    double al = (a + b) * (1 + n2 / 4 + n4 / 64) / 2;
-    double bt = -3 * n / 2 + 9 * n3 / 16 - 3 * n5 / 32;
-    double gm = 15 * n2 / 16 - 15 * n4 / 32;
-    double dt = -35 * n3 / 48 + 105 * n5 / 256;
-    double ep = 315 * n4 / 512;
-
-    B = B * iPI;
-    L = L * iPI;
-    L0 = L0 * iPI;
-
-    double l = L - L0, cl = (cos(B) * l), cl2 = (cl * cl), cl3 = (cl2 * cl), cl4 = (cl2 * cl2), cl5 = (cl4 * cl), cl6 = (cl5 * cl), cl7 = (cl6 * cl), cl8 = (cl4 * cl4);
-    double lB = al * (B + bt * sin(2 * B) + gm * sin(4 * B) + dt * sin(6 * B) + ep * sin(8 * B));
-    double t = tan(B), t2 = (t * t), t4 = (t2 * t2), t6 = (t4 * t2);
-    double Nn = a / sqrt(1 - ee * sin(B) * sin(B));
-    double yt = e2 * cos(B) * cos(B);
-    N = lB;
-    N += t * Nn * cl2 / 2;
-    N += t * Nn * cl4 * (5 - t2 + 9 * yt + 4 * yt * yt) / 24;
-    N += t * Nn * cl6 * (61 - 58 * t2 + t4 + 270 * yt - 330 * t2 * yt) / 720;
-    N += t * Nn * cl8 * (1385 - 3111 * t2 + 543 * t4 - t6) / 40320;
-
-    E = Nn * cl;
-    E += Nn * cl3 * (1 - t2 + yt) / 6;
-    E += Nn * cl5 * (5 - 18 * t2 + t4 + 14 * yt - 58 * t2 * yt) / 120;
-    E += Nn * cl7 * (61 - 479 * t2 + 179 * t4 - t6) / 5040;
-
-    E += 500000;
-    if (nFlag == 1)
-    {
-        //UTM投影
-        N = 0.9996 * N;
-        E = 0.9996 * (E - 500000.0) + 500000.0;//Get y
-    }
-    if (nFlag == 2)
-    {
-        //UTM投影
-        N = 0.9999 * N;
-        E = 0.9999 * (E - 500000.0) + 250000.0;//Get y
-    }
-
-    //原
-    //pt2d.x = N;
-    //pt2d.y = E;
-    //
-    gp.gps_x = E - 280000;
-    gp.gps_y = N - 4325000;
-
-}
-
-
-double iv::decition::GetL0InDegree(double dLIn)
-{
-    //3°带求带号及中央子午线经度(d的形式)
-    //具体公式请参阅《常用大地坐标系及其变换》朱华统，解放军出版社138页
-    double L = dLIn;//d.d
-    double L_ddd_Style = L;
-    double ZoneNumber = (int)((L_ddd_Style - 1.5) / 3.0) + 1;
-    double L0 = ZoneNumber * 3.0;//degree
-    return L0;
-}

src1/decision/common/adc_tools/transfer.h

@@ -1,27 +0,0 @@
-#pragma once
-#ifndef _IV_DECITION_TRANSFER_
-#define _IV_DECITION_TRANSFER_
-
-#include <gps_type.h>
-#include <decition_type.h>
-#include<vector> 
-
-namespace iv {
-    namespace decition {
-        //根据传感器传输来的信息作出决策
-
-        double GetL0InDegree(double dLIn);
-
-        void BLH2XYZ(GPS_INS gps_ins);
-
-        ///大地转车体
-        Point2D Coordinate_Transfer(double x_path, double y_path, GPS_INS pos);
-
-        ///车体转大地
-        GPS_INS Coordinate_UnTransfer(double x_path, double y_path, GPS_INS pos);
-        double GetDistance(Point2D x1, Point2D x2);
-        double GetDistance(GPS_INS p1, GPS_INS p2);
-    }
-}
-
-#endif // ! _IV_DECITION_TRANSFER_

src1/decision/common/common/car_status.cpp

@@ -1,24 +0,0 @@
-#include "car_status.h"
-
-#ifdef linux
-
-unsigned long GetTickCount()
-{
- struct timespec ts;
- clock_gettime(CLOCK_MONOTONIC,&ts);
- return (ts.tv_sec * 1000 + ts.tv_nsec/(1000*1000) );
-}
-#endif
-
-iv::CarStatus::CarStatus()
-{
-	speed = 0;
-	braking_pressure = 0;
-	wheel_angle = 0;
-	location = boost::shared_ptr<iv::GPS_INS>(new iv::GPS_INS);
-    mRunTime.start();
-}
-
-iv::CarStatus::~CarStatus()
-{
-}

src1/decision/common/common/car_status.h

@@ -1,199 +0,0 @@
-#pragma once
-#ifndef _IV_COMMON_CAR_STATUS_
-#define _IV_COMMON_CAR_STATUS_
-
-#include <common/boost.h>
-#include <boost/serialization/singleton.hpp>
-#include <gps_type.h>
-#include <common/obstacle_type.h>
-#include <time.h>
-#include <common/mobileye.h>
-#include <QTime>
-
-///kkcai, 2020-01-03
-#include <QMutex>
-///////////////////////////////////////
-
-#include "platform/platform.h"
-
-#include "ultrasonic_type.h"
-
-//#include "common/decitionview.pb.h"
-
-#include "common/sysparam_type.h"
-
-#include "common/roadmode_type.h"
-
-#define RADAR_OBJ_NUM 64
-
-#ifdef linux
-unsigned long GetTickCount();
-#endif
-namespace iv {
-    class CarStatus : public boost::noncopyable {
-    public:
-        CarStatus();
-        ~CarStatus();
-
-        float speed;			//车速
-        double mfCalAcc = 0;
-        std::int16_t wheel_angle;		//方向盘转角
-        std::uint8_t braking_pressure;	//刹车压力
-         float torque_st=0;
-        bool mbRunPause = false;
-        bool mbBrainCtring = false;
-        bool status[6] = { true, false, false, false, true, false };	//bool arrive = false;	//  x4:是否到达站点（0:未到 1:到达）
-                                                                //bool people = false;	//	x3:车上是否有人（0:无人 1:有人）
-                                                                //bool stop = false;	    //	x2:是否停车（0:否   1:是）
-                                                                //bool call = false;		//	x1:是否叫车（0:否	1:是）
-                                                                //bool available = true;	// 是否可被叫车
-                                                                //bool fire = false;
- //       int carState = 0;	// 0:停车	1:正常循迹	2:前往站点
-
-        int carState = 0;	// 0:停车	1:正常循迹	2:前往站点
-        int emergencyStop=0;
-        int station_control_door=0;//0: door close    1:door open
-        bool station_control_door_option=false;
-                int istostation = 0;
-                //int ctostation = 0;
-                int currentstation = 0;
-                //int nextstation = 0;
-                int clientto = 0;
-                double amilng = 0.0;
-                double amilat = 0.0;
-                bool busmode = false;
-                bool netconnect = false;
-                bool doorsta=false;
-                unsigned char speedlimte;
-
-        int grade=0;
-        int driverMode=0;
-        int epb=0;
-        int epsMode=1;
-        float engine_speed=0;
-        bool receiveEps=false;
-
-
-              bool speed_control=true;
-              bool group_control =false;
-              int group_server_status=0;
-              int  group_state = 0;
-              float group_x_local = 0.0;
-              float group_y_local = 0.0;
-              float group_velx_local = 0.0;
-              float group_vely_local = 0.0;
-              float group_comm_speed = 0.0;
-              int group_pathpoint=0;
-              float group_offsetx=0.0;
-              float group_frontDistance=0.0;
-
-              float mfttc = 0;
-
-
-        std::vector <iv::platform::station> car_stations;
-
-
-        AWS_display aws_display;
-        lane Lane;
-        obstacle_status obstacleStatus;
-        obstacle_data_a obstacleStatusA[15];
-        obstacle_data_b obstacleStatusB[15];
-        obstacle_data_c obstacleStatusC[15];
-        aftermarket_lane aftermarketLane;
-        lka_left_lane_a LKAleftLaneA;
-        lka_left_lane_b LKAleftLaneB;
-        lka_right_lane_a LKArightLaneA;
-        lka_right_lane_b LKArightLaneB;
-        next_lane_a nextLaneA_left[4], nextLaneA_right[4];
-        next_lane_b nextLaneB_left[4], nextLaneB_right[4];
-        ref_points refPoints;
-        num_of_next_lane_mark_reported numOfNextLaneMarkReported;
-
-
-        double mfAcc = 0;
-        double mfVehAcc=0;
-        double mfWheel = 0;
-        double mfBrake = 0;
-        double steerAngle=0;
-        QTime mRunTime;
-
-        int mRadarS = -1;
-        int mLidarS = -1;
-        int mRTKStatus = 0;
-        int mLidarPer = -1;
-
-        double mLidarObs;
-        double mRadarObs;
-        double mObs;
-
-
-        GPS_INS aim_gps_ins;
-        bool bocheMode=false;
-        int bocheEnable=0;
-
-        double mfParkLat;
-        double mfParkLng;
-        double mfParkHeading;
-        int mnParkType;
-
-
-        double mLidarRotation;
-        double mLidarRangeUnit;
-
-        iv::GPSData location;		//当前车辆位置
-        boost::array<iv::ObstacleBasic, 64> obs_radar;//毫米波雷达的障碍物数据
-         boost::array<iv::ObstacleBasic, 64> obs_rear_radar;//houxiang毫米波雷达的障碍物数据
-        iv::ultrasonic_obs multrasonic_obs;
-        std::vector<iv::TracePoint> obsTraceLeft,obsTraceRight;
-
-        double mfGPSAcc = 0;
-
-//        iv::brain::decitionview mdecitionview;
-        iv::sysparam msysparam;
-        iv::roadmode_vel mroadmode_vel;
-        int vehGroupId;
-        double mfEPSOff = 0.0;
-        float socfDis=15;   //停障触发距离
-        float aocfDis=25;   //避障触发距离
-        float aocfTimes=5; //避障触发次数
-        float aobzDis=5;   //避障保障距离
-        /// traffice light : 0x90
-        int iTrafficeLightID = 0;       //红绿灯ID
-        int iTrafficeLightColor = 2;    //0x0: 红色
-        //0x1: 黄色
-        //0x2: 绿色
-        int iTrafficeLightTime = 0;     //红绿灯剩余时间
-        double iTrafficeLightLat = 0;
-        double iTrafficeLightLon = 0;
-
-        bool daocheMode=false;
-
-        //////////////////////////////////////////////////////
-
-        ///kkcai, 2020-01-03
-        QMutex mMutexMap;
-        /////////////////////////////////////
-
-        //mobileEye chuku
-
-        float vehSpeed_st=0;
-
-        bool useMobileEye = false;
-        bool m_bOutGarage=false;//车道线识别出库标志
-        float outGarageLong=10;
-        float waitGpsTimeWidth=5000;
-
-        int mnBocheComplete = 0;   //When boche comple set a value.
-
-        uint32_t ultraDistance[13];
-        bool useObsPredict = false;
-        bool useLidarPerPredict = false;
-        bool avoidObs = false;
-        bool inRoadAvoid = false;
-        //hapo station 2021/02/05
-        iv::StationCmd   stationCmd;
-    };
-    typedef boost::serialization::singleton<iv::CarStatus> CarStatusSingleton;//非线程安全，注意多线程加锁，单例模式
-}
-#define ServiceCarStatus iv::CarStatusSingleton::get_mutable_instance()
-#endif // !_IV_COMMON_CAR_STATUS_

src1/decision/common/common/common.pri

@@ -1,22 +0,0 @@
-DISTFILES +=
-
-HEADERS += \
-    $$PWD/boost.h \
-    $$PWD/car_status.h \
-    $$PWD/constants.h \
-    $$PWD/gps_type.h \
-    $$PWD/lidar.h \
-    $$PWD/logout.h \
-    $$PWD/obstacle_type.h \
-    $$PWD/ultrasonic_type.h \
-    $$PWD/hmi_type.h \
-    $$PWD/platform_type.h \
-    $$PWD/group_type.h \
-    $$PWD/fusion.h \
-    $$PWD/gps_type.h \
-    $$PWD/roadmode_type.h \
-    $$PWD/sysparam_type.h
-
-SOURCES += \
-    $$PWD/car_status.cpp \
-    $$PWD/lidar.cpp

src1/decision/common/common/constants.h

@@ -1,43 +0,0 @@
-#pragma once
-#ifndef _IV_COMMON_CONSTANTS_
-#define _IV_COMMON_CONSTANTS_
-
-#include <common/car_status.h>
-namespace iv {
-
-
-    const std::uint8_t SPEED_MAX = 30;						//车速上限
-    const std::uint8_t BRAKING_PRESSURE_PERCENT_MAX = 50;	//刹车压力上限
-    const std::uint8_t ACCELERATE_PERCENT_MAX = 1;			//油门开合度上限
-
-    const std::uint8_t CONTROL_SPEED_TOLERANCE = 5;		//控制车速时容忍误差范围
-    const std::uint8_t CONTROL_WHEEL_ANGLE_TOLERANCE = 5;	//控制方向盘时容忍误差范围
-
-    const std::uint16_t WHEEL_ZERO = 0x1F4A;
-    //static iv::CarStatus CarStatus_(new iv::CarStatusBasic);
-
-    const float SPEED_RATIO = 0.08;		//实际车速和can中读出的车速比率
-
-    const float Veh_Width = 2.1;		//车宽
-    const float Veh_Lenth = 4.6;
-    const float Esr_Offset = 0;		//ESR偏移量
-    const float Esr_Y_Offset = 2.5;  //ESR纵向偏移量
-
-    const std::uint8_t THREAD_EXECUTECONTROL_MAXNUM = 1;	//decition_executer 执行线程最大数量
-
-
-    const double MaxValue = 1.7976931348623157E+308;  //     Represents the largest possible value of a System.Double. This field is constant.
-
-
-    const double MinValue = -1.7976931348623157E+308; //     Represents the smallest possible value of a System.Double. This field is constant.
-
-
-
-
-
-
-}
-
-
-
-#endif // !_IV_COMMON_CONSTANTS_

src1/decision/common/common/fusion.h

@@ -1,37 +0,0 @@
-#pragma once
-#ifndef FUSION_H
-#define FUSION_H
-
-#include <common/boost.h>
-#include <boost/serialization/singleton.hpp>
-#include <common/obstacle_type.h>
-#include <common/car_status.h>
-#include <QMutex>
-
-
-namespace iv {
-    class Fusion : public boost::noncopyable {
-    public:
-        Fusion();
-        ~Fusion();
-        bool genggai1 = false, genggai2 = false;
-        bool is_run = false;
-        boost::mutex mtx1, mtx2;
-        iv::ObsFsuion fusion_Grid;
-        iv::ObsFsuion fusion_;
-        QMutex mMutexper;
-        std::shared_ptr<iv::fusion::fusionobjectarray> mper;
-
-        void copyfromlidar(iv::ObsLiDAR& obs);
-        void copylidarto(iv::ObsLiDAR& obs);
-        void copyfromlidarobs(iv::ObsLiDAR& obs);
-        void copylidarobsto(iv::ObsLiDAR& obs);
-
-        void copyfromlidarper(std::shared_ptr<std::vector<iv::Perception::PerceptionOutput>> & per);
-        void copylidarperto(std::shared_ptr<std::vector<iv::Perception::PerceptionOutput>> & per);
-        bool did_lidar_ok();
-    };
-    typedef boost::serialization::singleton<iv::Fusion> FusionSingleton;
-}
-#define ServiceFusion iv::FusionSingleton::get_mutable_instance()
-#endif // FUSION_H

src1/decision/common/common/group_type.h

@@ -1,18 +0,0 @@
-#ifndef GROUP_TYPE_H
-#define GROUP_TYPE_H
-
-
-namespace iv {
-    namespace group{
-        struct GroupMsg
-        {
-            int car_id=0;
-            float command_speed=0;
-            float veh_state=0;
-
-        };
-    }
-}
-
-
-#endif // GROUP_TYPE_H

src1/decision/common/common/hmi_type.h

@@ -1,16 +0,0 @@
-#ifndef HMI_TYPE_H
-#define HMI_TYPE_H
-
-#include <common/boost.h>
-namespace iv {
-    namespace hmi {
-        struct HMIBasic {
-            bool mbPause = false;
-            bool mbBocheMode = false;
-            bool mbbusmode = false;
-        };
-        typedef boost::shared_ptr<HMIBasic> Decition;	//决策
-    }
-}
-
-#endif // HMI_TYPE_H

src1/decision/common/common/logout.h

@@ -1,136 +0,0 @@
-/**
-* 用于输出log文件的类.
-*/
-
-
-#ifndef LOG_H      
-#define LOG_H      
-
-
-//log文件路径    
-#define LOG_FILE_NAME "log_319.txt"    
-
-//启用开关    
-#define LOG_ENABLE    
-
-#include <fstream>      
-#include <string>      
-#include <ctime>
-//#include <Windows.h>
-//#include <tchar.h>
-/*
-using namespace std;
-
-class CLog
-{
-public:
-	static void GetLogFilePath(CHAR* szPath)
-	{
-		GetModuleFileNameA(NULL, szPath, MAX_PATH);
-		memset(strrchr(szPath, _T('\\')), NULL, strlen(strrchr(szPath, _T('\\'))) * sizeof(CHAR));
-		strcat(szPath, "\\");
-		strcat(szPath, LOG_FILE_NAME);
-	}
-	//输出一个内容，可以是字符串(ascii)、整数、浮点数、布尔、枚举    
-	//格式为：[2011-11-11 11:11:11] aaaaaaa并换行    
-	template <class T>
-	static void WriteLog(T x)
-	{
-		CHAR szPath[MAX_PATH] = { 0 };
-		GetLogFilePath(szPath);
-
-		ofstream fout(szPath, ios::app);
-		fout.seekp(ios::end);
-		fout << GetSystemTime() << x << endl;
-		fout.close();
-	}
-
-	//输出2个内容，以等号连接。一般用于前面是一个变量的描述字符串，后面接这个变量的值    
-	template<class T1, class T2>
-	static void WriteLog2(T1 x1, T2 x2)
-	{
-		CHAR szPath[MAX_PATH] = { 0 };
-		GetLogFilePath(szPath);
-		ofstream fout(szPath, ios::app);
-		fout.seekp(ios::end);
-		fout << GetSystemTime() << x1 << " = " << x2 << endl;
-		fout.close();
-	}
-
-	template<class T1, class T2, class T3, class T4, class T5, class T6 >
-	static void WriteLog6(T1 x1, T2 x2, T3 x3, T4 x4, T5 x5, T6 x6)
-	{
-		CHAR szPath[MAX_PATH] = { 0 };
-		GetLogFilePath(szPath);
-		ofstream fout(szPath, ios::app);
-		fout.seekp(ios::end);
-		fout << GetSystemTime() << x1 << "  " << x2 << "  "  
-								<< x3 << "  " << x4 << "  " 
-								<< x5 << "  " << x6 << endl;
-		fout.close();
-	}
-
-	//输出一行当前函数开始的标志,宏传入__FUNCTION__    
-	template <class T>
-	static void WriteFuncBegin(T x)
-	{
-		CHAR szPath[MAX_PATH] = { 0 };
-		GetLogFilePath(szPath);
-		ofstream fout(szPath, ios::app);
-		fout.seekp(ios::end);
-		fout << GetSystemTime() << "    --------------------" << x << "  Begin--------------------" << endl;
-		fout.close();
-	}
-
-	//输出一行当前函数结束的标志，宏传入__FUNCTION__    
-	template <class T>
-	static void WriteFuncEnd(T x)
-	{
-		CHAR szPath[MAX_PATH] = { 0 };
-		GetLogFilePath(szPath);
-		ofstream fout(szPath, ios::app);
-		fout.seekp(ios::end);
-		fout << GetSystemTime() << "--------------------" << x << "  End  --------------------" << endl;
-		fout.close();
-	}
-
-
-private:
-	//获取本地时间，格式如"[2011-11-11 11:11:11] ";     
-	static string GetSystemTime()
-	{
-		time_t tNowTime;
-		time(&tNowTime);
-		tm* tLocalTime = localtime(&tNowTime);
-		char szTime[30] = { '\0' };
-		strftime(szTime, 30, "[%Y-%m-%d %H:%M:%S] ", tLocalTime);
-		string strTime = szTime;
-		return strTime;
-	}
-
-};
-
-#ifdef LOG_ENABLE    
-
-//用下面这些宏来使用本文件    
-#define LOG(x)              CLog::WriteLog(x);          //括号内可以是字符串(ascii)、整数、浮点数、bool等    
-#define LOG2(x1,x2)     CLog::WriteLog2(x1,x2);    
-#define LOG6(x1,x2,x3,x4,x5,x6)		CLog::WriteLog6(x1,x2,x3,x4,x5,x6);
-#define LOG_FUNC        LOG(__FUNCTION__)               //输出当前所在函数名    
-#define LOG_LINE        LOG(__LINE__)                       //输出当前行号    
-#define LOG_FUNC_BEGIN  CLog::WriteFuncBegin(__FUNCTION__);     //形式如：[时间]"------------FuncName  Begin------------"    
-#define LOG_FUNC_END     CLog::WriteFuncEnd(__FUNCTION__);      //形式如：[时间]"------------FuncName  End------------"    
-
-#else    
-
-#define LOG(x)                  
-#define LOG2(x1,x2)         
-#define LOG6(x1,x2,x3,x4,x5,x6)
-#define LOG_FUNC            
-#define LOG_LINE            
-#define LOG_FUNC_BEGIN      
-#define LOG_FUNC_END        
-
-#endif    
-*/
-#endif      

src1/decision/common/common/mobileye.h

@@ -1,169 +0,0 @@
-#ifndef MOBILEYE_H
-#define MOBILEYE_H
-
-// 0x700
-typedef struct _AWS_display
-{
-    bool dusk_time;
-    bool night_time;
-    bool lanes_on;
-}AWS_display;
-
-// 0x737
-typedef struct _lane{
-    double curvature;
-    double heading;
-    bool is_ca; //construction area
-    bool is_right_LDW_available;
-    bool is_left_LDW_available;
-    double yaw;
-    double pitch;
-}lane;
-
-// 0x738
-typedef struct _obstacle_status{
-    int num_obstacles;
-    int timestamp;
-    int app_version;
-    int active_version;
-    bool is_left_close_rang_cut_in;
-    bool is_right_close_rang_cut_in;
-    int go;
-    int protocol_version;
-    bool is_close_car;
-    int failsafe;
-}obstacle_status;
-
-// 0x739
-typedef struct _obstacle_data_a{
-    int obstacle_ID;
-    double obstacle_pos_x;
-    double obstacle_pos_y;
-    int blinker_info;
-    int cut_in_and_out;
-    double obstacle_rel_vel_x;
-    int obstacle_type;
-    int obstacle_status;
-    bool is_obstacle_brake_lights;
-    int obstacle_valid;
-}obstacle_data_a;
-
-// 0x73a
-typedef struct _obstacle_data_b{
-    double obstacle_length;
-    double obstacle_width;
-    int obstacle_age;
-    int obstacle_lane;
-    bool is_cipv_flag;
-    double radar_pos_x;
-    double radar_vel_x;
-    int radar_match_confidence;
-    int matched_radar_id;
-}obstacle_data_b;
-
-// 0x73b
-typedef struct _obstacle_data_c{
-    double obstacle_angle_rate;
-    double obstacle_scale_change;
-    double object_accel_x;
-    bool is_obstacle_replaced;
-    double obstacle_angle;
-}obstacle_data_c;
-
-// 0x669
-typedef struct _aftermarket_lane{
-    unsigned int lane_conf_left;
-    bool is_ldw_availablility_left;
-    unsigned int lane_type_left;
-    double dist_to_lane_l;
-    unsigned int lane_conf_right;
-    bool is_ldw_availablility_right;
-    unsigned int lane_type_right;
-    double dist_to_lane_r;
-}aftermarket_lane;
-
-// 0x766
-typedef struct _lka_left_lane_a
-{
-    int lane_type;
-    int quality;
-    int model_degree;
-    double position;
-    double curvature;
-    double curvature_derivative;
-    double width_left_marking;
-}lka_left_lane_a;
-
-// 0x767
-typedef struct _lka_left_lane_b
-{
-    double heading_angle;
-    double view_range;
-    bool is_view_range_availability;
-}lka_left_lane_b;
-
-// 0x768
-typedef struct _lka_right_lane_a
-{
-    int lane_type;
-    int quality;
-    int model_degree;
-    double position;
-    double curvature;
-    double curvature_derivative;
-    double width_left_marking;
-}lka_right_lane_a;
-
-// 0x769
-typedef struct _lka_right_lane_b
-{
-    double heading_angle;
-    double view_range;
-    bool is_view_range_availability;
-}lka_right_lane_b;
-
-// 0x76c
-typedef struct _next_lane_a
-{
-    int lane_type;
-    int quality;
-    int model_degree;
-    double position;
-    double curvature;
-    double curvature_derivative;
-    double lane_mark_width;
-}next_lane_a;
-
-// 0x76d
-typedef struct _next_lane_b
-{
-    double heading_angle;
-    double view_range;
-    bool is_view_range_availability;
-}next_lane_b;
-
-// 0x76a
-typedef struct _ref_points
-{
-    double ref_point_1_position;
-    double ref_point_1_dist;
-    bool is_ref_point_1_validity;
-    double ref_point_2_position;
-    double ref_point_2_dist;
-    bool is_ref_point_2_validity;
-}ref_points;
-
-// 0x76b
-typedef struct _num_of_next_lane_mark_reported
-{
-    int num_of_next_lane_mark_reported;
-}num_of_next_lane_mark_reported;
-
-typedef struct _obstacle_info
-{
-    obstacle_data_a obs_a;
-    obstacle_data_b obs_b;
-    obstacle_data_c obs_c;
-}obstacle_info;
-
-#endif // MOBILEYE_H

src1/decision/common/common/platform_type.h

@@ -1,18 +0,0 @@
-#ifndef PLATFORM_TYPE_H
-#define PLATFORM_TYPE_H
-
-namespace iv {
-namespace platform{
-struct PlatFormMsg
-{
-    int carState = 0;	// 0:停车	1:正常循迹	2:前往站点
-    int istostation = 0;
-    int currentstation = 0;
-    int clientto = 0;
-    double amilng = 0.0;
-    double amilat = 0.0;
-};
-}
-}
-
-#endif // PLATFORM_TYPE_H

src1/decision/common/common/roadmode_type.h

@@ -1,25 +0,0 @@
-#ifndef ROADMODE_TYPE_H
-#define ROADMODE_TYPE_H
-
-
-namespace iv {
-    struct roadmode_vel
-    {
-
-        double mfmode0 = 10;
-         double mfmode1 = 10;
-        double mfmode11 = 30;
-        double mfmode14 = 10;
-        double mfmode15 = 10;
-
-        double mfmode5;
-        double mfmode13;
-        double mfmode16;
-        double mfmode17;
-        double mfmode18;
-
-
-    };
-}
-
-#endif // ROADMODE_TYPE_H

src1/decision/common/common/sysparam_type.h

@@ -1,40 +0,0 @@
-#ifndef SYSPARAM_TYPE_H
-#define SYSPARAM_TYPE_H
-
-#include <string>
-
-namespace iv {
-    struct sysparam
-    {
-
-
-        std::string mstrvin;
-        std::string mstriccid;
-        std::string mstrid = "1";
-        std::string mstrepsoff;
-        std::string mvehtype;
-        std::string mgroup;
-        float mzhuchetime;
-
-        float lidarGpsXiuzheng=3.5;
-        float radarGpsXiuzheng=3.5;
-        float rearRadarGpsXiuzheng=0.5;
-        float rearLidarGpsXiuzheng=3.5;
-        float frontGpsXiuzheng=2.0;
-        float rearGpsXiuzheng=1.0;
-        float lidarMobileyeXiuzheng=0;
-        float radarMobileyeXiuzheng=0;
-        float rearRadarMobileyeXiuzheng=0;
-        float gpsOffset_x=0;
-        float gpsOffset_y=0;
-
-        float vehWidth= 2.1;
-        float vehLenth= 4.6;
-
-        bool keni =false;
-
-
-    };
-}
-
-#endif // SYSPARAM_TYPE_H

src1/decision/common/common/ultrasonic_type.h

@@ -1,19 +0,0 @@
-#ifndef ULTRASONIC_TYPE_H
-#define ULTRASONIC_TYPE_H
-
-namespace  iv {
-struct ultrasonic_obs
-{
-   float mfront_left;
-   float mfront_leftmid;
-   float mfront_rightmid;
-   float mfront_right;
-   float mrear_left;
-   float mrear_leftmid;
-   float mrear_rightmid;
-   float mrear_right;
-};
-
-}
-
-#endif // ULTRASONIC_TYPE_H

src1/decision/common/common/vv7.h

@@ -1,41 +0,0 @@
-#pragma once
-
-struct Command_Bit
-{
-    unsigned char head;
-
-    unsigned char acce;
-
-    unsigned char ang_H;
-
-    unsigned char ang_L;
-
-    unsigned char right_turn : 1;
-
-    unsigned char left_turn : 1;
-
-    unsigned char bright : 1;
-
-    unsigned char flicker : 1;
-
-    unsigned char Reserved : 4;
-
-    unsigned char engine : 2;
-
-    unsigned char door : 2;
-
-    unsigned char speaker : 1;
-
-    unsigned char Reserved1 : 3;
-
-    unsigned char heartbreak;
-
-    unsigned char checksum;
-};
-
-union Command
-{
-    Command_Bit bit;
-
-    unsigned char byte[8];
-};

src1/decision/common/platform/dataformat.h

@@ -1,532 +0,0 @@
-#ifndef DATAFORMAT_H
-#define DATAFORMAT_H
-
-//////////////////////////////
-//         命令标识          //
-/////////////////////////////
-#define carCheckin (0x01)//车辆登入
-#define realtimeInfoUpload (0x02)//实时信息上报
-#define supplementInfoUpload (0x03)//补发信息上报
-#define carCheckout (0x04)//车辆登出
-#define platformReserve1 (0x05)//平台数据传输占用
-#define platfoemReserve2 (0x06)//平台数据传输占用
-#define heartbeats (0x07)//心跳
-#define terminalCheckTime (0x08)//终端校时
-#define carStatus (0x09)//车身状态
-#define driveDataLow (0x0A)//驾驶行为数据（低频）
-#define driveDataHigh (0x0B)//驾驶行为数据（高频）
-#define carAlarm (0x0C)//车辆报警信息
-#define sensorConfig (0x0D)//传感器配置信息
-#define multimedia (0x0E)//多媒体信息
-#define V2X (0x0F)//V2X信息
-#define APPcarcall (0x10)//APP约车上行信息
-#define humanIntervene (0x11)//人工干预信息
-#define inquire (0x80)//查询命令
-#define setting (0x81)//设置命令
-#define carTerminalControl (0x82)//车载终端控制命令
-#define carLongdistanceControl (0x83)//车身远程遥控命令
-#define longdistanceDrive (0x84)//远程遥控驾驶
-#define appointmentDispatch (0x85)//预约与远程调度
-#define routePlane (0x86)//路径规划
-#define carCamControl (0x87)//车载摄像头控制
-#define platformTo (0x88)//与APP通讯下行信息
-#define downDataReserve (0x89)//下行数据系统预留
-#define platformReserve (0xC0）//平台交换自定义数据
-
-//////////////////////////////
-//         应答标识          //
-/////////////////////////////
-#define resuccess (0x01)//成功
-#define reerror (0x02)//错误
-#define VINrepetition (0x03)//VIN重复
-#define reserves (0x40)//保留
-#define comment (0xFE)//命令
-
-#include <string>
-
-using namespace std;
-
-namespace iv {
-namespace platform {
-    struct DataPackageHead//数据包结构
-    {
-        unsigned char startSymbol1;//起始符1
-        unsigned char startSymbol2;//起始符2
-        unsigned char commentSymbol;//命令标识
-        unsigned char responseSymbol;//应答标识
-        char VIN[17];//车辆识别码
-        unsigned char encryptionType;//数据加密方式
-        char dataLength[2];//数据单元长度
-       // unsigned char *data;//数据单元
-       // unsigned char verifyCode;//校验码
-    };
-
-    struct CarcheckinData //车辆登入数据包格式 0x01
-    {
-        unsigned char year;//年
-        unsigned char month;//月
-        unsigned char day;//日
-        unsigned char hour;//时
-        unsigned char minute;//分
-        unsigned char second;//秒
-        char checkinID[2];//登入号
-        char ICCID[20];//sim卡iccid号
-        unsigned char storageSysCount;//可充电储能系统数
-        unsigned char storageSysEncodeLen;//可充电储能系统编码长度
-        //string storageSysEncode[];//可充电储能系统编码
-    };
-
-    struct RealtimeInfo//实时数据上报  0x02
-    {
-        unsigned char year;//年
-        unsigned char month;//月
-        unsigned char day;//日
-        unsigned char hour;//时
-        unsigned char minute;//分
-        unsigned char second;//秒
-    };
-
-    struct CarData//整车数据
-    {
-        unsigned char head;//头
-        unsigned char carstatus;//车辆状态
-        unsigned char chargeStatus;//充电状态
-        unsigned char runMode;//运行模式
-        char carSpeed[2];//车速
-        char carMileage[4];//累计里程
-        char totalVoltage[2];//总电压
-        char totalElectricity[2];//总电流
-        unsigned char SOC;//SOC
-        unsigned char DCDC;//DC-DC状态
-        unsigned char gear;//档位
-        char resistance[2];//绝缘电阻
-        unsigned char accelerateMile;//加速踏板行程值
-        unsigned char brakingMile;//制动踏板行程值
-    };
-
-    struct DriveMotorInfo//驱动电机数据
-    {
-        unsigned char head;//头
-        unsigned char motorCount;//驱动电机个数
-        unsigned char motorNum;//驱动电机序号
-        unsigned char motorStatus;//驱动电机状态
-        unsigned char motorControlTemperature;//驱动电机控制器温度
-        char motorRevolveSpeed[2];//驱动电机转速
-        char motorTorque[2];//驱动电机转矩
-        unsigned char motorTemperature;//驱动电机温度
-        char motorInputVoltage[2];//电机控制器输入电压
-        char motorElectricity[2];//电机控制器直流母线电流
-
-    };
-
-    struct FuelCellInfo//燃料电池数据
-    {
-        unsigned char head;//头
-        char fuelCellVoltage[2];//燃料电池电压
-        char fuelCellElectricity[2];//燃料电池电流
-        char fuelConsumptionRate[2];//燃料消耗率
-        char fuelTemperatureNeedleCount[2];//燃料电池温度探针总数
-        char needleTemperature[4];//探针温度值
-        char hydrogenTemperature[2];//氢系统中最高温度
-        unsigned char hydrogenTemperatureNum;//氢系统中最高温度探针代号
-        char hydrogenConcentration[2];//氢气最高浓度
-        unsigned char hydrogenConcentrationNum;//氢气最高浓度传感器代号
-        char hydrogenPressure[2];//氢气最高压力
-        unsigned char hydrogenPressureNum;//氢气最高压力传感器代号
-        unsigned char DCDC;//高压DC-DC状态
-    };
-
-    struct EngineInfo//发动机数据
-    {
-        unsigned char head;//头
-        unsigned char engineStatus;//发动机状态
-        char bentAxleRevolveSpeed[2];//曲轴转速
-        char fuelConsumptionRate[2];//燃料消耗率
-    };
-
-    struct LocationInfo//定位数据
-    {
-        unsigned char head;//头
-        unsigned char localStatus;//定位状态
-        char longtitude[4];//经度
-        char latitude[4];//纬度
-    };
-
-    struct ExtremumInfo//极值数据
-    {
-        unsigned char highVoltageNum;//最高电压电池子系统号
-        unsigned char highCellVoltageNum;//最高电池电压单体代号
-        char highCellVoltage[2];//电池单体电压最高值
-        unsigned char lowVoltageNum;//最低电压电池子系统代号
-        unsigned char lowCellVoltageNum;//最低电压电池单体代号
-        char lowCellVoltage[2];//电池单体电压最低值
-        unsigned char highTemperatureNum;//最高温度子系统号
-        unsigned char highNeedleTemNum;//最高温度探针序号
-        unsigned char highTemperature;//最高温度值
-        unsigned char lowTemperatureNum;//最低温度子系统号
-        unsigned char lowNeedleTemNum;//最低温度探针序号
-        unsigned char lowTemperature;//最低温度值
-
-    };
-
-    struct AlarmInfo//报警数据
-    {
-        unsigned char head;//头
-        unsigned char highAlarmGrade;//最高报警等级
-        char normalAlarm[4];//通用报警
-        unsigned char chargeableDeviceCount;//可充电储能装置故障总数
-        char chargeableDevice;//可充电储能装置故障代码列表
-        unsigned char driveMotorCount;//驱动电机故障总数
-        char driveMotor[4];//驱动电机故障代码列表
-        unsigned char engineCount;//发动机故障总数
-        char engine[4];//发动机故障列表
-        unsigned char elseCount;//其他故障总数
-        char elses[4];//其他故障代码列表
-    };
-
-
-    struct ChargeableDeviceVoltageInfo//可充电储能系统单体电压数据
-    {
-        unsigned char head;//头
-        unsigned char chargeableDeviceCount;//可充电储能装置子系统个数
-        unsigned char chargeableDeviceNum;//可充电储能装置子系统号
-        char chargeableDeviceVoltage[2];//可充电储能装置电压
-        char chargeableDeviceElectricity[2];//可充电储能装置电流
-        char cellCount[2];//单体电池总数
-        char beginCellNum[2];//本帧起始电池序号
-        unsigned char beginCellCount;//本帧单体电池总数
-    };
-
-    struct ChargeableDeviceTemperatureInfo//可充电储能装置温度数据
-    {
-        unsigned char head;//头
-        unsigned char chargeableDeviceCount;//可充电储能装置子系统个数
-        unsigned char chargeableDeviceNum;//可充电储能装置子系统号
-        char temperatureNeedleCount[2];//可充电储能温度探针个数
-        char temperature[4];//可充电储能子系统温度探针温度值
-    };
-
-    struct CarcheckoutData //车辆登出数据包格式 0x04
-    {
-        unsigned char year;//年
-        unsigned char month;//月
-        unsigned char day;//日
-        unsigned char hour;//时
-        unsigned char minute;//分
-        unsigned char second;//秒
-        char checkoutID[2];//登入号
-    };
-
-    struct CarStatus//车身状态  0x09
-    {
-        unsigned char year;//年
-        unsigned char month;//月
-        unsigned char day;//日
-        unsigned char hour;//时
-        unsigned char minute;//分
-        unsigned char second;//秒
-        unsigned char leftFrontdoor;//左前车门
-        unsigned char rightFrontdoor;//右前车门
-        unsigned char leftBackdoor;//左后车门
-        unsigned char rightBackdoor;//右后车门
-        unsigned char backTrunk;//后尾箱
-        unsigned char leftFrontWindow;//左前车窗
-        unsigned char rightFrontWindow;//右前车窗
-        unsigned char leftBackWindow;//左后车窗
-        unsigned char rightBackWindow;//右后车窗
-        unsigned char skyWindow;//天窗
-        unsigned char leftFrontLock;//左前车锁
-        unsigned char rightFrontLock;//右前车锁
-        unsigned char leftBackLock;//左后车锁
-        unsigned char rightBackLock;//右后车锁
-        unsigned char handBrake;//手刹
-        unsigned char positionLight;//位置灯
-        unsigned char nearLight;//近光灯
-        unsigned char farLight;//远光灯
-        unsigned char frontFogLight;//前雾灯
-        unsigned char backFogLight;//后雾灯
-        unsigned char airConditioner;//空调
-        unsigned char block;//档位
-        unsigned char key;//钥匙
-
-    };
-
-    struct DriveDataLow//驾驶行为数据 （低频）  0x0A
-    {
-        unsigned char year;//年
-        unsigned char month;//月
-        unsigned char day;//日
-        unsigned char hour;//时
-        unsigned char minute;//分
-        unsigned char second;//秒
-        unsigned char autoDriveMode;//自动驾驶模式
-        unsigned char urgentStop;//急停开关状态
-        unsigned char UCStatus;//UC工作状态
-        unsigned char UCError;//UC故障状态
-        unsigned char UCMode;//UC工作模式
-        unsigned char UC2EPSRequest;//UC请求控制EPS请求位
-        unsigned char UC2EPSMess;//UC请求控制EPS消息有效性
-        unsigned char CDDAccelerate;//指示CDD加速度请求信号有效性
-        unsigned char UCBlock;//UC请求档位
-        unsigned char brakeSatus;//刹车踏板状态
-        unsigned char leftStatus;//左转向状态
-        unsigned char rightStatus;//右转向状态
-        unsigned char UC2ACCStart;//指示UC是否有ACC起步请求
-        unsigned char UC2ACCStandstill;//指示UC是否有ACC standstill 刹停请求
-    };
-
-    struct DriveDataHigh//驾驶行为数据 （高频）  0x0B
-    {
-         unsigned char year;//年
-         unsigned char month;//月
-         unsigned char day;//日
-         unsigned char hour;//时
-         unsigned char minute;//分
-         unsigned char second;//秒
-         unsigned char collectCycle;//数据采集周期
-         char UC2EPSAngle[2];//UC请求控制EPS目标角度
-         unsigned char UC2AEBDecelerate;//指示UC是否有AEB减速请求
-         char carSpeed[2];//车速
-         char engineRevolve[2];//发动机转速
-         char accelerate[2];//实际加速度（减速度）
-         unsigned char UC2Accelerator;//UC请求油门踏板开度
-    };
-
-    struct CarAlarm//车辆报警信息 0x0C
-    {
-        unsigned char year;//年
-        unsigned char month;//月
-        unsigned char day;//日
-        unsigned char hour;//时
-        unsigned char minute;//分
-        unsigned char second;//秒
-        unsigned char EPSStatus;//EPS状态
-        unsigned char UCStatus;//UC工作状态
-        unsigned char UCError;//UC故障状态
-        unsigned char MCUStatus;//MCU工作状态反馈
-        unsigned char BCMStatus;//BCM当前状态反馈
-        unsigned char VCMStatus;//VCM工作状态反馈
-        unsigned char EPS2ACCError;//指示ESP检测到ACC发出的信息是否有误
-    };
-
-    struct SensorConfig//传感器配置信息  0x0D
-    {
-        unsigned char year;//年
-        unsigned char month;//月
-        unsigned char day;//日
-        unsigned char hour;//时
-        unsigned char minute;//分
-        unsigned char second;//秒
-        unsigned char lidar64;//64线激光雷达
-        unsigned char lidar16left;//左16线激光雷达
-        unsigned char lidar16right;//右16线激光雷达
-        unsigned char lidar4;//4线激光雷达
-        unsigned char radarFront;//前毫米波雷达
-        unsigned char radarBack;//后毫米波雷达
-        unsigned char GPS;//GPS
-        unsigned char camera;//摄像头
-    };
-
-
-    struct Multimedia//多媒体信息上报 0X0E
-    {
-        unsigned char year;//年
-        unsigned char month;//月
-        unsigned char day;//日
-        unsigned char hour;//时
-        unsigned char minute;//分
-        unsigned char second;//秒
-        char multimediaID[4];//多媒体ID
-        unsigned char multimediaType;//多媒体类型
-        unsigned char multimediaEncode;//多媒体格式编码
-        unsigned char eventEncode;//事件项编码
-        unsigned char channelID;//通道ID
-        char localzation[28];//位置信息上报
-        char multimediaData[100];//多媒体数据包
-    };
-
-    struct V2XInfo//V2X 信息  0x0F
-    {
-
-    };
-
-    struct ToPlatform//与云平台服务的上行消息  0x10
-    {
-        unsigned char year;//年
-        unsigned char month;//月
-        unsigned char day;//日
-        unsigned char hour;//时
-        unsigned char minute;//分
-        unsigned char second;//秒
-        unsigned char commID1;//上传信息ID
-        unsigned char commInfo;//约车反馈
-
-    };
-
-
-    struct HumanIntervene//人工干预信息上报  0x11
-    {
-        unsigned char year;//年
-        unsigned char month;//月
-        unsigned char day;//日
-        unsigned char hour;//时
-        unsigned char minute;//分
-        unsigned char second;//秒
-        unsigned char humanInterType;//人工干预类型
-        char humanInterTime[2];//人工干预转换时间
-    };
-
-    struct CarControl//车辆远程控制信息  0x83  下行
-    {
-        unsigned char year;//年
-        unsigned char month;//月
-        unsigned char day;//日
-        unsigned char hour;//时
-        unsigned char minute;//分
-        unsigned char second;//秒
-        unsigned char controlID1;//控制ID1
-        unsigned char leftFrontdoor;//左前门控制
-        unsigned char controlID2;//控制ID2
-        unsigned char rightFrontdoor;//右前门控制
-        unsigned char controlID3;//控制ID3
-        unsigned char leftBackdoor;//左后门控制
-        unsigned char controlID4;//控制ID4
-        unsigned char rightBackdoor;//右后门控制
-        unsigned char controlID5;//控制ID5
-        unsigned char backTrunk;//后尾箱控制
-        unsigned char controlID6;//控制ID6
-        unsigned char leftFrontWindow;//左前窗控制
-        unsigned char controlID7;//控制ID7
-        unsigned char rightFrontWindow;//右前窗控制
-        unsigned char controlID8;//控制ID8
-        unsigned char leftBackWindow;//左后窗控制
-        unsigned char controlID9;//控制ID9
-        unsigned char rightBackWindow;//右后窗控制
-        unsigned char controlID0A;//控制ID10
-        unsigned char skyWindow;//天窗
-        unsigned char controlID0B;//控制ID11
-        unsigned char leftFrontLock;//左前车锁
-        unsigned char controlID0C;//控制ID12
-        unsigned char rightFrontLock;//右前车锁
-        unsigned char controlID0D;//控制ID13
-        unsigned char leftBackLock;//左后车锁
-        unsigned char controlID0E;//控制ID14
-        unsigned char rightBackLock;//右后车锁
-        unsigned char controlID0F;//控制ID15
-        unsigned char handBrake;//手刹
-        unsigned char controlID10;//控制ID16
-        unsigned char positionLight;//位置灯
-        unsigned char controlID11;//控制ID17
-        unsigned char nearLight;//近光灯
-        unsigned char controlID12;//控制ID18
-        unsigned char farLight;//远光灯
-        unsigned char controlID13;//控制ID19
-        unsigned char frontFogLight;//前雾灯
-        unsigned char controlID14;//控制ID20
-        unsigned char backFogLight;//后雾灯
-        unsigned char controlID15;//控制ID21
-        unsigned char airConditioner;//空调
-        unsigned char controlID16;//控制ID22
-        char speed[2];//速度设置
-        unsigned char controlID17;//控制ID23
-        unsigned char key;//钥匙
-    };
-
-    struct FarControl//远程遥控驾驶  0x84  下行
-    {
-        unsigned char year;//年
-        unsigned char month;//月
-        unsigned char day;//日
-        unsigned char hour;//时
-        unsigned char minute;//分
-        unsigned char second;//秒
-        unsigned char platMode;//云平台工作模式
-        char CP2EPSAngle[2];//CP请求控制EPS目标角度
-        char CP2Decelerate[2];//CP请求制动减速度值
-        unsigned char CP2Accelerate;//CP请求油门开度
-        unsigned char CP2Block;//CP请求档位信息
-    };
-
-    struct AppointmentDispatch//预约与远程调度 0x85  下行
-    {
-        unsigned char year;//年
-        unsigned char month;//月
-        unsigned char day;//日
-        unsigned char hour;//时
-        unsigned char minute;//分
-        unsigned char second;//秒
-        unsigned char appointYear;//年  预约时间
-        unsigned char appointMonth;//月 预约时间
-        unsigned char appointDay;//日 预约时间
-        unsigned char appointHour;//时 预约时间
-        unsigned char appointMinute;//分 预约时间
-        unsigned char appointSecond;//秒 预约时间
-        unsigned char humanCount;//预约人数
-        unsigned char srcLocalStatus;//预约出发地定位状态
-        char srcLocalLong[4];//预约出发地经度
-        char srcLocalLat[4];//预约出发地纬度
-        unsigned char desLocalStatus;//目的地定位状态
-        char desLocalLong[4];//目的地定位经度
-        char desLocalLat[4];//目的地定位纬度
-
-    };
-
-    struct RoutePlaning
-    {
-        unsigned char year;//年
-        unsigned char month;//月
-        unsigned char day;//日
-        unsigned char hour;//时
-        unsigned char minute;//分
-        unsigned char second;//秒
-        unsigned char lineID;
-        char numName[5];
-        char usename[3];
-        char password[3];
-        char address[6];
-        char port[2];
-        char filename1[7];
-        char filename2[12];
-        char URLAddress[16];
-        char connectTime[2];
-    };
-
-
-    struct PlatformTo//与云平台服务的下行消息  0x88
-    {
-        unsigned char year;//年
-        unsigned char month;//月
-        unsigned char day;//日
-        unsigned char hour;//时
-        unsigned char minute;//分
-        unsigned char second;//秒
-        unsigned char infoID1;//下发信息ID
-        unsigned char appointStart;//出发
-        unsigned char infoID2;//下发信息ID
-        unsigned char carStart;//车辆出发
-        unsigned char infoID3;//下发信息ID
-        unsigned char carStop;//停车
-        unsigned char infoID4;//下发信息ID
-        unsigned char backToGarage;//返回车库
-        unsigned char infoID5;//下发信息ID
-        unsigned char reAppoint;//重新预约
-        unsigned char infoID6;//下发信息ID
-        unsigned char keepGoing;//继续行程
-    };
-
-    struct station
-    {
-        double longtitude;
-        double latitude;
-        int ID;
-    };
-
-
-
-
-
-}
-
-}
-
-#endif // DATAFORMAT_H

src1/decision/common/platform/platform.h

@@ -1,112 +0,0 @@
-#ifndef PLATFORM_H
-#define PLATFORM_H
-
-
-#include <common/boost.h>
-#include <istream>
-#include <stdlib.h>
-#include <string.h>
-#include <vector>
-#include <fstream>
-#include <qtimer.h>
-#include "dataformat.h"
-//#include <control/can.h>
-//#include <control/control_status.h>
-//#include <common/lidar.h>
-#include <common/car_status.h>
-#include <QtNetwork/QTcpSocket>
-#include <QFile>
-#include <QtNetwork/QNetworkRequest>
-#include <QtNetwork/QNetworkAccessManager>
-#include <QUrl>
-#include <QtNetwork/QNetworkReply>
-#include <QByteArray>
-//#include <QMessageBox>
-#include <QFileInfo>
-#include <QDir>
-
-#define SRV_ADDR_ "123.127.164.44"
-#define SRV_PORT_ 5602
-
-using namespace std;
-
-namespace iv {
-    namespace platform {
-        class Client
-        {
-        public:
-            Client();
-            ~Client();
-
-
-            double srclng;
-            double srclat;
-            double deslng;
-            double deslat;
-
-
-            void start();
-
-            void run();
-
-            void stop();
-
-            void Heartdetect();
-
-            int PackagetHeadInfo(unsigned char commendID, int dataLen);
-
-
-            bool CarCheckin(int count);//车辆登入0x01
-            void RealtimeInfo();//实时信息上报0x01
-            void SupplementInfo();//补发信息上报0x03
-            bool CarCheckout(int count);//车辆登出0x04
-
-            void Heartbeat();//发送心跳 0x07
-            void TerminalCheckTime();//终端校时0x08
-            void CarStatusInfo();//车身状态数据0x09
-            void DriveDataLowInfo();//驾驶行为数据  低频 0x0A
-            void DriveDataHighInfo();//驾驶行为数据  高频  0x0B
-            void SenserConfig();
-            void CarControlInfo(char recvBuf[]);
-            void LongdistanceDriveInfo(char recvBuf[]);
-            void AppointmentDispatchInfo(char recvBuf[]);
-            void PlatformtToInfo(char recvBuf[]);
-            void RoutePlaningInfo(char recvBuf[]);
-
-            void CarCallSwitch();
-
-
-            void Receive();//接收数据
-
-            char BCCEncode(char sbuf[],int len);//计算校验
-
-            bool BCCDecode(char sbuf[], int len);
-            void ReadStation(char *filepath);
-
-        private:
-
-
-            QTcpSocket *socket;
-            bool is_checkin ;
-            bool is_receive;
-            int heartdetect;
-            int heartdetect_last;
-            int checkInOutNum;
-            bool is_order;
-            bool is_going;
-            bool is_stopnext;
-            bool is_stopend;
-            bool is_stopmid;
-            bool is_endnow;
-            bool is_endbegin;
-            bool is_reorder;
-            bool is_keepgoing;
-
-            iv::platform::CarStatus carStatusInfo;
-            iv::platform::DataPackageHead packageDataHead;
-
-        };
-    }
-}
-
-#endif // PLATFORM_H

src1/decision/common/platform/platform.pri

@@ -1,6 +0,0 @@
-HEADERS += \
-    $$PWD/platform.h \
-    $$PWD/dataformat.h
-
-SOURCES += \
-    $$PWD/platform.cpp

src1/decision/decision_brain/.gitignore

@@ -1,73 +0,0 @@
-# This file is used to ignore files which are generated
-# ----------------------------------------------------------------------------
-
-*~
-*.autosave
-*.a
-*.core
-*.moc
-*.o
-*.obj
-*.orig
-*.rej
-*.so
-*.so.*
-*_pch.h.cpp
-*_resource.rc
-*.qm
-.#*
-*.*#
-core
-!core/
-tags
-.DS_Store
-.directory
-*.debug
-Makefile*
-*.prl
-*.app
-moc_*.cpp
-ui_*.h
-qrc_*.cpp
-Thumbs.db
-*.res
-*.rc
-/.qmake.cache
-/.qmake.stash
-
-# qtcreator generated files
-*.pro.user*
-
-# xemacs temporary files
-*.flc
-
-# Vim temporary files
-.*.swp
-
-# Visual Studio generated files
-*.ib_pdb_index
-*.idb
-*.ilk
-*.pdb
-*.sln
-*.suo
-*.vcproj
-*vcproj.*.*.user
-*.ncb
-*.sdf
-*.opensdf
-*.vcxproj
-*vcxproj.*
-
-# MinGW generated files
-*.Debug
-*.Release
-
-# Python byte code
-*.pyc
-
-# Binaries
-# --------
-*.dll
-*.exe
-

src1/decision/decision_brain/main.cpp

@@ -1,14 +0,0 @@
-#include <QCoreApplication>
-
-#include "ivlog.h"
-
-iv::Ivlog * givlog;
-
-
-int main(int argc, char *argv[])
-{
-    QCoreApplication a(argc, argv);
-
-    givlog = new iv::Ivlog("decision_brain");
-    return a.exec();
-}

src1/decision/interface/decition_type.h

@@ -1,61 +0,0 @@
-#pragma once
-#ifndef _IV_DECITION_DECITION_TYPE_
-#define _IV_DECITION_DECITION_TYPE_
-#include "boost.h"
-namespace iv {
-namespace decition {
-struct DecitionBasic {
-    float speed;				//车速
-    float wheel_angle;			//转向角度
-    float brake;				//刹车
-    float accelerator;			//油门
-    float torque;               //力矩
-    bool leftlamp;				//左转向灯
-    bool rightlamp;				//右转向灯
-
-    int  engine;
-    int  grade;
-    int  mode;
-    int handBrake;
-    bool speak;
-    bool door;
-    bool bright;
-    int dangWei;
-
-    float angSpeed;
-    int brakeType :1;
-    char brakeEnable;  //add by fjk
-    bool left;         //add by fjk
-    bool right;        //add by fjk
-
-    bool angleEnable;
-    bool speedEnable;
-    bool dangweiEnable;
-    int  driveMode;
-
-    int directLight;
-    int brakeLight;
-    int backLight;
-    int topLight;
-
-    int farLight;
-    int nearLight;
-
-
-
-    bool   air_enable ;                  //空调使能
-    bool   air_on;
-    float   air_temp ;                  //空调温度
-    float   air_mode ;                  //空调模式
-    float   wind_level ;                  //空调风力
-    float   roof_light ;                  //顶灯
-    float   home_light ;                  //日光灯
-    float   air_worktime ;                  //空调工作时间
-    float   air_offtime ;                  //空调关闭时间
-
-};
-typedef boost::shared_ptr<DecitionBasic> Decition;	//决策
-}
-}
-#endif // !_IV_DECITION_DECITION_TYPE_
-

src1/decision/interface/ivdecision.cpp

@@ -1,367 +0,0 @@
-#include "ivdecision.h"
-
-#include "gnss_coordinate_convert.h"
-
-namespace iv {
-
-
-ivdecision::ivdecision()
-{
-
-}
-
-void ivdecision::modulerun()
-{
-    ModuleFun fungps = std::bind(&iv::ivdecision::UpdateGPS,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
-    mpagpsmsg = iv::modulecomm::RegisterRecvPlus(mstrgpsmsgname.data(),fungps);
-    ModuleFun funlidar = std::bind(&iv::ivdecision::UpdateLIDAR,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
-    mpalidarmsg = iv::modulecomm::RegisterRecvPlus(mstrlidarmsgname.data(),funlidar);
-    ModuleFun funradar = std::bind(&iv::ivdecision::UpdateRADAR,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
-    mparadarmsg = iv::modulecomm::RegisterRecvPlus(mstrradarmsgname.data(),funradar);
-    ModuleFun funmap = std::bind(&iv::ivdecision::UpdateMAP,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
-    mpamapmsg = iv::modulecomm::RegisterRecvPlus(mstrmapmsgname.data(),funmap);
-    ModuleFun funmobileye = std::bind(&iv::ivdecision::UpdateMobileye,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
-    mpamobileyemsg = iv::modulecomm::RegisterRecvPlus(mstrmobileyemsgname.data(),funmobileye);
-    ModuleFun funhmi = std::bind(&iv::ivdecision::UpdateHMI,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
-    mpahmimsg = iv::modulecomm::RegisterRecvPlus(mstrhmimsgname.data(),funhmi);
-    mpapadmsg = iv::modulecomm::RegisterRecvPlus(mstrpadmsgname.data(),funhmi);
-
-    while(mbrun)
-    {
-        //RunDecision
-        //ShareMsg
-    }
-
-    iv::modulecomm::Unregister(mpapadmsg);
-    iv::modulecomm::Unregister(mpahmimsg);
-    iv::modulecomm::Unregister(mpamobileyemsg);
-    iv::modulecomm::Unregister(mpamapmsg);
-    iv::modulecomm::Unregister(mparadarmsg);
-    iv::modulecomm::Unregister(mpalidarmsg);
-    iv::modulecomm::Unregister(mpagpsmsg);
-}
-
-void ivdecision::UpdateGPS(const char *strdata, const unsigned int nSize, const unsigned int index, const QDateTime *dt, const char *strmemname)
-{
-    (void )&index;
-    (void )&dt;
-    (void )strmemname;
-
-    if(nSize<1)return;
-
-    iv::gps::gpsimu xgpsimu;
-    if(!xgpsimu.ParseFromArray(strdata,nSize))
-    {
-       std::cout<<"ivdecision::UpdateGPSMsg error."<<std::endl;
-       return;
-    }
-
-    iv::GPSData data(new iv::GPS_INS);
-    data->ins_status = xgpsimu.ins_state();
-    data->rtk_status = xgpsimu.rtk_state();
-
-    data->accel_x = xgpsimu.acce_x();
-    data->accel_y = xgpsimu.acce_y();
-    data->accel_z = xgpsimu.acce_z();
-    data->ang_rate_x = xgpsimu.gyro_x();
-    data->ang_rate_y = xgpsimu.gyro_y();
-    data->ang_rate_z = xgpsimu.gyro_z();
-
-    data->gps_lat = xgpsimu.lat();
-    data->gps_lng = xgpsimu.lon();
-    data->gps_z = xgpsimu.height();
-
-    data->ins_heading_angle = xgpsimu.heading();
-    data->ins_pitch_angle = xgpsimu.pitch();
-    data->ins_roll_angle = xgpsimu.roll();
-
-    data->speed = sqrt(pow(xgpsimu.ve(),2)+pow(xgpsimu.vn(),2)) * 3.6;
-
-    GaussProjCal(data->gps_lng, data->gps_lat, &data->gps_x, &data->gps_y);
-
-    mMutexGPS.lock();
-    iv::gps::gpsimu * pgpsimu = new iv::gps::gpsimu;
-    pgpsimu->CopyFrom(xgpsimu);
-    mGPSIMUptr.reset(pgpsimu);
-    mNowGPS = data;
-    mnGPSUpdateTime = QDateTime::currentMSecsSinceEpoch();
-    mMutexGPS.unlock();
-
-
-
-
-}
-
-bool ivdecision::GetGPS(GPSData &xGPSData)
-{
-    if((QDateTime::currentMSecsSinceEpoch() - mnGPSUpdateTime)>mnNewThresh)
-    {
-        xGPSData = NULL;
-        return false;
-    }
-    xGPSData = mNowGPS;
-    return true;
-}
-
-void ivdecision::UpdateRADAR(const char *strdata, const unsigned int nSize, const unsigned int index, const QDateTime *dt, const char *strmemname)
-{
-    (void )&index;
-    (void )&dt;
-    (void )strmemname;
-
-    if(nSize<1)return;
-
-    iv::radar::radarobjectarray * pradar = new iv::radar::radarobjectarray;
-    if(!pradar->ParseFromArray(strdata,nSize))
-    {
-        std::cout<<"ivdecision::UpdateRADARMsg Parse Error"<<std::endl;
-        return;
-    }
-
-    mMutexRADAR.lock();
-    mRADAR.reset(pradar);
-    mnRADARUpdateTime = QDateTime::currentMSecsSinceEpoch();
-    mMutexRADAR.unlock();
-
-}
-
-bool ivdecision::GetRADAR(std::shared_ptr<radar::radarobjectarray> &xRADAR)
-{
-    if((QDateTime::currentMSecsSinceEpoch() - mnRADARUpdateTime)>mnNewThresh)
-    {
-        xRADAR = NULL;
-        return false;
-    }
-    xRADAR = mRADAR;
-    return true;
-}
-
-void ivdecision::UpdateLIDAR(const char *strdata, const unsigned int nSize, const unsigned int index, const QDateTime *dt, const char *strmemname)
-{
-    (void )&index;
-    (void )&dt;
-    (void )strmemname;
-
-    if(nSize<1)return;
-
-    unsigned int nCount = nSize/sizeof(iv::ObstacleBasic);
-    std::shared_ptr<std::vector<iv::ObstacleBasic>> lidar_obs = std::shared_ptr<std::vector<iv::ObstacleBasic>>(new std::vector<iv::ObstacleBasic>);
-    lidar_obs->resize(nCount);
-    memcpy(lidar_obs->data(),strdata,nCount*sizeof(iv::ObstacleBasic));
-    mMutexLIDAR.lock();
-    mlidar_obs = lidar_obs;
-    mnLIDARUpdateTime = QDateTime::currentMSecsSinceEpoch();
-    mMutexLIDAR.unlock();
-}
-
-bool ivdecision::GetLIDARGrid(LidarGridPtr &lidargridptr)
-{
-    if((QDateTime::currentMSecsSinceEpoch() - mnLIDARUpdateTime)>mnNewThresh)
-    {
-        lidargridptr = NULL;
-        return false;
-    }
-
-    LidarGridPtr ptr = (Obs_grid *)malloc(sizeof(Obs_grid[iv::grx][iv::gry]));
-    memset(ptr, 0, sizeof(Obs_grid[iv::grx][iv::gry]));
-    for (int i = 0; i <iv::grx; i++)//复制到指针数组
-    {
-        for (int j = 0; j <iv::gry; j++)
-        {
-//            ptr[i * (iv::gry) + j].high = obs_grid[i][j].high;
-//            ptr[i * (iv::gry) + j].low = obs_grid[i][j].low;
-            ptr[i * (iv::gry) + j].ob = 0;
-//            ptr[i * (iv::gry) + j].obshight = obs_grid[i][j].obshight;
-//            ptr[i * (iv::gry) + j].pointcount = obs_grid[i][j].pointcount;
-        }
-    }
-
-    mMutexLIDAR.lock();
-
-    for(unsigned int i=0;i<mlidar_obs->size();i++)
-    {
-        iv::ObstacleBasic xobs = mlidar_obs->at(i);
-        int dx,dy;
-        dx = (xobs.nomal_x + gridwide * (double)centerx)/gridwide;
-        dy = (xobs.nomal_y + gridwide * (double)centery)/gridwide;
-        if((dx>=0)&&(dx<iv::grx)&&(dy>=0)&&(dy<iv::gry))
-        {
-            ptr[dx*(iv::gry) +dy].high = xobs.high;
-            ptr[dx*(iv::gry) +dy].low = xobs.low;
-            ptr[dx*(iv::gry) + dy].ob = 2;
-            ptr[dx*(iv::gry) + dy].obshight = xobs.nomal_z;
-            ptr[dx*(iv::gry) + dy].pointcount = 5;
-        }
-
-    }
-    lidargridptr = ptr;
-
-
-    mMutexLIDAR.unlock();
-
-    return true;
-}
-
-bool ivdecision::GetLIDARObs(std::shared_ptr<std::vector<ObstacleBasic> > &xlidar_obs)
-{
-    if((QDateTime::currentMSecsSinceEpoch() - mnLIDARUpdateTime)>mnNewThresh)
-    {
-        xlidar_obs = NULL;
-        return false;
-    }
-
-    mMutexLIDAR.lock();
-    xlidar_obs = mlidar_obs;
-    mMutexLIDAR.unlock();
-    return true;
-}
-
-void ivdecision::UpdateMAP(const char *strdata, const unsigned int nSize, const unsigned int index, const QDateTime *dt, const char *strmemname)
-{
-    (void )&index;
-    (void )&dt;
-    (void )strmemname;
-
-    if(nSize<1)return;
-
-    //    std::cout<<"update map "<<std::endl;
-    int gpsunitsize = sizeof(iv::GPS_INS);
-    int nMapSize = nSize/gpsunitsize;
-    //    std::cout<<"map size is "<<nMapSize<<std::endl;
-
-    if(nMapSize < 1)return;
-
-    bool bUpdate = true;
-//    if(nMapSize != mnavigation_data.size())
-//    {
-//        bUpdate = true;
-//    }
-//    else
-//    {
-//        iv::GPS_INS * p = (iv::GPS_INS *)mapdata;
-//        if((p->gps_lat == navigation_data.at(0)->gps_lat)&&(p->ins_heading_angle == navigation_data.at(0)->ins_heading_angle))
-//        {
-//            //           qDebug("same map");
-//            bUpdate = false;
-//        }
-//        else
-//        {
-//            bUpdate = true;
-//        }
-//    }
-
-    if(bUpdate)
-    {
-
-        mMutexMAP.lock();
-        mnavigation_data.clear();
-        mnavigation_data.resize(nMapSize);
-        memcpy(mnavigation_data.data(),strdata,nMapSize * sizeof(iv::GPS_INS));
-        mnMAPUpdateTime = QDateTime::currentMSecsSinceEpoch();
-        mMutexMAP.unlock();
-
-    }
-    else
-    {
-
-    }
-}
-
-bool ivdecision::IsMAPUpdate(qint64 &nLastUpdateTime)
-{
-    if(mnavigation_data.size()<1)
-    {
-        return false;
-    }
-    if(nLastUpdateTime != mnMAPUpdateTime)
-    {
-        return true;
-    }
-    return false;
-}
-
-bool ivdecision::GetMAP(std::vector<GPSData> &navigation_data, qint64 &nLastUpdateTime)
-{
-    if(mnavigation_data.size()<1)
-    {
-        return false;
-    }
-    mMutexMAP.unlock();
-    nLastUpdateTime = mnMAPUpdateTime;
-    navigation_data = mnavigation_data;
-    mMutexMAP.unlock();
-    return true;
-}
-
-void ivdecision::UpdateMobileye(const char *strdata, const unsigned int nSize, const unsigned int index, const QDateTime *dt, const char *strmemname)
-{
-    (void )&index;
-    (void )&dt;
-    (void )strmemname;
-
-    if(nSize<1)return;
-
-    std::shared_ptr<iv::mobileye::mobileye> xmobileye = std::shared_ptr<iv::mobileye::mobileye>(new iv::mobileye::mobileye);
-
-    if(!xmobileye->ParseFromArray(strdata,nSize))
-    {
-        std::cout<<"ivdecision::UpdateMobileye Parse Error."<<std::endl;
-        return;
-    }
-
-    mMutexmobileye.lock();
-    mmobileye = xmobileye;
-    mnmobileyeUpdateTime = QDateTime::currentMSecsSinceEpoch();
-    mMutexmobileye.unlock();
-}
-
-bool ivdecision::GetMobileye(std::shared_ptr<mobileye::mobileye> &xmobileye)
-{
-    if((QDateTime::currentMSecsSinceEpoch() - mnmobileyeUpdateTime)>mnNewThresh)
-    {
-        xmobileye = NULL;
-        return false;
-    }
-
-    mMutexmobileye.lock();
-    xmobileye = mmobileye;
-    mMutexmobileye.unlock();
-    return true;
-}
-
-void ivdecision::UpdateHMI(const char *strdata, const unsigned int nSize, const unsigned int index, const QDateTime *dt, const char *strmemname)
-{
-    (void )&index;
-    (void )&dt;
-    (void )strmemname;
-
-    if(nSize<1)return;
-
-    std::shared_ptr<iv::hmi::hmimsg> xhmiptr = std::shared_ptr<iv::hmi::hmimsg>(new iv::hmi::hmimsg);
-
-    if(!xhmiptr->ParseFromArray(strdata,nSize))
-    {
-        std::cout<<"ivdecision::UpdateHMI Parse Error."<<std::endl;
-        return;
-    }
-
-    mMutexHMI.lock();
-    mHMImsg = xhmiptr;
-    mnHMIUpdateTime = QDateTime::currentMSecsSinceEpoch();
-    mMutexHMI.unlock();
-}
-
-bool ivdecision::GetHMImsg(std::shared_ptr<hmi::hmimsg> &xhmimsg)
-{
-    if((QDateTime::currentMSecsSinceEpoch() - mnHMIUpdateTime)>mnNewThresh)
-    {
-        xhmimsg = NULL;
-        return false;
-    }
-    mMutexHMI.lock();
-    mHMImsg = xhmimsg;
-    mMutexHMI.unlock();
-    return true;
-}
-
-}

src1/decision/interface/ivdecision.h

@@ -1,113 +0,0 @@
-#ifndef IVDECISION_H
-#define IVDECISION_H
-
-#include "ivmodule.h"
-
-#include "modulecomm.h"
-
-#include "gps_type.h"
-#include "obstacle_type.h"
-#include "gpsimu.pb.h"
-#include "radarobjectarray.pb.h"
-#include "mobileye.pb.h"
-#include "hmi.pb.h"
-#include "decition.pb.h"
-
-#include <QMutex>
-
-#include <memory>
-
-namespace iv {
-
-class ivdecision : public ivmodule
-{
-public:
-    ivdecision();
-
-public:
-    virtual void modulerun();
-
-
-public:
-    virtual int getdecision(iv::brain::decition & xdecition) = 0;
-    virtual void sharemsg() = 0;
-
-private:
-    std::string mstrgpsmsgname = "hcp2_gpsimu";
-    std::string mstrlidarmsgname = "lidar_obs";
-    std::string mstrradarmsgname = "radar";
-    std::string mstrmapmsgname = "tracemap";
-    std::string mstrmobileyemsgname = "mobileye";
-    std::string mstrhmimsgname = "hmi";
-    std::string mstrpadmsgname = "pad";
-
-private:
-    void * mpagpsmsg;
-    void * mpalidarmsg;
-    void * mparadarmsg;
-    void * mpamapmsg;
-    void * mpamobileyemsg;
-    void * mpahmimsg;
-    void * mpapadmsg;
-
-private:
-    void UpdateGPS(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname);
-    void UpdateRADAR(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname);
-    void UpdateLIDAR(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname);
-    void UpdateMAP(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname);
-    void UpdateMobileye(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname);
-    void UpdateHMI(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname);
-
-
-private:
-
-    iv::GPSData mNowGPS;
-    std::shared_ptr<iv::gps::gpsimu> mGPSIMUptr = 0;
-    qint64 mnGPSUpdateTime = 0;
-    QMutex mMutexGPS;
-
-    std::shared_ptr<iv::radar::radarobjectarray> mRADAR = 0;
-    qint64 mnRADARUpdateTime = 0;
-    QMutex mMutexRADAR;
-
-    std::shared_ptr<std::vector<iv::ObstacleBasic> > mlidar_obs;
-    qint64 mnLIDARUpdateTime = 0;
-    QMutex mMutexLIDAR;
-
-    std::vector<iv::GPSData> mnavigation_data;	//导航数据，GPS结构体数组
-    qint64 mnMAPUpdateTime = 0;
-    QMutex mMutexMAP;
-
-    std::shared_ptr<iv::mobileye::mobileye> mmobileye;
-    qint64 mnmobileyeUpdateTime = 0;
-    QMutex mMutexmobileye;
-
-    std::shared_ptr<iv::hmi::hmimsg> mHMImsg;
-    qint64 mnHMIUpdateTime = 0;
-    QMutex mMutexHMI;
-
-private:
-    const qint64 mnNewThresh = 1000; //3 seconds
-
-
-public:
-    bool GetGPS(iv::GPSData & xGPSData);
-    bool GetRADAR(std::shared_ptr<iv::radar::radarobjectarray> & xRADAR);
-    bool GetLIDARGrid(iv::LidarGridPtr & lidargridptr);
-    bool GetLIDARObs(std::shared_ptr<std::vector<iv::ObstacleBasic> > & xlidar_obs);
-    bool IsMAPUpdate(qint64 & nLastUpdateTime);
-    bool GetMAP(std::vector<iv::GPSData> & navigation_data,qint64 & nLastUpdateTime);
-    bool GetMobileye(std::shared_ptr<iv::mobileye::mobileye> & xmobileye);
-    bool GetHMImsg(std::shared_ptr<iv::hmi::hmimsg> & xhmimsg);
-
-//    void GetLidarPtr();
-//    void GetRadar();
-//    void GetMap();
-//    bool IsMapUpdate();
-//    void GetMobileye();
-
-};
-
-}
-
-#endif // IVDECISION_H

src1/detection/detection_radar_delphi_esr/.gitignore

@@ -1,73 +0,0 @@
-# This file is used to ignore files which are generated
-# ----------------------------------------------------------------------------
-
-*~
-*.autosave
-*.a
-*.core
-*.moc
-*.o
-*.obj
-*.orig
-*.rej
-*.so
-*.so.*
-*_pch.h.cpp
-*_resource.rc
-*.qm
-.#*
-*.*#
-core
-!core/
-tags
-.DS_Store
-.directory
-*.debug
-Makefile*
-*.prl
-*.app
-moc_*.cpp
-ui_*.h
-qrc_*.cpp
-Thumbs.db
-*.res
-*.rc
-/.qmake.cache
-/.qmake.stash
-
-# qtcreator generated files
-*.pro.user*
-
-# xemacs temporary files
-*.flc
-
-# Vim temporary files
-.*.swp
-
-# Visual Studio generated files
-*.ib_pdb_index
-*.idb
-*.ilk
-*.pdb
-*.sln
-*.suo
-*.vcproj
-*vcproj.*.*.user
-*.ncb
-*.sdf
-*.opensdf
-*.vcxproj
-*vcxproj.*
-
-# MinGW generated files
-*.Debug
-*.Release
-
-# Python byte code
-*.pyc
-
-# Binaries
-# --------
-*.dll
-*.exe
-

src1/detection/detection_radar_delphi_esr/detection_radar_delphi_esr.pro

@@ -1,35 +0,0 @@
-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
-
-SOURCES += \
-        ivdetection_radar_delphi_esr.cpp \
-        main.cpp
-
-# Default rules for deployment.
-qnx: target.path = /tmp/$${TARGET}/bin
-else: unix:!android: target.path = /opt/$${TARGET}/bin
-!isEmpty(target.path): INSTALLS += target
-
-!include(../../../include/ivprotobuf.pri ) {
-    error( "Couldn't find the ivprotobuf.pri file!" )
-}
-
-!include(../interface/ivdetection_radar.pri ) {
-    error( "Couldn't find the ivdetection_radar.pri file!" )
-}
-
-HEADERS += \
-    ivdetection_radar_delphi_esr.h

src1/detection/detection_radar_delphi_esr/ivdetection_radar_delphi_esr.cpp

@@ -1,94 +0,0 @@
-#include "ivdetection_radar_delphi_esr.h"
-
-#include <math.h>
-
-namespace iv {
-
-
-ivdetection_radar_delphi_esr::ivdetection_radar_delphi_esr(std::string strxmlpath)
-{
-    iv::xmlparam::Xmlparam xp(strxmlpath);
-
-    std::string strcanmsgname = xp.GetParam("canrecv","canrecv1");
-    std::string strradarmsgname = xp.GetParam("radar","radar");
-    setcanmsgname(strcanmsgname);
-    setradarmsgname(strradarmsgname);
-}
-
-int ivdetection_radar_delphi_esr::DecodeCANMsg(radar::radarobjectarray &xradar, can::canraw *prawmsg)
-{
-    int nrtn = 0;
-    int32_t range, rate, angle;
-    static int ncount = 0;
-    static qint64 nlastsend = 0;
-
-    ncount++;
-
-    iv::can::canraw canmsg;
-    canmsg.CopyFrom(*prawmsg);
-
-    if((canmsg.id() >= 0x500)&&(canmsg.id() <= 0x53f))
-    {
-        int  data[8];
-        unsigned char cdata[8];
-        memcpy(cdata,canmsg.data().data(),8);
-
-        int radar_ID_index = canmsg.id() - 0x500;
-
-        int j;
-        for(j=0;j<8;j++)data[j] = cdata[j];
-
-        angle = ((data[1] & 0x1F) << 5) + ((data[2] & 0xF8) / 8);
-        range = ((data[2] & 0x07) << 8) + data[3];
-        rate = ((data[6] & 0x3F) << 8) | data[7];
-        if (angle & 0x200) {
-            angle = angle | 0xFFFFFC00;
-        }
-        if (rate & 0x2000) {
-            rate = rate | 0xFFFFC000;
-        }
-        //           qDebug("range = %d angle = %d ",range,angle);
-        //If angle and range both are 0, it is an invalid data.
-        if (angle != 0 || range != 0) {
-            iv::radar::radarobject * pobj = xradar.add_obj();
-            pobj->set_bvalid(true);
-
-            pobj->set_x(range * 0.1 * sin(angle * 1.0 / 1800.0 * M_PI));
-            pobj->set_y(range * 0.1 * cos(angle * 1.0 / 1800.0 * M_PI));
-            pobj->set_vel(rate * 1.0 / 100.0);
-            pobj->set_vx(pobj->vel() * sin(angle / 1800.0 * M_PI));
-            pobj->set_vy(pobj->vel() * cos(angle / 1800.0 * M_PI));
-        }
-        else {
-            iv::radar::radarobject * pobj = xradar.add_obj();
-            pobj->set_bvalid(false);
-        }
-
-
-        if(canmsg.id() == 0x53f)
-        {
-            nrtn = 1;
-        }
-
-        if(ncount > 100)
-        {
-            nrtn = 1;
-        }
-
-        if((QDateTime::currentMSecsSinceEpoch() - nlastsend) > 100)
-        {
-            nrtn = 1;
-        }
-    }
-
-    if(nrtn == 1)
-    {
-        ncount = 0;
-        nlastsend = QDateTime::currentMSecsSinceEpoch();
-    }
-    //       qDebug("id is %08x",canmsg.id());
-
-    return nrtn;
-}
-
-}

src1/detection/detection_radar_delphi_esr/ivdetection_radar_delphi_esr.h

@@ -1,21 +0,0 @@
-#ifndef IVDETECTION_RADAR_DELPHI_ESR_H
-#define IVDETECTION_RADAR_DELPHI_ESR_H
-
-#include "ivdetection_radar.h"
-
-#include "xmlparam.h"
-
-namespace iv {
-
-class ivdetection_radar_delphi_esr : public ivdetection_radar
-{
-public:
-    ivdetection_radar_delphi_esr(std::string strxmlpath);
-
-public:
-    virtual int DecodeCANMsg(radar::radarobjectarray &xradar, can::canraw *prawmsg);
-};
-
-}
-
-#endif // IVDETECTION_RADAR_DELPHI_ESR_H

src1/detection/detection_radar_delphi_esr/main.cpp

@@ -1,23 +0,0 @@
-#include <QCoreApplication>
-
-#include <iostream>
-
-#include "ivdetection_radar_delphi_esr.h"
-
-int main(int argc, char *argv[])
-{
-    QCoreApplication a(argc, argv);
-
-    QString strpath = QCoreApplication::applicationDirPath();
-    if(argc < 2)
-        strpath = strpath + "/detection_radar_esr.xml";
-    else
-        strpath = argv[1];
-    std::cout<<strpath.toStdString()<<std::endl;
-
-    iv::ivmodule * pivmodule = new iv::ivdetection_radar_delphi_esr(strpath.toStdString());
-
-    pivmodule->start();
-
-    return a.exec();
-}

src1/detection/interface/ivdetection.cpp

@@ -1,10 +0,0 @@
-#include "ivdetection.h"
-
-namespace iv {
-
-ivdetection::ivdetection()
-{
-
-}
-
-}