reset src1 folder.

src1/common/modulecomm/.gitignore

@@ -0,0 +1,73 @@
+# 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

@@ -0,0 +1,152 @@
+#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

@@ -0,0 +1,64 @@
+#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

@@ -0,0 +1,36 @@
+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

@@ -0,0 +1,323 @@
+// 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

@@ -0,0 +1,253 @@
+// 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

@@ -0,0 +1,13 @@
+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

@@ -0,0 +1,138 @@
+// 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

@@ -0,0 +1,61 @@
+// 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

@@ -0,0 +1,198 @@
+// 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

@@ -0,0 +1,56 @@
+// 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

@@ -0,0 +1,206 @@
+// 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

@@ -0,0 +1,72 @@
+// 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

@@ -0,0 +1,13 @@
+#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

@@ -0,0 +1,63 @@
+#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

@@ -0,0 +1,40 @@
+#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

@@ -0,0 +1,45 @@
+#-------------------------------------------------
+#
+# 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

@@ -0,0 +1,140 @@
+#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

@@ -0,0 +1,41 @@
+#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

@@ -0,0 +1,73 @@
+# 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

@@ -0,0 +1,480 @@
+#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

@@ -0,0 +1,109 @@
+#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

@@ -0,0 +1,56 @@
+#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

@@ -0,0 +1,37 @@
+#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

@@ -0,0 +1,33 @@
+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

@@ -0,0 +1,90 @@
+# 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

@@ -0,0 +1,13 @@
+#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

@@ -0,0 +1,54 @@
+#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

@@ -0,0 +1,42 @@
+#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

@@ -0,0 +1,41 @@
+#-------------------------------------------------
+#
+# 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

@@ -0,0 +1,378 @@
+#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

@@ -0,0 +1,112 @@
+#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

@@ -0,0 +1,310 @@
+#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

@@ -0,0 +1,99 @@
+#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/build-decision_brain-Debug/.qmake.stash

@@ -0,0 +1,24 @@
+QMAKE_CXX.QT_COMPILER_STDCXX = 201402L
+QMAKE_CXX.QMAKE_GCC_MAJOR_VERSION = 7
+QMAKE_CXX.QMAKE_GCC_MINOR_VERSION = 5
+QMAKE_CXX.QMAKE_GCC_PATCH_VERSION = 0
+QMAKE_CXX.COMPILER_MACROS = \
+    QT_COMPILER_STDCXX \
+    QMAKE_GCC_MAJOR_VERSION \
+    QMAKE_GCC_MINOR_VERSION \
+    QMAKE_GCC_PATCH_VERSION
+QMAKE_CXX.INCDIRS = \
+    /usr/include/c++/7 \
+    /usr/include/x86_64-linux-gnu/c++/7 \
+    /usr/include/c++/7/backward \
+    /usr/lib/gcc/x86_64-linux-gnu/7/include \
+    /usr/local/include \
+    /usr/lib/gcc/x86_64-linux-gnu/7/include-fixed \
+    /usr/include/x86_64-linux-gnu \
+    /usr/include
+QMAKE_CXX.LIBDIRS = \
+    /usr/lib/gcc/x86_64-linux-gnu/7 \
+    /usr/lib/x86_64-linux-gnu \
+    /usr/lib \
+    /lib/x86_64-linux-gnu \
+    /lib

src1/decision/common/adc_adapter/base_adapter.h

@@ -0,0 +1,44 @@
+#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

@@ -0,0 +1,330 @@
+#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

@@ -0,0 +1,43 @@
+#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

@@ -0,0 +1,199 @@
+#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

@@ -0,0 +1,60 @@
+#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

@@ -0,0 +1,367 @@
+#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

@@ -0,0 +1,43 @@
+#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

@@ -0,0 +1,317 @@
+#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

@@ -0,0 +1,41 @@
+#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

@@ -0,0 +1,156 @@
+#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

@@ -0,0 +1,44 @@
+#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

@@ -0,0 +1,157 @@
+#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

@@ -0,0 +1,44 @@
+#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

@@ -0,0 +1,194 @@
+
+#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

@@ -0,0 +1,42 @@
+#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

@@ -0,0 +1,67 @@
+#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

@@ -0,0 +1,352 @@
+#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

@@ -0,0 +1,76 @@
+#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

@@ -0,0 +1,40 @@
+#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

@@ -0,0 +1,48 @@
+#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

@@ -0,0 +1,58 @@
+#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

@@ -0,0 +1,44 @@
+#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

@@ -0,0 +1,644 @@
+#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

@@ -0,0 +1,161 @@
+#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

@@ -0,0 +1,196 @@
+#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

@@ -0,0 +1,38 @@
+#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

@@ -0,0 +1,2102 @@
+#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

@@ -0,0 +1,105 @@
+#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

@@ -0,0 +1,439 @@
+/*
+ * 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

@@ -0,0 +1,149 @@
+#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

@@ -0,0 +1,45 @@
+#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

@@ -0,0 +1,26 @@
+#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

@@ -0,0 +1,109 @@
+#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

@@ -0,0 +1,138 @@
+#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

@@ -0,0 +1,27 @@
+#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

@@ -0,0 +1,24 @@
+#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

@@ -0,0 +1,199 @@
+#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

@@ -0,0 +1,22 @@
+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

@@ -0,0 +1,43 @@
+#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

@@ -0,0 +1,37 @@
+#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

@@ -0,0 +1,18 @@
+#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

@@ -0,0 +1,16 @@
+#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

@@ -0,0 +1,136 @@
+/**
+* 用于输出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

@@ -0,0 +1,169 @@
+#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

@@ -0,0 +1,18 @@
+#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

@@ -0,0 +1,25 @@
+#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

@@ -0,0 +1,40 @@
+#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

@@ -0,0 +1,19 @@
+#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

@@ -0,0 +1,41 @@
+#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

@@ -0,0 +1,532 @@
+#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

@@ -0,0 +1,112 @@
+#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

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

src1/decision/decision_brain/.gitignore

@@ -0,0 +1,73 @@
+# 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/ivdecision_brain.cpp

@@ -43,20 +43,172 @@ int ivdecision_brain::getdecision(brain::decition &xdecition)
         mbisFirstRun = true;
     }
 
-    if(mbisFirstRun)
+    iv::LidarGridPtr lidarptr;
+    GetLIDARGrid(lidarptr);
+
+    std::vector<iv::Perception::PerceptionOutput> xvectorper;
+
+    iv::TrafficLight xtrafficlight;
+    std::shared_ptr<iv::vbox::vbox> xvbox_ptr;
+    if(Getvboxmsg(xvbox_ptr))
+    {
+        xtrafficlight.leftColor=xvbox_ptr->st_left();
+        xtrafficlight.rightColor=xvbox_ptr->st_right();
+        xtrafficlight.straightColor=xvbox_ptr->st_straight();
+        xtrafficlight.uturnColor=xvbox_ptr->st_turn();
+        xtrafficlight.leftTime=xvbox_ptr->time_left();
+        xtrafficlight.rightTime=xvbox_ptr->time_right();
+        xtrafficlight.straightTime=xvbox_ptr->time_straight();
+        xtrafficlight.uturnTime=xvbox_ptr->time_turn();
+    }
+
+    updatev2x();
+    updateultra();
+    updateradar();
+
+    iv::decition::Decition decition= getDecideFromGPS(*now_gps_ins,mgpsMapLine,lidarptr,xvectorper,xtrafficlight);
+
+    xdecition.set_accelerator(decition->accelerator);
+    xdecition.set_brake(decition->brake);
+    xdecition.set_leftlamp(decition->leftlamp);
+    xdecition.set_rightlamp(decition->rightlamp);
+    xdecition.set_speed(decition->speed);
+    xdecition.set_wheelangle(decition->wheel_angle);
+    xdecition.set_wheelspeed(decition->angSpeed);
+    xdecition.set_torque(decition->torque);
+    xdecition.set_mode(decition->mode);
+    xdecition.set_gear(decition->dangWei);
+    xdecition.set_handbrake(decition->handBrake);
+    xdecition.set_grade(decition->grade);
+    xdecition.set_engine(decition->engine);
+    xdecition.set_brakelamp(decition->brakeLight);
+    xdecition.set_ttc(ServiceCarStatus.mfttc);
+    xdecition.set_air_enable(decition->air_enable);
+    xdecition.set_air_temp(decition->air_temp);
+    xdecition.set_air_mode(decition->air_mode);
+    xdecition.set_wind_level(decition->wind_level);
+    xdecition.set_roof_light(decition->roof_light);
+    xdecition.set_home_light(decition->home_light);
+    xdecition.set_air_worktime(decition->air_worktime);
+    xdecition.set_air_offtime(decition->air_offtime);
+    xdecition.set_air_on(decition->air_on);
+    xdecition.set_door(decition->door);
+
+    return 0;
+}
+
+void ivdecision_brain::updatev2x()
+{
+    std::shared_ptr<iv::v2x::v2x> xv2x_ptr;
+
+    if(false == Getv2xmsg(xv2x_ptr))
     {
-        //Init
+        return;
     }
 
-    switch (mvehState) {
-    case VehState::normalRun:
-//        return getdecision_normal(xdecition);
-        break;
-    default:
-        break;
+    ServiceCarStatus.stationCmd.received=true;
+
+    ServiceCarStatus.stationCmd.has_carID=xv2x_ptr->has_carid();
+    if(ServiceCarStatus.stationCmd.has_carID)
+    {
+            ServiceCarStatus.stationCmd.carID=xv2x_ptr->carid();
+    }
+
+    ServiceCarStatus.stationCmd.has_carMode=xv2x_ptr->has_carmode();
+    if(ServiceCarStatus.stationCmd.has_carMode)
+    {
+            ServiceCarStatus.stationCmd.carMode=xv2x_ptr->carmode();
+
+            qDebug("ServiceCarStatus.stationCmd.carMode:",ServiceCarStatus.stationCmd.carMode);
+            givlog->debug("brain_v2x","ServiceCarStatus.stationCmd.carMode:%d",ServiceCarStatus.stationCmd.carMode);
+    }
+
+    ServiceCarStatus.stationCmd.has_emergencyStop=xv2x_ptr->has_emergencystop();
+    if(ServiceCarStatus.stationCmd.has_emergencyStop)
+    {
+            ServiceCarStatus.stationCmd.emergencyStop=xv2x_ptr->emergencystop();
+
+            qDebug("ServiceCarStatus.stationCmd.emergencyStop:",ServiceCarStatus.stationCmd.emergencyStop);
+            givlog->debug("brain_v2x","ServiceCarStatus.stationCmd.emergencyStop:%d",ServiceCarStatus.stationCmd.emergencyStop);
+    }
+
+    ServiceCarStatus.stationCmd.has_stationStop=xv2x_ptr->has_stationstop();
+    if(ServiceCarStatus.stationCmd.has_stationStop)
+    {
+            ServiceCarStatus.stationCmd.stationStop=xv2x_ptr->stationstop();
+
+            qDebug("ServiceCarStatus.stationCmd.stationStop:",ServiceCarStatus.stationCmd.stationStop);
+            givlog->debug("brain_v2x","ServiceCarStatus.stationCmd.stationStop:%d",ServiceCarStatus.stationCmd.stationStop);
+    }
+    int num=xv2x_ptr->stationid_size();
+    if(num>0)
+    {
+        ServiceCarStatus.stationCmd.stationTotalNum=num;
+        for(int i=0;i<ServiceCarStatus.stationCmd.stationTotalNum;i++)
+        {
+            ServiceCarStatus.stationCmd.stationGps[i].gps_lat=xv2x_ptr->stgps(i).lat();
+            ServiceCarStatus.stationCmd.stationGps[i].gps_lng=xv2x_ptr->stgps(i).lon();
+
+            qDebug("stationGps: %d, lat: %.7f, lon: %.7f", xv2x_ptr->stationid(i), ServiceCarStatus.stationCmd.stationGps[i].gps_lat, ServiceCarStatus.stationCmd.stationGps[i].gps_lng);
+            givlog->debug("brain_v2x","stationGps: %d, lat: %.7f, lon: %.7f",
+                           xv2x_ptr->stationid(i),ServiceCarStatus.stationCmd.stationGps[i].gps_lat,
+                          ServiceCarStatus.stationCmd.stationGps[i].gps_lng);
+        }
+        ServiceCarStatus.mbRunPause=false;
+    }
+
+}
+
+void ivdecision_brain::updateultra()
+{
+    std::shared_ptr<iv::ultrasonic::ultrasonic> xultra_ptr;
+
+    if(false == Getultrasonic(xultra_ptr))
+    {
+        return;
+    }
+
+    ServiceCarStatus.ultraDistance[1]=xultra_ptr->sigobjdist_flcorner();
+    ServiceCarStatus.ultraDistance[2]=xultra_ptr->sigobjdist_flmiddle();
+    ServiceCarStatus.ultraDistance[3]=xultra_ptr->sigobjdist_flside();
+    ServiceCarStatus.ultraDistance[4]=xultra_ptr->sigobjdist_frcorner();
+    ServiceCarStatus.ultraDistance[5]=xultra_ptr->sigobjdist_frmiddle();
+    ServiceCarStatus.ultraDistance[6]=xultra_ptr->sigobjdist_frside();
+    ServiceCarStatus.ultraDistance[7]=xultra_ptr->sigobjdist_rlcorner();
+    ServiceCarStatus.ultraDistance[8]=xultra_ptr->sigobjdist_rlmiddle();
+    ServiceCarStatus.ultraDistance[9]=xultra_ptr->sigobjdist_rlside();
+    ServiceCarStatus.ultraDistance[10]=xultra_ptr->sigobjdist_rrcorner();
+    ServiceCarStatus.ultraDistance[11]=xultra_ptr->sigobjdist_rrmiddle();
+    ServiceCarStatus.ultraDistance[12]=xultra_ptr->sigobjdist_rrside();
+}
+
+void ivdecision_brain::updateradar()
+{
+    std::shared_ptr<iv::radar::radarobjectarray> xradar_ptr
+            = std::shared_ptr<iv::radar::radarobjectarray>(new iv::radar::radarobjectarray);
+
+    unsigned int i;
+    if(false == GetRADAR(xradar_ptr))
+    {
+        for(i=0;i<64;i++)
+        {
+            ServiceCarStatus.obs_radar[i].valid = false;
+        }
+        return;
+    }
+
+    for(i=0;i<64;i++)
+    {
+        iv::radar::radarobject * pradarobj = xradar_ptr->mutable_obj(i);
+        ServiceCarStatus.obs_radar[i].valid = pradarobj->bvalid();
+        ServiceCarStatus.obs_radar[i].nomal_x = pradarobj->x();
+        ServiceCarStatus.obs_radar[i].nomal_y = pradarobj->y();
+        ServiceCarStatus.obs_radar[i].speed_relative = pradarobj->vel();
+        ServiceCarStatus.obs_radar[i].speed_x = pradarobj->vx();
+        ServiceCarStatus.obs_radar[i].speed_y = pradarobj->vy();
     }
 
-    return -1000;//mode error.
+
 }
 
 

src1/decision/decision_brain/ivdecision_brain.h

@@ -61,6 +61,11 @@ private:
                                                                        iv::LidarGridPtr lidarGridPtr,
                                             std::vector<iv::Perception::PerceptionOutput> lidar_per,
                                                                        iv::TrafficLight trafficLight);
+
+private:
+    void updatev2x();
+    void updateultra();
+    void updateradar();
 private:
     int mnRunMode = 0;
     bool mbisFirstRun = true;

src1/decision/decision_brain/main.cpp

@@ -0,0 +1,14 @@
+#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

@@ -0,0 +1,61 @@
+#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

@@ -0,0 +1,464 @@
+#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();
+    xhmimsg = mHMImsg;
+    mMutexHMI.unlock();
+    return true;
+}
+
+void ivdecision::UpdateVbox(const char *strdata, const unsigned int nSize, const unsigned int index, const QDateTime *dt, const char *strmemname)
+{
+    (void )&index;
+    (void )&dt;
+    (void )strmemname;
+
+    std::shared_ptr<iv::vbox::vbox> vbox_ptr = std::shared_ptr<iv::vbox::vbox>(new iv::vbox::vbox);
+    if(!vbox_ptr->ParseFromArray(strdata,nSize))
+    {
+        std::cout<<"vdecision::UpdateVbox Parse Error"<<std::endl;
+        return;
+    }
+    mMutexvbox.lock();
+    mvboxmsg = vbox_ptr;
+    mnvboxUpdateTime = QDateTime::currentMSecsSinceEpoch();
+    mMutexvbox.unlock();
+}
+
+bool ivdecision::Getvboxmsg(std::shared_ptr<vbox::vbox> &xvboxmsg)
+{
+    if((QDateTime::currentMSecsSinceEpoch() - mnvboxUpdateTime)>mnNewThresh)
+    {
+        xvboxmsg = NULL;
+        return false;
+    }
+    mMutexvbox.lock();
+    xvboxmsg = mvboxmsg;
+    mMutexvbox.unlock();
+    return true;
+}
+
+void ivdecision::Updatev2x(const char *strdata, const unsigned int nSize, const unsigned int index, const QDateTime *dt, const char *strmemname)
+{
+    (void )&index;
+    (void )&dt;
+    (void )strmemname;
+
+    std::shared_ptr<iv::v2x::v2x> xv2x_ptr = std::shared_ptr<iv::v2x::v2x>(new iv::v2x::v2x);
+    if(!xv2x_ptr->ParseFromArray(strdata,nSize))
+    {
+        std::cout<<"ivdecision::Updatev2x Parse Error."<<std::endl;
+        return;
+    }
+
+    mMutexv2x.lock();
+    mv2xmsg = xv2x_ptr;
+    mnv2xUpdateTime = QDateTime::currentMSecsSinceEpoch();
+    mMutexv2x.unlock();
+}
+
+bool ivdecision::Getv2xmsg(std::shared_ptr<v2x::v2x> &xv2xmsg)
+{
+    if((QDateTime::currentMSecsSinceEpoch() - mnv2xUpdateTime)>mnNewThresh)
+    {
+        xv2xmsg = NULL;
+        return false;
+    }
+    mMutexv2x.lock();
+    xv2xmsg = mv2xmsg;
+    mMutexv2x.unlock();
+    return true;
+}
+
+void ivdecision::Updateultrasonic(const char *strdata, const unsigned int nSize, const unsigned int index, const QDateTime *dt, const char *strmemname)
+{
+    (void )&index;
+    (void )&dt;
+    (void )strmemname;
+
+    std::shared_ptr<iv::ultrasonic::ultrasonic> xultrasonic_ptr
+            = std::shared_ptr<iv::ultrasonic::ultrasonic>(new iv::ultrasonic::ultrasonic);
+
+    if(!xultrasonic_ptr->ParseFromArray(strdata,nSize))
+    {
+        std::cout<<"ivdecision::Updateultrasonic Parse Error."<<std::endl;
+        return;
+    }
+
+    mMutexultrasonic.lock();
+    multrasonicmsg = xultrasonic_ptr;
+    mnultrasonicUpdateTime = QDateTime::currentMSecsSinceEpoch();
+    mMutexultrasonic.unlock();
+}
+
+bool ivdecision::Getultrasonic(std::shared_ptr<ultrasonic::ultrasonic> &xultramsg)
+{
+    if((QDateTime::currentMSecsSinceEpoch() - mnultrasonicUpdateTime)>mnNewThresh)
+    {
+        xultramsg = NULL;
+        return false;
+    }
+    mMutexultrasonic.lock();
+    xultramsg = multrasonicmsg;
+    mMutexultrasonic.unlock();
+    return true;
+}
+
+}

src1/decision/interface/ivdecision.h

@@ -0,0 +1,133 @@
+#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 "vbox.pb.h"
+#include "v2x.pb.h"
+#include "ultrasonic.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);
+    void UpdateVbox(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname);
+    void Updatev2x(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname);
+    void Updateultrasonic(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;
+
+    std::shared_ptr<iv::vbox::vbox> mvboxmsg;
+    qint64 mnvboxUpdateTime = 0;
+    QMutex mMutexvbox;
+
+    std::shared_ptr<iv::v2x::v2x> mv2xmsg;
+    qint64 mnv2xUpdateTime = 0;
+    QMutex mMutexv2x;
+
+    std::shared_ptr<iv::ultrasonic::ultrasonic> multrasonicmsg;
+    qint64 mnultrasonicUpdateTime = 0;
+    QMutex mMutexultrasonic;
+
+private:
+    const qint64 mnNewThresh = 1000; //1 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);
+    bool Getvboxmsg(std::shared_ptr<iv::vbox::vbox> & xvboxmsg);
+    bool Getv2xmsg(std::shared_ptr<iv::v2x::v2x> & xv2xmsg);
+    bool Getultrasonic(std::shared_ptr<iv::ultrasonic::ultrasonic> & xultramsg);
+
+//    void GetLidarPtr();
+//    void GetRadar();
+//    void GetMap();
+//    bool IsMapUpdate();
+//    void GetMobileye();
+
+};
+
+}
+
+#endif // IVDECISION_H

src1/detection/detection_radar_delphi_esr/.gitignore

@@ -0,0 +1,73 @@
+# 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

@@ -0,0 +1,35 @@
+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

@@ -0,0 +1,94 @@
+#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

@@ -0,0 +1,21 @@
+#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

@@ -0,0 +1,23 @@
+#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

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