add gps_speed_limit

.gitignore

@@ -1,9 +1,6 @@
 # ---> Qt
 # C++ objects and libs
 
-build_partial.sh
-deploy.sh
-
 *.slo
 *.lo
 *.o

autogen.sh

@@ -1,7 +1,12 @@
 
-#qtmake="/opt/Qt5.10.1/5.10.1/gcc_64/bin/qmake"
-#qtmake=/usr/bin/qmake
+#qt_com=$(arch)
+qt_com=`arch`
+if [ qt_com = aarch64 ];then
 qtmake="/usr/lib/aarch64-linux-gnu/qt5/bin/qmake"
+else
+qtmake="/opt/Qt5.10.1/5.10.1/gcc_64/bin/qmake"
+fi
+
 if [ ! $qtmake ];then
 	echo -e "\e[33m qtmake not set, auto find it\e[0m"
 	qtmake=`find /opt -name "qmake" 2>/dev/null | grep 'gcc_64'`

autogen_lib.sh

@@ -1,7 +1,11 @@
 
-#qtmake="/opt/Qt5.10.1/5.10.1/gcc_64/bin/qmake"
-#qtmake=/usr/bin/qmake
+#qt_com=$(arch)
+qt_com=`arch`
+if [qt_com = aarch64 ];then
 qtmake="/usr/lib/aarch64-linux-gnu/qt5/bin/qmake"
+else
+qtmake="/opt/Qt5.10.1/5.10.1/gcc_64/bin/qmake"
+fi
 MAKEOPT=-j8
 
 mkdir bin

build_partial.sh

@@ -0,0 +1,70 @@
+#!/bin/bash
+
+#qt_com=$(arch)
+qt_com=`arch`
+if [ qt_com = aarch64 ];then
+qtmake="/usr/lib/aarch64-linux-gnu/qt5/bin/qmake"
+else
+qtmake="/opt/Qt5.10.1/5.10.1/gcc_64/bin/qmake"
+fi
+
+check_result()
+{
+	 
+	if [ "$1" != 0 ];then
+    	echo -e "\e[33m*************************************************\e[0m"
+    	echo -e "\e[31m    Please modify build error first,Exit!\e[0m"
+    	echo -e "\e[33m*************************************************\e[0m"
+ 	   	exit 1
+    fi
+
+}
+
+module=$1
+echo -e "\e[33m module name: \e[1;33m $module \e[0m"
+
+projectDir=`pwd`
+echo -e "project folder \e[33m$projectDir\e[0m"
+
+MAKEOPT=-j8
+
+mkdir bin
+outputDir=$projectDir"/bin"
+echo -e "output folder \e[33m$outputDir\e[0m"
+
+cd src
+sourceDir=`pwd`
+echo -e "source code folder \e[33m$sourceDir\e[0m"
+
+moduleDir=`find . -type d -name $module`
+if [ $moduleDir ];then
+	moduleDir=$sourceDir${moduleDir#*.}
+	echo -e "module code folder \e[33m$moduleDir\e[0m"
+	cd $moduleDir
+	moduleName=$module".pro"
+	echo -e "\e[1;35;47m START BUILD\e[0m"
+	$qtmake $moduleName
+	make $MAKEOPT
+	check_result $? 
+	make clean
+	cp $module $outputDir
+	rm Makefile
+	rm .qmake.stash
+	rm $module
+	cd $projectDir
+	echo -e "\e[1;33;44m Build $module Success \e[0m"
+	sleep 1
+
+	echo -e "\e[35m Start deploy\e[0m"
+	cp $outputDir"/"$module ./
+	bash deploy.sh $module
+	if [ "$?" == 1 ];then
+		exit 1
+	fi
+	rm $module
+	echo -e "\e[1;33;44m Deploy $module Success \e[0m"
+
+else
+	echo -e "\e[31m Can't find module code, exit.....\e[0m"
+	exit
+fi

deploy.sh

@@ -0,0 +1,100 @@
+#! /bin/bash
+
+
+#qt_com=$(arch)
+qt_com=`arch`
+if [qt_com = aarch64 ];then
+Qtgccdir='/usr/lib/aarch64-linux-gnu/qt5'
+QtPlatformdir=$Qtgccdir/plugins/platforms
+QtLibDir=/usr/lib/aarch64-linux-gnu/
+else
+Qtgccdir='/opt/Qt5.10.1/5.10.1/gcc_64'
+QtPlatformdir=$Qtgccdir/plugins/platforms
+QtLibDir=$Qtgccdir/lib
+fi
+
+
+ignore_lib_name=(
+libstdc++.so.*
+libm.so.*
+libgcc_s.so.*
+libc.so.*
+libpthread.so.*
+libGL.so.*
+libz.so.*
+libgthread*
+libglib*
+libexpat*
+libxcb*
+libdl.so.*
+libxshmfence*
+libglapi.so.*
+libXext.so.*
+libXdamage.so.*
+libXfixes.so.*
+libX11*
+libXxf86vm.so.*
+libdrm.so.*
+libpcre.so.*
+libXau.so.*
+libXdmcp.so.*
+)
+
+EXE="$1"
+PWD=`pwd`
+rm -rf app
+mkdir app
+cd app
+mkdir lib
+cd ..
+mkdir commonlib
+cd commonlib
+mkdir platforms
+cp $QtPlatformdir/libqxcb.so platforms
+cd platforms
+mkdir lib
+libfiles=`ldd libqxcb.so | awk '{ if(match($3,"^/"))printf("%s "),$3 }'` 
+cp $libfiles $PWD/lib 
+cd ..
+mkdir lib
+cp $QtLibDir/libQt5DBus.* $PWD/lib
+cp $QtLibDir/libQt5XcbQpa.* $PWD/lib
+rm -rf $PWD/platforms/lib
+cd platforms
+
+cd ..
+cd ..
+
+files=`ldd $EXE | awk '{ if(match($3,"^/"))printf("%s "),$3 }'`
+cp $files $PWD/app/lib
+cp $PWD/commonlib/lib/* $PWD/app/lib
+cp -r  $PWD/commonlib/platforms $PWD/app
+cp $EXE $PWD/app
+
+for x in ${ignore_lib_name[@]}
+do
+rm -f $PWD/app/lib/${x}
+done
+
+rm -rf commonlib
+
+cd app
+patchelf --set-rpath '$ORIGIN/lib/' $EXE
+if [ "$?" != 0 ];then
+	echo -e "\e[31m deploy.sh: patchelf $EXE faile, Ensure patchelf tool installed\e[0m"
+	exit 1
+fi
+cd platforms
+patchelf --set-rpath '$ORIGIN/../lib/' libqxcb.so
+if [ "$?" != 0 ];then
+	echo -e "\e[31m deploy.sh: patchelf $EXE faile, Ensure patchelf tool installed\e[0m"
+#	exit 1
+fi
+cd ..
+cd ..
+
+cp -r app $PWD/deploy/
+
+rm -rf app
+
+

include/alog.h

@@ -0,0 +1,134 @@
+#ifndef ALOG_H
+#define ALOG_H
+
+#include <stdarg.h>
+#include <string.h>
+#include <fstream>
+#include <string>
+
+#ifndef __FILENAME__
+
+#ifdef _WIN32
+   #define __FILENAME__ (strrchr(__FILE__, '\\') ? strrchr(__FILE__, '\\') + 1 : __FILE__)
+#else
+   #define __FILENAME__ (strrchr(__FILE__, '/') ? strrchr(__FILE__, '/') + 1 : __FILE__)
+#endif
+
+#endif
+
+// Global Logger class
+namespace iv {
+
+class Logger
+{
+private:
+        Logger()
+        {
+            mbFileOpen = false;
+            mbCallBack = false;
+        }
+        ~Logger()
+        {
+
+        }
+public:
+
+        static Logger & Inst()
+        {
+            static iv::Logger instance_;
+            return instance_;
+        }
+        void Log(char const* file, char const* func, int line, char const* format, ...)
+        {
+            static char complete_entry[2048];
+            static char message[1024];
+
+            va_list args;
+            va_start(args, format);
+            vsnprintf(message, 1024, format, args);
+
+    #ifndef DEBUG_NOTRACE
+            snprintf(complete_entry, 2048, "%s |  %s() | %d: %s", file, func,line, message);
+
+    #else
+            strncpy(complete_entry, message, 1024);
+    #endif
+
+            va_end(args);
+            if((!mbFileOpen) &&(!mbCallBack))
+                printf("%s\n", complete_entry);
+            else
+            {
+                if(mbFileOpen)
+                {
+                    file_ << complete_entry << std::endl;
+                    file_.flush();
+                }
+                if(mbCallBack)
+                {
+                    callback_(complete_entry);
+                }
+            }
+        }
+
+        void setlogfile(std::string strfilepath)
+        {
+            if (file_.is_open())
+            {
+                    // Close any open logfile, perhaps user want a new with unique filename
+                    file_.close();
+            }
+
+            file_.open(strfilepath.c_str());
+            if (file_.fail())
+            {
+                mbFileOpen = false;
+                    printf("Cannot open log file: %s \n",
+                            strfilepath.c_str());
+            }
+            else
+            {
+                mbFileOpen = true;
+            }
+        }
+
+        typedef void(*FuncPtr)(const char*);
+
+        void SetCallback(FuncPtr callback)
+        {
+            callback_ = callback;
+            if(callback_ != 0)mbCallBack = true;
+            else mbCallBack = false;
+        }
+        bool IsCallbackSet()
+        {
+            return callback_ != 0;
+        }
+        bool IsFileOpen() { return file_.is_open(); }
+
+private:
+
+        FuncPtr callback_ = 0;
+        std::ofstream file_;
+        bool mbFileOpen;
+        bool mbCallBack;
+
+};
+
+}
+
+
+#ifndef  NOTUSEALOG
+
+#define ALOG(Format_, ...)  iv::Logger::Inst().Log(__FILENAME__, __FUNCTION__, __LINE__, Format_, ##__VA_ARGS__)
+
+#else
+
+#define ALOG(Format_, ...)
+
+#endif
+
+
+
+
+#endif 

src/common/common/xodr/OpenDrive/ObjectSignal.cpp

@@ -0,0 +1,514 @@
+#include "ObjectSignal.h"
+
+#include <iostream>
+
+signal_positionRoad::signal_positionRoad(double s, double t, double zOffset, double hOffset, double pitch, double roll)
+{
+    ms = s;
+    mt = t;
+    mzOffset = zOffset;
+    mhOffset = hOffset;
+    mpitch = pitch;
+    mroll = roll;
+}
+
+double signal_positionRoad::Gets()
+{
+    return ms;
+}
+
+double signal_positionRoad::Gett()
+{
+    return mt;
+}
+
+double signal_positionRoad::GetzOffset()
+{
+    return mzOffset;
+}
+
+double signal_positionRoad::GethOffset()
+{
+    return mhOffset;
+}
+
+double signal_positionRoad::Getpitch()
+{
+    return mpitch;
+}
+
+double signal_positionRoad::Getroll()
+{
+    return mroll;
+}
+
+void signal_positionRoad::Sets(double s)
+{
+    ms = s;
+}
+
+void signal_positionRoad::Sett(double t)
+{
+    mt = t;
+}
+
+void signal_positionRoad::SetzOffset(double zOffset)
+{
+    mzOffset = zOffset;
+}
+
+void signal_positionRoad::SethOffset(double hOffset)
+{
+    mhOffset = hOffset;
+}
+
+void signal_positionRoad::Setpitch(double pitch)
+{
+    mpitch = pitch;
+}
+
+void signal_positionRoad::Setroll(double roll)
+{
+    mroll = roll;
+}
+
+signal_positionInertial::signal_positionInertial(double x, double y,double z, double hdg, double pitch, double roll)
+{
+    mx = x;
+    my = y;
+    mz = z;
+    mhdg = hdg;
+    mpitch = pitch;
+    mroll = roll;
+}
+
+double signal_positionInertial::Getx()
+{
+    return mx;
+}
+
+double signal_positionInertial::Gety()
+{
+    return my;
+}
+
+double signal_positionInertial::Getz()
+{
+    return mz;
+}
+
+double signal_positionInertial::Gethdg()
+{
+    return mhdg;
+}
+
+double signal_positionInertial::Getpitch()
+{
+    return mpitch;
+}
+
+double signal_positionInertial::Getroll()
+{
+    return mroll;
+}
+
+void signal_positionInertial::Setx(double x)
+{
+    mx = x;
+}
+
+void signal_positionInertial::Sety(double y)
+{
+    my = y;
+}
+
+void signal_positionInertial::Setz(double z)
+{
+    mz = z;
+}
+
+void signal_positionInertial::Sethdg(double hdg)
+{
+    mhdg = hdg;
+}
+
+void signal_positionInertial::Setpitch(double pitch)
+{
+    mpitch = pitch;
+}
+
+void signal_positionInertial::Setroll(double roll)
+{
+    mroll = roll;
+}
+
+
+signal_laneValidity::signal_laneValidity(int fromLane,int toLane)
+{
+    mfromLane = fromLane;
+    mtoLane = toLane;
+}
+
+int signal_laneValidity::GetfromLane()
+{
+    return mfromLane;
+}
+
+int signal_laneValidity::GettoLane()
+{
+    return mtoLane;
+}
+
+void signal_laneValidity::SetfromLane(int fromLane)
+{
+    mfromLane = fromLane;
+}
+
+void signal_laneValidity::SettoLane(int toLane)
+{
+    mtoLane = toLane;
+}
+
+
+Signal::Signal(double s, double t, std::string id, std::string name, bool dynamic,string orientation,
+               double zOffset, string type, std::string country, std::string countryRevision,
+               string subtype, double hOffset, double pitch, double roll, double height, double width)
+{
+    ms = s;
+    mt = t;
+    mid = id;
+    mname = name;
+    mdynamic = dynamic;
+    morientation = orientation;
+    mzOffset = zOffset;
+    mtype = type;
+    mcountry = country;
+    mcountryRevision = countryRevision;
+    msubtype = subtype;
+    mhOffset = hOffset;
+    mpitch = pitch;
+    mroll = roll;
+    mheight = height;
+    mwidth = width;
+    mpsignal_laneValidity = 0;
+    mpsignal_positionInertial = 0;
+    mpsignal_positionRoad = new signal_positionRoad(s,t,zOffset,hOffset,pitch,roll);
+}
+
+Signal::Signal()
+{
+    mpsignal_positionInertial = 0;
+    mpsignal_positionRoad = 0;
+    mpsignal_laneValidity = 0;
+
+}
+Signal::~Signal()
+{
+    if(mpsignal_laneValidity != 0)delete mpsignal_laneValidity;
+    if(mpsignal_positionInertial != 0)delete mpsignal_positionInertial;
+    if(mpsignal_positionRoad != 0)delete mpsignal_positionRoad;
+}
+
+Signal& Signal::operator=(const Signal& x)
+{
+    if (this != &x)
+    {
+        this->ms = x.ms;
+        this->mt = x.mt;
+        this->mid = x.mid;
+        this->mname = x.mname;
+        this->mdynamic = x.mdynamic;
+        this->morientation = x.morientation;
+        this->mzOffset = x.mzOffset;
+        this->mtype = x.mtype;
+        this->mcountry = x.mcountry;
+        this->mcountryRevision = x.mcountryRevision;
+        this->msubtype = x.msubtype;
+        this->mhOffset = x.mhOffset;
+        this->mpitch = x.mpitch;
+        this->mroll = x.mroll;
+        this->mheight = x.mheight;
+        this->mwidth = x.mwidth;
+        this->mpsignal_positionInertial = 0;
+        if(x.mpsignal_positionInertial != 0)
+        {
+            this->mpsignal_positionInertial = new signal_positionInertial(x.mpsignal_positionInertial->Getx(),
+                                                                      x.mpsignal_positionInertial->Gety(),
+                                                                      x.mpsignal_positionInertial->Getz(),
+                                                                      x.mpsignal_positionInertial->Gethdg(),
+                                                                      x.mpsignal_positionInertial->Getpitch(),
+                                                                      x.mpsignal_positionInertial->Getroll());
+        }
+        this->mpsignal_laneValidity = 0;
+        if(x.mpsignal_laneValidity != 0)
+        {
+            this->mpsignal_laneValidity = new signal_laneValidity(x.mpsignal_laneValidity->GetfromLane(),
+                                                              x.mpsignal_laneValidity->GettoLane());
+        }
+        this->mpsignal_positionRoad = new signal_positionRoad(ms,mt,mzOffset,mhOffset,mpitch,mroll);
+    }
+    return *this;
+}
+
+Signal::Signal(const Signal &x)
+{
+    ms = x.ms;
+    mt = x.mt;
+    mid = x.mid;
+    mname = x.mname;
+    mdynamic = x.mdynamic;
+    morientation = x.morientation;
+    mzOffset = x.mzOffset;
+    mtype = x.mtype;
+    mcountry = x.mcountry;
+    mcountryRevision = x.mcountryRevision;
+    msubtype = x.msubtype;
+    mhOffset = x.mhOffset;
+    mpitch = x.mpitch;
+    mroll = x.mroll;
+    mheight = x.mheight;
+    mwidth = x.mwidth;
+    this->mpsignal_positionInertial = 0;
+    if(x.mpsignal_positionInertial != 0)
+    {
+        this->mpsignal_positionInertial = new signal_positionInertial(x.mpsignal_positionInertial->Getx(),
+                                                                  x.mpsignal_positionInertial->Gety(),
+                                                                  x.mpsignal_positionInertial->Getz(),
+                                                                  x.mpsignal_positionInertial->Gethdg(),
+                                                                  x.mpsignal_positionInertial->Getpitch(),
+                                                                  x.mpsignal_positionInertial->Getroll());
+    }
+    this->mpsignal_laneValidity = 0;
+    if(x.mpsignal_laneValidity != 0)
+    {
+        this->mpsignal_laneValidity = new signal_laneValidity(x.mpsignal_laneValidity->GetfromLane(),
+                                                          x.mpsignal_laneValidity->GettoLane());
+    }
+    mpsignal_positionRoad = new signal_positionRoad(ms,mt,mzOffset,mhOffset,mpitch,mroll);
+}
+
+double Signal::Gets()
+{
+    return ms;
+}
+
+double Signal::Gett()
+{
+    return mt;
+}
+
+string Signal::Getid()
+{
+    return mid;
+}
+
+string Signal::Getname()
+{
+    return mname;
+}
+
+bool Signal::Getdynamic()
+{
+    return mdynamic;
+}
+
+string Signal::Getorientation()
+{
+    return morientation;
+}
+
+double Signal::GetzOffset()
+{
+    return mzOffset;
+}
+
+string Signal::Gettype()
+{
+    return mtype;
+}
+
+string Signal::Getcountry()
+{
+    return mcountry;
+}
+
+string Signal::GetcountryRevision()
+{
+    return mcountryRevision;
+}
+
+string Signal::Getsubtype()
+{
+    return msubtype;
+}
+
+double Signal::GethOffset()
+{
+    return mhOffset;
+}
+
+double Signal::Getpitch()
+{
+    return mpitch;
+}
+
+double Signal::Getroll()
+{
+    return mroll;
+}
+
+double Signal::Getheight()
+{
+    return mheight;
+}
+
+double Signal::Getwidth()
+{
+    return mwidth;
+}
+
+signal_positionRoad * Signal::GetpositionRoad()
+{
+    return mpsignal_positionRoad;
+}
+
+signal_positionInertial * Signal::GetpositionInertial()
+{
+    return mpsignal_positionInertial;
+}
+
+void Signal::Sets(double s)
+{
+    ms = s;
+}
+
+void Signal::Sett(double t)
+{
+    mt = t;
+}
+
+void Signal::Setid(std::string id)
+{
+    mid = id;
+}
+
+void Signal::Setname(std::string name)
+{
+    mname = name;
+}
+
+void Signal::Setdynamic(bool dynamic)
+{
+    mdynamic = dynamic;
+}
+
+void Signal::Setorientation(std::string orientation)
+{
+    morientation = orientation;
+}
+
+void Signal::SetzOffset(double zOffset)
+{
+    mzOffset = zOffset;
+}
+
+void Signal::Settype(string type)
+{
+    mtype = type;
+}
+
+void Signal::Setcountry(std::string country)
+{
+    mcountry = country;
+}
+
+void Signal::SetcountryRevision(std::string countryRevision)
+{
+    mcountryRevision = countryRevision;
+}
+
+void Signal::Setsubtype(string subtype)
+{
+    msubtype = subtype;
+}
+
+void Signal::SethOffset(double hOffset)
+{
+    mhOffset = hOffset;
+}
+
+void Signal::Setpitch(double pitch)
+{
+    mpitch = pitch;
+}
+
+void Signal::Setroll(double roll)
+{
+    mroll = roll;
+}
+
+void Signal::Setheight(double height)
+{
+    mheight = height;
+}
+
+void Signal::Setwidth(double width)
+{
+    mwidth = width;
+}
+
+void Signal::SetlaneValidity(int fromLane, int toLane)
+{
+    if(mpsignal_laneValidity == 0)
+    {
+        mpsignal_laneValidity = new signal_laneValidity(fromLane,toLane);
+    }
+    else
+    {
+        mpsignal_laneValidity->SetfromLane(fromLane);
+        mpsignal_laneValidity->SettoLane(toLane);
+    }
+}
+
+void Signal::SetpositionRoad(double s, double t, double zOffset, double hOffset, double pitch,double roll)
+{
+    if(mpsignal_positionRoad == 0)
+    {
+        mpsignal_positionRoad = new signal_positionRoad(s,t,zOffset,hOffset,pitch,roll);
+    }
+    else
+    {
+        mpsignal_positionRoad->Sets(s);
+        mpsignal_positionRoad->Sett(t);
+        mpsignal_positionRoad->SetzOffset(zOffset);
+        mpsignal_positionRoad->SethOffset(hOffset);
+        mpsignal_positionRoad->Setpitch(pitch);
+        mpsignal_positionRoad->Setroll(roll);
+    }
+}
+
+void Signal::SetpositionInertial(double x, double y, double z, double hdg, double pitch, double roll)
+{
+    if(mpsignal_positionInertial == 0)
+    {
+        mpsignal_positionInertial = new signal_positionInertial(x,y,z,hdg,pitch,roll);
+    }
+    else
+    {
+        mpsignal_positionInertial->Setx(x);
+        mpsignal_positionInertial->Sety(y);
+        mpsignal_positionInertial->Setz(z);
+        mpsignal_positionInertial->Sethdg(hdg);
+        mpsignal_positionInertial->Setpitch(pitch);
+        mpsignal_positionInertial->Setroll(roll);
+    }
+}
+
+signal_laneValidity * Signal::GetlaneValidity()
+{
+    return mpsignal_laneValidity;
+}
+
+
+
+

src/common/common/xodr/OpenDrive/ObjectSignal.h

@@ -0,0 +1,163 @@
+#ifndef OBJECTSIGNAL_H
+#define OBJECTSIGNAL_H
+
+#include <vector>
+#include <string>
+
+using std::vector;
+using std::string;
+
+
+//***********************************************************************************
+//Object
+//***********************************************************************************
+class Object
+{
+public:
+	Object(){}
+};
+//----------------------------------------------------------------------------------
+
+
+
+class signal_positionRoad
+{
+private:
+    double ms;
+    double mt;
+    double mzOffset;
+    double mhOffset;
+    double mpitch;
+    double mroll;
+public:
+    signal_positionRoad(double s,double t,double zOffset,double hOffset, double pitch,double roll);
+    double Gets();
+    double Gett();
+    double GetzOffset();
+    double GethOffset();
+    double Getpitch();
+    double Getroll();
+    void Sets(double s);
+    void Sett(double t);
+    void SetzOffset(double zOffset);
+    void SethOffset(double hOffset);
+    void Setpitch(double pitch);
+    void Setroll(double roll);
+};
+
+class signal_positionInertial
+{
+private:
+    double mx;
+    double my;
+    double mz;
+    double mhdg;
+    double mpitch;
+    double mroll;
+public:
+    signal_positionInertial(double x,double y,double z,double hdg,double pitch,double roll );
+    double Getx();
+    double Gety();
+    double Getz();
+    double Gethdg();
+    double Getpitch();
+    double Getroll();
+    void Setx(double x);
+    void Sety(double y);
+    void Setz(double z);
+    void Sethdg(double hdg);
+    void Setpitch(double pitch);
+    void Setroll(double roll);
+};
+
+class signal_laneValidity
+{
+private:
+    int mfromLane;
+    int mtoLane;
+public:
+    signal_laneValidity(int fromLane,int toLane);
+    int GetfromLane();
+    int GettoLane();
+    void SetfromLane(int fromLane);
+    void SettoLane(int toLane);
+};
+
+
+//***********************************************************************************
+//Signal
+//***********************************************************************************
+class Signal
+{
+private:
+    double ms;
+    double mt;
+    string mid;
+    string mname;
+    bool mdynamic;
+    string morientation;
+    double mzOffset;
+    string mtype;
+    string mcountry;
+    string mcountryRevision;
+    string msubtype;
+    double mhOffset;
+    double mpitch;
+    double mroll;
+    double mheight;
+    double mwidth;
+    signal_positionRoad * mpsignal_positionRoad;
+    signal_positionInertial * mpsignal_positionInertial;
+    signal_laneValidity * mpsignal_laneValidity;
+public:
+    Signal(double s,double t,string id,string name,bool dynamic,string orientation,double zOffset,string type,string country,string countryRevision,
+           string subtype,double hOffset,double pitch,double roll ,double height,double width);
+    Signal();
+    ~Signal();
+    Signal& operator=(const Signal& x);
+    Signal(const Signal & x);
+    double Gets();
+    double Gett();
+    string Getid();
+    string Getname();
+    bool Getdynamic();
+    string Getorientation();
+    double GetzOffset();
+    string Gettype();
+    string Getcountry();
+    string GetcountryRevision();
+    string Getsubtype();
+    double GethOffset();
+    double Getpitch();
+    double Getroll();
+    double Getheight();
+    double Getwidth();
+    signal_positionRoad * GetpositionRoad();
+    signal_positionInertial * GetpositionInertial();
+    signal_laneValidity * GetlaneValidity();
+    void Sets(double s);
+    void Sett(double t);
+    void Setid(string id);
+    void Setname(string name);
+    void Setdynamic(bool dynamic);
+    void Setorientation(string orientation);
+    void SetzOffset(double zOffset);
+    void Settype(string type);
+    void Setcountry(string country);
+    void SetcountryRevision(string countryRevision);
+    void Setsubtype(string subtype);
+    void SethOffset(double hOffset);
+    void Setpitch(double pitch);
+    void Setroll(double roll);
+    void Setheight(double height);
+    void Setwidth(double width);
+    void SetlaneValidity(int fromLane, int toLane);
+    void SetpositionRoad(double s,double t, double zOffset,double hOffset,double pitch,double roll);
+    void SetpositionInertial(double x,double y, double z, double hdg,double pitch,double roll);
+
+
+};
+//----------------------------------------------------------------------------------
+
+
+#endif

src/driver/driver_map_xodrload/OpenDrive/OpenDrive.cpp → src/common/common/xodr/OpenDrive/OpenDrive.cpp

@@ -299,3 +299,9 @@ void Header::SetXYValues(double north, double south, double east,double west)
 	mEast=east;
 	mWest=west;
 }
+
+void Header::GetLat0Lon0(double &lat0, double &lon0)
+{
+    lat0 = mLat0;
+    lon0 = mLon0;
+}

src/driver/driver_map_xodrload/OpenDrive/OpenDrive.h → src/common/common/xodr/OpenDrive/OpenDrive.h

@@ -181,6 +181,9 @@ public:
 
     void SetAllParams(unsigned short int revMajor, unsigned short int revMinor, string name, float version, string date,
         double north, double south, double east,double west,double lat0,double lon0,double hdg0);
+
+
+    void GetLat0Lon0(double & lat0,double & lon0);
 };
 
 

src/driver/driver_map_xodrload/OpenDrive/OpenDriveXmlParser.cpp → src/common/common/xodr/OpenDrive/OpenDriveXmlParser.cpp

@@ -75,7 +75,7 @@ bool OpenDriveXmlParser::ReadRoad(TiXmlElement *node)
 
 	int checker=TIXML_SUCCESS;
 	
-	checker+=node->QueryStringAttribute("name",&name);
+//	checker+=node->QueryStringAttribute("name",&name);
 	checker+=node->QueryDoubleAttribute("length",&length);
 	checker+=node->QueryStringAttribute("id",&id);
 	checker+=node->QueryStringAttribute("junction",&junction);
@@ -85,6 +85,11 @@ bool OpenDriveXmlParser::ReadRoad(TiXmlElement *node)
 		cout<<"Error parsing Road attributes"<<endl;
 		return false;
 	}
+
+    if(node->QueryStringAttribute("name",&name) != TIXML_SUCCESS)
+    {
+        name = "";
+    }
 	//fill in
 	mOpenDrive->AddRoad(name, length, id, junction);
 	Road* road = mOpenDrive->GetLastAddedRoad();
@@ -910,8 +915,117 @@ bool OpenDriveXmlParser::ReadObjects (Road* road, TiXmlElement *node)
 
 bool OpenDriveXmlParser::ReadSignals (Road* road, TiXmlElement *node)
 {
-	return true;
+
+    TiXmlElement *subNode = node->FirstChildElement("signal");
+    while (subNode)
+    {
+        ReadSignal(road, subNode);
+        subNode=subNode->NextSiblingElement("signal");
+    }
+    return true;
+
+
+}
+
+bool OpenDriveXmlParser::ReadSignal(Road *road, TiXmlElement *node)
+{
+    double s;
+    double t;
+    string id;
+    string name;
+    bool dynamic;
+    string strdynamic;
+    string orientation;
+    double zOffset;
+    string type;
+    string country;
+    string countryRevision;
+    string subtype;
+    double hOffset;
+    double pitch;
+    double roll;
+    double height;
+    double width;
+
+    int checker=TIXML_SUCCESS;
+    checker+=node->QueryDoubleAttribute("s",&s);
+    checker+=node->QueryDoubleAttribute("t",&t);
+    checker+=node->QueryStringAttribute("id",&id);
+    checker+=node->QueryStringAttribute("name",&name);
+    checker+=node->QueryStringAttribute("dynamic",&strdynamic);
+    checker+=node->QueryStringAttribute("orientation",&orientation);
+    checker+=node->QueryDoubleAttribute("zOffset",&zOffset);
+    checker+=node->QueryStringAttribute("type",&type);
+    checker+=node->QueryStringAttribute("country",&country);
+    checker+=node->QueryStringAttribute("countryRevision",&countryRevision);
+    checker+=node->QueryStringAttribute("subtype",&subtype);
+    checker+=node->QueryDoubleAttribute("hOffset",&hOffset);
+    checker+=node->QueryDoubleAttribute("pitch",&pitch);
+    checker+=node->QueryDoubleAttribute("roll",&roll);
+    checker+=node->QueryDoubleAttribute("height",&height);
+    checker+=node->QueryDoubleAttribute("width",&width);
+    if (checker!=TIXML_SUCCESS)
+    {
+        cout<<"Error parsing Lane Signals attributes"<<endl;
+        return false;
+    }
+    if(strncmp(strdynamic.data(),"no",256) == 0)dynamic = false;
+    else dynamic = true;
+    road->AddSignal(s,t,id,name,dynamic,orientation,zOffset,type,country,countryRevision,subtype,hOffset,
+                    pitch,roll,height,width);
+
+    Signal * pSignal = road->GetSignal(road->GetSignalCount() - 1);
+    TiXmlElement * subNode;
+    //Proceed to Signals
+    subNode=node->FirstChildElement("validity");
+    if (subNode)
+    {
+        ReadSignal_laneValidity(pSignal, subNode);
+    }
+    subNode=node->FirstChildElement("positionInertial");
+    if(subNode)
+    {
+        ReadSignal_positionInertial(pSignal,subNode);
+    }
+    return true;
 }
+
+bool OpenDriveXmlParser::ReadSignal_laneValidity(Signal *pSignal, TiXmlElement *node)
+{
+    int fromLane;
+    int toLane;
+    int checker=TIXML_SUCCESS;
+    checker+=node->QueryIntAttribute("fromLane",&fromLane);
+    checker+=node->QueryIntAttribute("toLane",&toLane);
+    if (checker!=TIXML_SUCCESS)
+    {
+        cout<<"Error parsing laneValidity attributes"<<endl;
+        return false;
+    }
+    pSignal->SetlaneValidity(fromLane,toLane);
+    return true;
+}
+
+bool OpenDriveXmlParser::ReadSignal_positionInertial(Signal *pSignal, TiXmlElement *node)
+{
+    double x,y,z,hdg,pitch,roll;
+    int checker=TIXML_SUCCESS;
+    checker+=node->QueryDoubleAttribute("x",&x);
+    checker+=node->QueryDoubleAttribute("y",&y);
+    checker+=node->QueryDoubleAttribute("z",&z);
+    checker+=node->QueryDoubleAttribute("hdg",&hdg);
+    checker+=node->QueryDoubleAttribute("pitch",&pitch);
+    checker+=node->QueryDoubleAttribute("roll",&roll);
+    if (checker!=TIXML_SUCCESS)
+    {
+        cout<<"Error parsing positionInertial attributes"<<endl;
+        return false;
+    }
+    pSignal->SetpositionInertial(x,y,z,hdg,pitch,roll);
+    return true;
+}
+
+
 //--------------
 
 bool OpenDriveXmlParser::ReadSurface (Road* road, TiXmlElement *node)

src/driver/driver_map_xodrload/OpenDrive/OpenDriveXmlParser.h → src/common/common/xodr/OpenDrive/OpenDriveXmlParser.h

@@ -64,6 +64,9 @@ public:
 
 	bool ReadObjects (Road* road, TiXmlElement *node);
 	bool ReadSignals (Road* road, TiXmlElement *node);
+    bool ReadSignal(Road * road,TiXmlElement * node);
+    bool ReadSignal_positionInertial(Signal * pSignal, TiXmlElement *node);
+    bool ReadSignal_laneValidity(Signal * pSignal,TiXmlElement * node);
 	//--------------
 
 	bool ReadSurface (Road* road, TiXmlElement *node);

src/driver/driver_map_xodrload/OpenDrive/OpenDriveXmlWriter.cpp → src/common/common/xodr/OpenDrive/OpenDriveXmlWriter.cpp

@@ -920,8 +920,83 @@ bool OpenDriveXmlWriter::WriteSignals (TiXmlElement *node, Road* road)
 	TiXmlElement* nodeSignals = new TiXmlElement("signals");
 	node->LinkEndChild(nodeSignals);
 
+    unsigned int lSignalSectionCount = road->GetSignalCount();
+    for(unsigned int i=0; i<lSignalSectionCount; i++)
+    {
+        WriteSignal(nodeSignals, road->GetSignal(i));
+    }
+
 	return true;
 }
+
+bool OpenDriveXmlWriter::WriteSignal(TiXmlElement *node, Signal * pSignal)
+{
+    TiXmlElement* nodeSignal = new TiXmlElement("signal");
+    node->LinkEndChild(nodeSignal);
+
+    nodeSignal->SetAttribute("s",pSignal->Gets());
+    nodeSignal->SetAttribute("t",pSignal->Gett());
+    nodeSignal->SetAttribute("id",pSignal->Getid());
+    nodeSignal->SetAttribute("name",pSignal->Getname());
+    if(pSignal->Getdynamic() == true)
+    nodeSignal->SetAttribute("dynamic","yes");
+    else
+        nodeSignal->SetAttribute("dynamic","no");
+    nodeSignal->SetAttribute("orientation",pSignal->Getorientation());
+    nodeSignal->SetAttribute("zOffset",pSignal->GetzOffset());
+    nodeSignal->SetAttribute("type",pSignal->Gettype());
+    nodeSignal->SetAttribute("country",pSignal->Getcountry());
+    nodeSignal->SetAttribute("countryRevision",pSignal->GetcountryRevision());
+    nodeSignal->SetAttribute("subtype",pSignal->Getsubtype());
+    nodeSignal->SetAttribute("hOffset",pSignal->GethOffset());
+    nodeSignal->SetAttribute("pitch",pSignal->Getpitch());
+    nodeSignal->SetAttribute("roll",pSignal->Getroll());
+    nodeSignal->SetAttribute("height",pSignal->Getheight());
+    nodeSignal->SetAttribute("width",pSignal->Getwidth());
+
+    signal_laneValidity * psignal_lanevalidity = pSignal->GetlaneValidity();
+    if(psignal_lanevalidity != 0)
+    {
+        WriteSignal_laneValidity(nodeSignal,psignal_lanevalidity);
+    }
+
+    signal_positionInertial * psignal_positionInertial = pSignal->GetpositionInertial();
+    if(psignal_positionInertial != 0)
+    {
+        WriteSignal_positionInertial(nodeSignal,psignal_positionInertial);
+    }
+
+    return true;
+}
+
+bool OpenDriveXmlWriter::WriteSignal_positionInertial(TiXmlElement *node, signal_positionInertial *pSignal_positionInertial)
+{
+    TiXmlElement* nodepositionInertial = new TiXmlElement("positionInertial");
+
+    node->LinkEndChild(nodepositionInertial);
+
+    nodepositionInertial->SetAttribute("x",pSignal_positionInertial->Getx());
+    nodepositionInertial->SetAttribute("y",pSignal_positionInertial->Gety());
+    nodepositionInertial->SetAttribute("z",pSignal_positionInertial->Getz());
+    nodepositionInertial->SetAttribute("hdg",pSignal_positionInertial->Gethdg());
+    nodepositionInertial->SetAttribute("pitch",pSignal_positionInertial->Getpitch());
+    nodepositionInertial->SetAttribute("roll",pSignal_positionInertial->Getroll());
+
+    return true;
+}
+
+bool OpenDriveXmlWriter::WriteSignal_laneValidity(TiXmlElement *node, signal_laneValidity *pSignal_laneValidity)
+{
+    TiXmlElement* nodelaneValidity = new TiXmlElement("validity");
+
+    node->LinkEndChild(nodelaneValidity);
+
+    nodelaneValidity->SetAttribute("fromLane",pSignal_laneValidity->GetfromLane());
+    nodelaneValidity->SetAttribute("toLane",pSignal_laneValidity->GettoLane());
+
+    return true;
+}
+
 //--------------
 
 bool OpenDriveXmlWriter::WriteSurface (TiXmlElement *node, Road* road)

src/driver/driver_map_xodrload/OpenDrive/OpenDriveXmlWriter.h → src/common/common/xodr/OpenDrive/OpenDriveXmlWriter.h

@@ -65,6 +65,9 @@ public:
 
 	bool WriteObjects (TiXmlElement *node, Road* road);
 	bool WriteSignals (TiXmlElement *node, Road* road);
+    bool WriteSignal(TiXmlElement * node, Signal * pSignal);
+    bool WriteSignal_positionInertial(TiXmlElement * node, signal_positionInertial * pSignal_positionInertial);
+    bool WriteSignal_laneValidity(TiXmlElement * node, signal_laneValidity * pSignal_laneValidity);
 	//--------------
 
 	bool WriteSurface (TiXmlElement *node, Road* road);

src/driver/driver_map_xodrload/OpenDrive/Road.cpp → src/common/common/xodr/OpenDrive/Road.cpp

@@ -610,12 +610,18 @@ unsigned int Road::AddObject()
 	return index;
 }
 //-------------
-unsigned int Road::AddSignal()
+unsigned int Road::AddSignal(double s,double t,string id,string name,bool dynamic,string orientation,double zOffset,string type,string country,string countryRevision,
+                             string subtype,double hOffset,double pitch,double roll ,double height,double width)
 {
 	// Check the first method in the group for details
 
 	unsigned int index=GetSignalCount();
-	mSignalsVector.push_back(Signal());	
+    Signal x(s,t,id,name,dynamic,orientation,zOffset,type,country,countryRevision,
+             subtype,hOffset,pitch,roll,height,width);
+    mSignalsVector.push_back(x);
+
+//    mSignalsVector.push_back(Signal(s,t,id,name,dynamic,orientation,zOffset,type,country,countryRevision,
+//                                    subtype,hOffset,pitch,roll,height,width));
 	mLastAddedSignal=index;
 	return index;
 }

src/driver/driver_map_xodrload/OpenDrive/Road.h → src/common/common/xodr/OpenDrive/Road.h

@@ -229,7 +229,8 @@ public:
 	unsigned int AddCrossfall (string side, double s, double a, double b, double c, double d);
 	unsigned int AddLaneSection(double s);
 	unsigned int AddObject();
-	unsigned int AddSignal();
+    unsigned int AddSignal(double s,double t,string id,string name,bool dynamic,string orientation,double zOffset,string type,string country,string countryRevision,
+                           string subtype,double hOffset,double pitch,double roll ,double height,double width);
 
 	/**
 	 * Methods used to clone child records in the respective vectors

src/driver/driver_map_xodrload/OpenDrive/RoadGeometry.h → src/common/common/xodr/OpenDrive/RoadGeometry.h

@@ -30,7 +30,7 @@ protected:
 	double mHdg;
 	double mLength;
 	double mS2;
-    short int mGeomType;	//0-line, 1-spiral, 2-arc 3-poly3 4-parampoly3
+    short int mGeomType;	//0-line, 1-arc, 2-spiral 3-poly3 4-parampoly3
 public:
 	/**
 	 * Constructor that initializes the base properties of teh record

src/decition/common/common/gps_type.h

@@ -44,7 +44,7 @@ namespace iv {
 
         int speed_mode = 0;
         int mode2 = 0;
-        double speed = 3.5;			//速度  若导航点则为导航预设速度  若为当前点则为当前车速
+        double speed = -1;			//速度  若导航点则为导航预设速度  若为当前点则为当前车速
 
         int roadMode;
         int runMode;

src/decition/decition_brain/decition/brain.cpp

@@ -374,6 +374,10 @@ void iv::decition::BrainDecition::run() {
     std::string gpsOffset_x = xp.GetParam("gpsOffset_X","0");
     std::string gpsOffset_y = xp.GetParam("gpsOffset_Y","0");
     std::string strexternmpc = xp.GetParam("ExternMPC","false");  //If Use MPC set true
+
+
+    adjuseGpsLidarPosition();
+
     if(strexternmpc == "true")
     {
         mbUseExternMPC = true;
@@ -1365,3 +1369,13 @@ void iv::decition::BrainDecition::Updateultra(const char *pdata, const int ndata
 void iv::decition::BrainDecition::UpdateSate(){
      decitionGps00->isFirstRun=true;
 }
+
+void iv::decition::BrainDecition::adjuseGpsLidarPosition(){
+
+    ServiceCarStatus.msysparam.lidarGpsXiuzheng -=  ServiceCarStatus.msysparam.gpsOffset_y;
+    ServiceCarStatus.msysparam.radarGpsXiuzheng -=  ServiceCarStatus.msysparam.gpsOffset_y;
+    ServiceCarStatus.msysparam.rearRadarGpsXiuzheng +=  ServiceCarStatus.msysparam.gpsOffset_y;
+    ServiceCarStatus.msysparam.rearLidarGpsXiuzheng +=  ServiceCarStatus.msysparam.gpsOffset_y;
+    ServiceCarStatus.msysparam.frontGpsXiuzheng -=  ServiceCarStatus.msysparam.gpsOffset_y;
+    ServiceCarStatus.msysparam.rearGpsXiuzheng +=  ServiceCarStatus.msysparam.gpsOffset_y;
+}

src/decition/decition_brain/decition/brain.h

@@ -161,6 +161,7 @@ namespace iv {
         private:
 //            Adcivstatedb * mpasd;
             void UpdateChassis(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname);
+            void adjuseGpsLidarPosition();
 
             fanyaapi mmpcapi;
 

src/decition/decition_brain/decition/decide_gps_00.cpp

@@ -184,7 +184,6 @@ std::vector<double> lastDistanceVector(roadSum, -1);
 
 bool qiedianCount = false;
 //日常展示
-static int sensor_update=0;
 
 iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_gps_ins,
                                                                    const std::vector<GPSData> gpsMapLine,
@@ -234,11 +233,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
                                                           DecideGps00::minDis,
                                                           DecideGps00::maxAngle);
         DecideGps00::lastGpsIndex = PathPoint;
-        if(sensor_update==0)
-        {
-            adjuseGpsLidarPosition();
-            sensor_update=1;
-        }
+
         if(ServiceCarStatus.speed_control==true){
             Compute00().getDesiredSpeed(gpsMapLine);   //add by zj
         }
@@ -1298,7 +1293,12 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
     {
         dSpeed = min(4.0,dSpeed);
 
-    }
+    }else{
+        dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode0,dSpeed);
+        gps_decition->leftlamp = false;
+        gps_decition->rightlamp = false;
+
+}
 
 
 
@@ -1309,6 +1309,13 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
         dSpeed = min(25.0,dSpeed);
 
     }
+
+    if((gpsMapLine[PathPoint]->speed)>0.001)
+    {
+        dSpeed = min((gpsMapLine[PathPoint]->speed*3.6),dSpeed);
+    }
+
+
     dSecSpeed = dSpeed / 3.6;
 
 
@@ -1909,6 +1916,10 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
     //------------------------------v2x traffic light end--------------------------------------------------------------------------------------
 
 
+    transferGpsMode2(gpsMapLine);
+
+
+
     if((ServiceCarStatus.msysparam.mvehtype=="hapo")&&(abs(realspeed)<18)){
         if(obsDistance>0 && obsDistance<6){
                 dSpeed=0;
@@ -3752,15 +3763,7 @@ float iv::decition::DecideGps00::computeTrafficSpeedLimt(float trafficDis){
     return limit;
 }
 
-void iv::decition::DecideGps00::adjuseGpsLidarPosition(){
 
-    ServiceCarStatus.msysparam.lidarGpsXiuzheng -=  ServiceCarStatus.msysparam.gpsOffset_y;
-    ServiceCarStatus.msysparam.radarGpsXiuzheng -=  ServiceCarStatus.msysparam.gpsOffset_y;
-    ServiceCarStatus.msysparam.rearRadarGpsXiuzheng +=  ServiceCarStatus.msysparam.gpsOffset_y;
-    ServiceCarStatus.msysparam.rearLidarGpsXiuzheng +=  ServiceCarStatus.msysparam.gpsOffset_y;
-    ServiceCarStatus.msysparam.frontGpsXiuzheng -=  ServiceCarStatus.msysparam.gpsOffset_y;
-    ServiceCarStatus.msysparam.rearGpsXiuzheng +=  ServiceCarStatus.msysparam.gpsOffset_y;
-}
 
 bool iv::decition::DecideGps00::adjuseultra(){
      bool front=false,back=false,left=false,right=false;
@@ -3794,4 +3797,11 @@ bool iv::decition::DecideGps00::adjuseultra(){
 
 }
 
-
+void iv::decition::DecideGps00::transferGpsMode2( const std::vector<GPSData> gpsMapLine){
+    if(  gpsMapLine[PathPoint]->mode2==3000){
+        if(obsDistance>5){
+            obsDistance=200;
+        }
+        dSpeed=min(dSpeed,5.0);
+    }
+}

src/decition/decition_brain/decition/decide_gps_00.h

@@ -203,11 +203,12 @@ public:
 
     bool computeTrafficPass(float trafficDis,int trafficColor,float trafficTime,float realSecSpeed,float dSecSpeed);
     float computeTrafficSpeedLimt(float trafficDis);
-    void adjuseGpsLidarPosition();
+
     bool adjuseultra();
 
     iv::Station getNearestStation(iv::GPS_INS now_gps_ins,std::vector<Station> station_n,std::vector<GPSData> gpsMap);
 
+    void transferGpsMode2( const std::vector<GPSData> gpsMapLine);
     GPS_INS aim_gps_ins;
     GPS_INS chaoche_start_gps;
     bool is_arrivaled=false;

src/decition/decition_brain_sf/decition/brain.cpp

@@ -334,6 +334,10 @@ void iv::decition::BrainDecition::run() {
     std::string gpsOffset_x = xp.GetParam("gpsOffset_X","0");
     std::string gpsOffset_y = xp.GetParam("gpsOffset_Y","0");
     std::string strexternmpc = xp.GetParam("ExternMPC","false");  //If Use MPC set true
+
+
+     adjuseGpsLidarPosition();
+
     if(strexternmpc == "true")
     {
         mbUseExternMPC = true;
@@ -462,7 +466,7 @@ void iv::decition::BrainDecition::run() {
 
         eyes->getSensorObstacle(obs_radar_, obs_camera_,gps_data_, obs_lidar_grid_);	//从传感器线程临界区交换数据
 
-
+/*
         if(obs_lidar_grid_!= NULL)
         {
             std::cout<<"receive fusion date"<<std::endl;
@@ -486,7 +490,7 @@ void iv::decition::BrainDecition::run() {
             }
               std::cout<<"print fusion date end"<<std::endl;
         }
-
+*/
 
         ServiceLidar.copylidarperto(lidar_per);
 
@@ -1001,6 +1005,11 @@ void iv::decition::BrainDecition::UpdateMap(const char *mapdata, const int mapda
             *data = x;
             navigation_data.push_back(data);
 
+   //         if(data->mode2 > 0)
+     //       {
+      //          int a = 1;
+       //     }
+
             fanya::MAP_DATA xmapdata;
             xmapdata.gps_lat = x.gps_lat;
             xmapdata.gps_lng = x.gps_lng;
@@ -1260,3 +1269,13 @@ void iv::decition::BrainDecition::UpdateVbox(const char *pdata, const int ndatas
 void iv::decition::BrainDecition::UpdateSate(){
      decitionGps00->isFirstRun=true;
 }
+
+void iv::decition::BrainDecition::adjuseGpsLidarPosition(){
+
+    ServiceCarStatus.msysparam.lidarGpsXiuzheng -=  ServiceCarStatus.msysparam.gpsOffset_y;
+    ServiceCarStatus.msysparam.radarGpsXiuzheng -=  ServiceCarStatus.msysparam.gpsOffset_y;
+    ServiceCarStatus.msysparam.rearRadarGpsXiuzheng +=  ServiceCarStatus.msysparam.gpsOffset_y;
+    ServiceCarStatus.msysparam.rearLidarGpsXiuzheng +=  ServiceCarStatus.msysparam.gpsOffset_y;
+    ServiceCarStatus.msysparam.frontGpsXiuzheng -=  ServiceCarStatus.msysparam.gpsOffset_y;
+    ServiceCarStatus.msysparam.rearGpsXiuzheng +=  ServiceCarStatus.msysparam.gpsOffset_y;
+}

src/decition/decition_brain_sf/decition/brain.h

@@ -156,6 +156,7 @@ namespace iv {
         private:
 //            Adcivstatedb * mpasd;
             void UpdateChassis(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname);
+            void adjuseGpsLidarPosition();
 
             fanyaapi mmpcapi;
 

src/decition/decition_brain_sf/decition/decide_gps_00.cpp

@@ -235,7 +235,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
                                                           DecideGps00::maxAngle);
         DecideGps00::lastGpsIndex = PathPoint;
 
-        adjuseGpsLidarPosition();
+
         if(ServiceCarStatus.speed_control==true){
             Compute00().getDesiredSpeed(gpsMapLine);   //add by zj
         }
@@ -253,6 +253,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
         }else{
             circleMode=false;
         }
+
         //     circleMode = true;
 
 
@@ -1299,6 +1300,11 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
     {
         dSpeed = min(4.0,dSpeed);
 
+    }else{
+            dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode0,dSpeed);
+            gps_decition->leftlamp = false;
+            gps_decition->rightlamp = false;
+
     }
 
 
@@ -1310,6 +1316,16 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
         dSpeed = min(25.0,dSpeed);
 
     }
+
+
+
+    if((gpsMapLine[PathPoint]->speed)>0.001)
+    {
+        dSpeed = min((gpsMapLine[PathPoint]->speed*3.6),dSpeed);
+    }
+
+
+
     dSecSpeed = dSpeed / 3.6;
 
 
@@ -1853,6 +1869,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
     //------------------------------v2x traffic light end--------------------------------------------------------------------------------------
 
 
+    transferGpsMode2(gpsMapLine);
 
    // if(obsDistance>6.5){
      //   obsDistance=500;
@@ -3400,13 +3417,11 @@ float iv::decition::DecideGps00::computeTrafficSpeedLimt(float trafficDis){
     return limit;
 }
 
-void iv::decition::DecideGps00::adjuseGpsLidarPosition(){
-
-    ServiceCarStatus.msysparam.lidarGpsXiuzheng -=  ServiceCarStatus.msysparam.gpsOffset_y;
-    ServiceCarStatus.msysparam.radarGpsXiuzheng -=  ServiceCarStatus.msysparam.gpsOffset_y;
-    ServiceCarStatus.msysparam.rearRadarGpsXiuzheng +=  ServiceCarStatus.msysparam.gpsOffset_y;
-    ServiceCarStatus.msysparam.rearLidarGpsXiuzheng +=  ServiceCarStatus.msysparam.gpsOffset_y;
-    ServiceCarStatus.msysparam.frontGpsXiuzheng -=  ServiceCarStatus.msysparam.gpsOffset_y;
-    ServiceCarStatus.msysparam.rearGpsXiuzheng +=  ServiceCarStatus.msysparam.gpsOffset_y;
-
+void iv::decition::DecideGps00::transferGpsMode2( const std::vector<GPSData> gpsMapLine){
+    if(  gpsMapLine[PathPoint]->mode2==3000){
+        if(obsDistance>5){
+            obsDistance=200;
+        }
+        dSpeed=min(dSpeed,5.0);
+    }
 }

src/decition/decition_brain_sf/decition/decide_gps_00.h

@@ -202,7 +202,9 @@ public:
 
     bool computeTrafficPass(float trafficDis,int trafficColor,float trafficTime,float realSecSpeed,float dSecSpeed);
     float computeTrafficSpeedLimt(float trafficDis);
-    void adjuseGpsLidarPosition();
+
+
+    void transferGpsMode2( const std::vector<GPSData> gpsMapLine);
 
     GPS_INS aim_gps_ins;
     GPS_INS chaoche_start_gps;

src/detection/detection_lidar_cnn_segmentation/detection_lidar_cnn_segmentation.pro

@@ -46,10 +46,10 @@ INCLUDEPATH += /usr/local/cuda-10.2/targets/aarch64-linux/include
 LIBS += -lpcl_io -lpcl_common
 
 
-LIBS += -lboost_system  -lavutil -lglog -lprotobuf
+LIBS += -lboost_system  -lavutil  -lprotobuf -lcudnn
 
 
-unix:!macx: LIBS += -L$$PWD/../../../thirdpartylib/caffe -lcaffe
+unix:!macx: LIBS += -L$$PWD/../../../thirdpartylib/caffe -lcaffe -lcudnn
 
 HEADERS += \
     cluster2d.h \

src/detection/detection_lidar_cnn_segmentation/main.cpp

@@ -16,6 +16,8 @@
 
 #include <thread>
 
+//#include "alog.h"
+
 CNNSegmentation gcnn;
 
 void * gpa;
@@ -401,6 +403,11 @@ void exitfunc()
 
 }
 
+//void testcall(const char * str)
+//{
+//    std::cout<<"call msg is "<<str<<std::endl;
+//}
+
 int main(int argc, char *argv[])
 {
     showversion("detection_lidar_cnn_segmentation");
@@ -410,7 +417,18 @@ int main(int argc, char *argv[])
     givlog = new iv::Ivlog("lidar_cnn_segmentation");
 
     gfault->SetFaultState(0,0,"cnn initialize.");
+\
+
+//    ALOG("cnn.");
+//    ALOG("VALUE = %d",1);
+
+//    iv::Logger::Inst().SetCallback(testcall);
+
+//    ALOG("cnn.");
+
+//    iv::Logger::Inst().setlogfile("/home/nvidia/logger.txt");
 
+//    ALOG("CNN");
 
     char * strhome = getenv("HOME");
     std::string protofile = strhome ;//

src/driver/driver_map_xodrload/OpenDrive/ObjectSignal.h

@@ -1,28 +0,0 @@
-#ifndef OBJECTSIGNAL_H
-#define OBJECTSIGNAL_H
-
-
-
-//***********************************************************************************
-//Object
-//***********************************************************************************
-class Object
-{
-public:
-	Object(){}
-};
-//----------------------------------------------------------------------------------
-
-
-//***********************************************************************************
-//Signal
-//***********************************************************************************
-class Signal
-{
-public:
-	Signal(){}
-};
-//----------------------------------------------------------------------------------
-
-
-#endif

src/driver/driver_map_xodrload/driver_map_xodrload.pro

@@ -19,19 +19,19 @@ DEFINES += QT_DEPRECATED_WARNINGS
 
 SOURCES += main.cpp     \
     ../../include/msgtype/v2x.pb.cc \
-OpenDrive/OpenDriveXmlWriter.cpp \
-OpenDrive/OtherStructures.cpp \
-OpenDrive/OpenDrive.cpp \
-OpenDrive/Road.cpp \
-OpenDrive/Junction.cpp \
-OpenDrive/Lane.cpp \
-OpenDrive/OpenDriveXmlParser.cpp \
-OpenDrive/ObjectSignal.cpp \
-OpenDrive/RoadGeometry.cpp \
-TinyXML/tinyxml.cpp \
-TinyXML/tinyxmlparser.cpp \
-TinyXML/tinystr.cpp \
-TinyXML/tinyxmlerror.cpp \
+    ../../common/common/xodr/OpenDrive/OpenDriveXmlWriter.cpp \
+    ../../common/common/xodr/OpenDrive/OtherStructures.cpp \
+    ../../common/common/xodr/OpenDrive/OpenDrive.cpp \
+    ../../common/common/xodr/OpenDrive/Road.cpp \
+    ../../common/common/xodr/OpenDrive/Junction.cpp \
+    ../../common/common/xodr/OpenDrive/Lane.cpp \
+    ../../common/common/xodr/OpenDrive/OpenDriveXmlParser.cpp \
+    ../../common/common/xodr/OpenDrive/ObjectSignal.cpp \
+    ../../common/common/xodr/OpenDrive/RoadGeometry.cpp \
+    ../../common/common/xodr/TinyXML/tinyxml.cpp \
+    ../../common/common/xodr/TinyXML/tinyxmlparser.cpp \
+    ../../common/common/xodr/TinyXML/tinystr.cpp \
+    ../../common/common/xodr/TinyXML/tinyxmlerror.cpp \
     fresnl.cpp \
     const.cpp \
     polevl.c \
@@ -46,17 +46,17 @@ TinyXML/tinyxmlerror.cpp \
 HEADERS += \
     ../../../include/ivexit.h \
     ../../include/msgtype/v2x.pb.h \
-OpenDrive/OpenDriveXmlParser.h \
-OpenDrive/RoadGeometry.h \
-OpenDrive/Road.h \
-OpenDrive/Junction.h \
-OpenDrive/OpenDriveXmlWriter.h \
-OpenDrive/Lane.h \
-OpenDrive/OtherStructures.h \
-OpenDrive/OpenDrive.h \
-OpenDrive/ObjectSignal.h \
-TinyXML/tinyxml.h \
-TinyXML/tinystr.h \
+    ../../common/common/xodr/OpenDrive/OpenDriveXmlParser.h \
+    ../../common/common/xodr/OpenDrive/RoadGeometry.h \
+    ../../common/common/xodr/OpenDrive/Road.h \
+    ../../common/common/xodr/OpenDrive/Junction.h \
+    ../../common/common/xodr/OpenDrive/OpenDriveXmlWriter.h \
+    ../../common/common/xodr/OpenDrive/Lane.h \
+    ../../common/common/xodr/OpenDrive/OtherStructures.h \
+    ../../common/common/xodr/OpenDrive/OpenDrive.h \
+    ../../common/common/xodr/OpenDrive/ObjectSignal.h \
+    ../../common/common/xodr/TinyXML/tinyxml.h \
+    ../../common/common/xodr/TinyXML/tinystr.h \
     mconf.h \
     globalplan.h \
     gps_type.h \
@@ -84,4 +84,6 @@ TinyXML/tinystr.h \
     error( "Couldn't find the iveigen.pri file!" )
 }
 
+INCLUDEPATH += $$PWD/../../common/common/xodr
+
 

src/driver/driver_map_xodrload/globalplan.cpp

@@ -19,6 +19,8 @@ extern "C"
 #include "dubins.h"
 }
 
+
+static void AddSignalMark(Road * pRoad, int nlane,std::vector<PlanPoint> & xvectorPP);
 /**
  * @brief GetLineDis 获得点到直线Geometry的距离。
  * @param pline
@@ -1916,7 +1918,9 @@ std::vector<PlanPoint> GetLanePoint(pathsection xps,std::vector<PlanPoint> xvPP)
         }
     }
 
-    Road * pRoad = xps.mpRoad;
+
+
+
 
 
     if(xps.mnStartLaneSel > 0)
@@ -1927,14 +1931,64 @@ std::vector<PlanPoint> GetLanePoint(pathsection xps,std::vector<PlanPoint> xvPP)
         {
             xvvPP.push_back(xvectorPP.at(nvsize-1-i));
         }
+        AddSignalMark(xps.mpRoad,xps.mainsel,xvvPP);
         return xvvPP;
     }
 
 //    pRoad->GetLaneSection(xps.msectionid)
 
+    AddSignalMark(xps.mpRoad,xps.mainsel,xvectorPP);
     return xvectorPP;
 }
 
+
+static void AddSignalMark(Road * pRoad, int nlane,std::vector<PlanPoint> & xvectorPP)
+{
+    if(pRoad->GetSignalCount() == 0)return;
+    int nsigcount = pRoad->GetSignalCount();
+    int i;
+    for(i=0;i<nsigcount;i++)
+    {
+        Signal * pSig = pRoad->GetSignal(i);
+        int nfromlane = -100;
+        int ntolane = 100;
+        signal_laneValidity * pSig_laneValidity = pSig->GetlaneValidity();
+        if(pSig_laneValidity != 0)
+        {
+            nfromlane = pSig_laneValidity->GetfromLane();
+            ntolane = pSig_laneValidity->GettoLane();
+        }
+        if((nlane < 0)&&(nfromlane >= 0))
+        {
+            continue;
+        }
+        if((nlane > 0)&&(ntolane<=0))
+        {
+            continue;
+        }
+
+        double s = pSig->Gets();
+        double fpos = s/pRoad->GetRoadLength();
+        if(nlane > 0)fpos = 1.0 -fpos;
+        int npos = fpos *xvectorPP.size();
+        if(npos <0)npos = 0;
+        if(npos>=xvectorPP.size())npos = xvectorPP.size()-1;
+        while(xvectorPP.at(npos).nSignal != -1)
+        {
+            if(npos > 0)npos--;
+            else break;
+        }
+        while(xvectorPP.at(npos).nSignal != -1)
+        {
+            if(npos < (xvectorPP.size()-1))npos++;
+            else break;
+        }
+        xvectorPP.at(npos).nSignal = atoi(pSig->Gettype().data());
+
+
+    }
+}
+
 static std::vector<PlanPoint> GetPlanPoint(std::vector<pathsection> xpathsection,Road * pRoad,GeometryBlock * pgeob,
                                           Road * pRoad_obj,GeometryBlock * pgeob_obj,
                                            const double x_now,const double y_now,const double head,
@@ -1949,6 +2003,7 @@ static std::vector<PlanPoint> GetPlanPoint(std::vector<pathsection> xpathsection
   int indexstart = indexinroadpoint(xvectorPP,nearx,neary);
   int i;
   std::vector<PlanPoint> xvPP = GetLanePoint(xpathsection[0],xvectorPP);
+
   if(xpathsection[0].mainsel < 0)
   {
   for(i=indexstart;i<xvPP.size();i++)
@@ -2255,6 +2310,7 @@ int MakePlan(xodrdijkstra * pxd,OpenDrive * pxodr,const double x_now,const doubl
         int nindexstart = indexinroadpoint(xvPP,nearx,neary);
         int nindexend =  indexinroadpoint(xvPP,nearx_obj,neary_obj);
         int nlane = getlanesel(nlanesel,pRoad->GetLaneSection(0),lr_start);
+        AddSignalMark(pRoad,nlane,xvPP);
         if((nindexend >nindexstart)&&(lr_start == 2))
         {
             bNeedDikstra = false;

src/driver/driver_map_xodrload/globalplan.h

@@ -24,6 +24,8 @@ public:
     double mfDisToLaneLeft;
     int mnLaneori = 0;  //from Right 0
     int mnLaneTotal = 1; //Lane Total
+
+    int nSignal = -1;   //if 0 no signal point
 };
 
 int GetNearPoint(const double x,const double y,OpenDrive * pxodr,Road ** pObjRoad,GeometryBlock ** pgeo, double & s,double & nearx,

src/driver/driver_map_xodrload/main.cpp

@@ -467,12 +467,30 @@ void SetPlan(xodrobj xo)
         givlog->verbose(" %d %11.7f %11.7f %11.3f ",i,data->gps_lat,
                data->gps_lng,data->ins_heading_angle);
 
-        data->roadSum = 1;
-        data->roadMode = 0;
-        data->roadOri = 0;
+        data->roadOri = xPlan[i].mnLaneori;
+        data->roadSum = xPlan[i].mnLaneTotal;
+        data->mfDisToLaneLeft = xPlan[i].mfDisToLaneLeft;
+        data->mfDisToRoadLeft = xPlan[i].mfDisToRoadLeft;
+        data->mfLaneWidth = xPlan[i].mWidth;
+        data->mfRoadWidth = xPlan[i].mfRoadWidth;
 
-        if(xPlan[i].lanmp == -1)data->roadMode = 15;
-        if(xPlan[i].lanmp == 1)data->roadMode = 14;
+        data->mode2 = xPlan[i].nSignal;
+        if(data->mode2 == 3000)
+        {
+            int k;
+            for(k=(mapdata.size()-1);k>(mapdata.size()-150);k--)
+            {
+                if(k<0)break;
+                mapdata.at(k)->mode2 = 3000;
+            }
+        }
+
+//        data->roadSum = 1;
+//        data->roadMode = 0;
+//        data->roadOri = 0;
+
+//        if(xPlan[i].lanmp == -1)data->roadMode = 15;
+//        if(xPlan[i].lanmp == 1)data->roadMode = 14;
 
         mapdata.push_back(data);
 
@@ -576,12 +594,29 @@ void MultiStationPlan(iv::v2x::v2x * pxv2x,double fsrclat,double fsrclon,double
         givlog->verbose(" %d %11.7f %11.7f %11.3f ",i,data->gps_lat,
                data->gps_lng,data->ins_heading_angle);
 
-        data->roadSum = 1;
-        data->roadMode = 0;
-        data->roadOri = 0;
+//        data->roadSum = 1;
+//        data->roadMode = 0;
+//        data->roadOri = 0;
 
-        if(xPlan[i].lanmp == -1)data->roadMode = 15;
-        if(xPlan[i].lanmp == 1)data->roadMode = 14;
+//        if(xPlan[i].lanmp == -1)data->roadMode = 15;
+//        if(xPlan[i].lanmp == 1)data->roadMode = 14;
+        data->roadOri = xPlan[i].mnLaneori;
+        data->roadSum = xPlan[i].mnLaneTotal;
+        data->mfDisToLaneLeft = xPlan[i].mfDisToLaneLeft;
+        data->mfDisToRoadLeft = xPlan[i].mfDisToRoadLeft;
+        data->mfLaneWidth = xPlan[i].mWidth;
+        data->mfRoadWidth = xPlan[i].mfRoadWidth;
+
+        data->mode2 = xPlan[i].nSignal;
+        if(data->mode2 == 3000)
+        {
+            int k;
+            for(k=(mapdata.size()-1);k>(mapdata.size()-150);k--)
+            {
+                if(k<0)break;
+                mapdata.at(k)->mode2 = 3000;
+            }
+        }
 
         mapdata.push_back(data);
 

src/fusion/fusion_DRG/mainwindow.cpp

@@ -9,6 +9,7 @@
 #include <algorithm>
 MainWindow * mw;
 
+#define APOLLO_FU   //宏开关，undef重新编译则恢复原来状态
 
 void ListenFusion(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
 {
@@ -51,10 +52,11 @@ MainWindow::MainWindow(QWidget *parent) :
 
     //发送珊格图的结果
     drgFusion = iv::modulecomm::RegisterSend("drgfusion",160000*30,3);
+    pa = iv::modulecomm::RegisterSend("drg_image",160000*30,3);
 
     QTimer * timer = new QTimer(this);
     connect(timer,SIGNAL(timeout()),this,SLOT(onTimer()));
-    timer->start(1000);
+    timer->start(1000);  //1秒（1000毫秒）更新一次
 
     setWindowTitle("栅格图生成");
 
@@ -88,8 +90,8 @@ void MainWindow::generateDRG(iv::fusion::fusionobjectarray &arr){
     for(int i=0; i<arr.obj_size(); i++){
         //处理融合数据，分析每个障碍物特点
         iv::fusion::fusionobject obj = arr.obj(i);
-        iv::fusion::PointXYZ center = obj.centroid();
-        iv::fusion::Dimension space = obj.dimensions();
+        iv::fusion::PointXYZ center = obj.centroid();//中心点
+        iv::fusion::Dimension space = obj.dimensions();//尺寸
 
         //每个障碍物占据一片像素点
         //先确定左上角和右下角
@@ -176,7 +178,6 @@ void MainWindow::drawImage()
     brush.setStyle(Qt::SolidPattern);
     painter.setBrush(brush);
 
-
     int psize = drg.pixels_size();
     if(psize>1){
         for(int i=0; i<psize; i++){
@@ -190,12 +191,37 @@ void MainWindow::drawImage()
     painter.restore();
     painter.end();
 
+
     ui->imageLabel->setPixmap(QPixmap::fromImage(image));
 }
 
 void MainWindow::clearImage(){
     QImage image(QSize(200,800),QImage::Format_ARGB32);
     image.fill("gray");
+
+#ifdef APOLLO_FU
+    //image.fill("white");
+    QPainter painter(&image);
+    painter.setRenderHint(QPainter::Antialiasing);   //线条抗锯齿
+    painter.setRenderHint(QPainter::TextAntialiasing);
+
+    //painter.setPen(QColor(0, 160, 230));
+    painter.setPen(QColor(0, 250, 0));   //RGB配色表   http://www.wahart.com.hk/rgb.htm
+    for(int i=0;i<=80;i++){
+        painter.drawLine(0,i*10,200,i*10);
+    }
+    for(int i=0;i<=20;i++){
+        painter.drawLine(i*10,0,i*10,800);
+    }
+#endif
+
+#ifdef APOLLO_FU
+    int nsize = image.byteCount();
+    unsigned char *data  = image.bits();
+    iv::modulecomm::ModuleSendMsg(pa,(const char*)data,nsize);
+#endif
+
+
     ui->imageLabel->setPixmap(QPixmap::fromImage(image));
 }
 

src/fusion/fusion_DRG/mainwindow.h

@@ -43,6 +43,7 @@ private:
     void clearImage();
     void resizeEvent(QResizeEvent* size);
     void * drgFusion;
+    void * pa;
     bool isPreview = true;
 
 private slots:

src/fusion/test_DRG/main.cpp

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

src/fusion/test_DRG/mainwindow.cpp

@@ -0,0 +1,54 @@
+#include "mainwindow.h"
+#include "ui_mainwindow.h"
+#include <QImage>
+#include <iostream>
+
+QImage ximage;
+MainWindow * mw;
+
+char * gbuffer;
+void ListenImage(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
+{
+    std::cout<<nSize<<std::endl;
+    if(nSize<1) return;
+    if(nSize >= 640000)
+    {
+        memcpy(gbuffer,strdata,640000);
+    }
+    mw->newimage();
+}
+
+
+void MainWindow::UpdateImage(const char *strdata, const unsigned int nSize, const unsigned int index, const QDateTime *dt, const char *strmemname)
+{
+    if(nSize >= 640000)
+    {
+        ximage = QImage ((uchar*)strdata,200,800,QImage::Format_ARGB32);
+        ui->label->setPixmap(QPixmap::fromImage(ximage));
+    }
+}
+
+void MainWindow::newimage()
+{
+    emit updateimage();
+}
+
+void MainWindow::drawimage()
+{
+    ximage = QImage ((uchar*)gbuffer,200,800,QImage::Format_ARGB32);
+    ui->label->setPixmap(QPixmap::fromImage(ximage));
+}
+MainWindow::MainWindow(QWidget *parent) :
+    QMainWindow(parent),
+    ui(new Ui::MainWindow)
+{
+    ui->setupUi(this);
+    gbuffer = new char[640000];
+    ModuleFun funradar =std::bind(&MainWindow::UpdateImage,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
+    void * pa = iv::modulecomm::RegisterRecvPlus("drg_image",funradar);
+}
+
+MainWindow::~MainWindow()
+{
+    delete ui;
+}

src/fusion/test_DRG/mainwindow.h

@@ -0,0 +1,31 @@
+#ifndef MAINWINDOW_H
+#define MAINWINDOW_H
+
+#include <QMainWindow>
+#include "modulecomm.h"
+
+namespace Ui {
+class MainWindow;
+}
+
+class MainWindow : public QMainWindow
+{
+    Q_OBJECT
+
+public:
+    explicit MainWindow(QWidget *parent = 0);
+    ~MainWindow();
+
+    void newimage();
+
+private slots:
+    void drawimage();
+signals:
+    void updateimage();
+private:
+    Ui::MainWindow *ui;
+
+    void UpdateImage(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname);
+};
+
+#endif // MAINWINDOW_H

src/fusion/test_DRG/mainwindow.ui

@@ -0,0 +1,54 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<ui version="4.0">
+ <class>MainWindow</class>
+ <widget class="QMainWindow" name="MainWindow">
+  <property name="geometry">
+   <rect>
+    <x>0</x>
+    <y>0</y>
+    <width>827</width>
+    <height>624</height>
+   </rect>
+  </property>
+  <property name="windowTitle">
+   <string>MainWindow</string>
+  </property>
+  <widget class="QWidget" name="centralWidget">
+   <widget class="QLabel" name="label">
+    <property name="geometry">
+     <rect>
+      <x>350</x>
+      <y>40</y>
+      <width>161</width>
+      <height>481</height>
+     </rect>
+    </property>
+    <property name="text">
+     <string>TextLabel</string>
+    </property>
+   </widget>
+  </widget>
+  <widget class="QMenuBar" name="menuBar">
+   <property name="geometry">
+    <rect>
+     <x>0</x>
+     <y>0</y>
+     <width>827</width>
+     <height>28</height>
+    </rect>
+   </property>
+  </widget>
+  <widget class="QToolBar" name="mainToolBar">
+   <attribute name="toolBarArea">
+    <enum>TopToolBarArea</enum>
+   </attribute>
+   <attribute name="toolBarBreak">
+    <bool>false</bool>
+   </attribute>
+  </widget>
+  <widget class="QStatusBar" name="statusBar"/>
+ </widget>
+ <layoutdefault spacing="6" margin="11"/>
+ <resources/>
+ <connections/>
+</ui>

src/fusion/test_DRG/test_DRG.pro

@@ -0,0 +1,38 @@
+#-------------------------------------------------
+#
+# Project created by QtCreator 2021-03-17T15:20:57
+#
+#-------------------------------------------------
+
+QT       += core gui
+
+greaterThan(QT_MAJOR_VERSION, 4): QT += widgets
+
+TARGET = test_DRG
+TEMPLATE = app
+
+# The following define makes your compiler emit warnings if you use
+# any feature of Qt which has been marked as deprecated (the exact warnings
+# depend on your compiler). Please consult the documentation of the
+# deprecated API in order to know how to port your code away from it.
+DEFINES += QT_DEPRECATED_WARNINGS
+
+# You can also make your code fail to compile if you use deprecated APIs.
+# In order to do so, uncomment the following line.
+# You can also select to disable deprecated APIs only up to a certain version of Qt.
+#DEFINES += QT_DISABLE_DEPRECATED_BEFORE=0x060000    # disables all the APIs deprecated before Qt 6.0.0
+QMAKE_LFLAGS += -no-pie
+
+SOURCES += \
+        main.cpp \
+        mainwindow.cpp
+
+HEADERS += \
+        mainwindow.h
+
+FORMS += \
+        mainwindow.ui
+
+
+INCLUDEPATH += $$PWD/../../../include/
+LIBS += -L$$PWD/../../../bin/ -lxmlparam -lmodulecomm -livlog -livfault

src/include/proto/scheduler.proto

@@ -0,0 +1,20 @@
+syntax = "proto2";
+
+package iv;
+
+message scheduler_unit
+{
+	optional int32 mtype = 1; // 0 
+	required double mflat = 2;
+	required double mflon  = 3;
+	required double mfheading = 4;
+	required int32 mStopSecond = 5;  //Stop second time
+};
+
+message scheduler
+{
+	repeated scheduler_unit mscheduler_unit = 1;
+	required int32 mcyble = 2;
+};
+
+

src/include/proto/switch.proto

@@ -0,0 +1,243 @@
+syntax = "proto2";
+
+package iv.Switch;
+
+//前视障碍物开关命令
+message CtrlCmd_FrontCamera_Obs{
+    required bool ID = 1;			//障碍物ID
+    required bool type = 2;			//障碍物类型
+    required bool confidenceDegree = 3;		//置信度
+    required bool state = 4;			//状态位
+    required bool lifeCycle = 5;			//生命周期
+    required bool route = 6;			//所处车道标志
+    required bool courseAngle = 7;		//航向角
+    required bool bearing = 8;			//方位
+    required bool size = 9;			//尺寸
+    required bool bearing_xyz = 10;		//方位（X/Y/Z 三轴信息）
+    required bool relativeSpeed = 11;		//相对速度（X/Y 两轴信息）
+    required bool absoluteSpeed = 12;		//绝对速度（X/Y两轴信息）
+    required bool relativeAcceleration = 13;	//相对加速度（X/Y 两轴信息）
+    required bool referencePoint = 14;		//参考点（目标中心点）
+    required bool contourPoint = 15;		//轮廓点
+    required bool framePoint = 16;		//外包框顶点
+    required bool obstacleMotionPrediction = 17;	//障碍物运动预测轨迹
+};
+
+//前视车道线开关命令
+message  CtrlCmd_FrontCamera_Laneline{
+    required bool No = 1;		//车道线编号
+    required bool type = 2;		//车道线类型
+    required bool color = 3;		//车道线颜色
+    required bool curvature = 4;	//车道线曲率
+    required bool geometryInf = 5;	//车道线几何信息
+};
+
+//前视交通标线开关命令
+message CtrlCmd_FrontCamera_Trafficline{
+    required bool type = 1;	//类型信息
+    required bool posInf = 2;	//方位信息包含(关键点坐标,附着车道 ID)
+};
+
+//前视可行驶区域开关命令
+message  CtrlCmd_FrontCamera_DriveableArea{
+    required bool type = 1;		//类型信息
+    required bool boundPoint = 2;	//可行驶域拟合信息
+};
+
+//前视交通标志开关命令
+message CtrlCmd_FrontCamera_TrafficSign{
+    required bool type = 1;	//类型信息
+    required bool posInf = 2;	//方位信息包含(关键点坐标,附着车道 ID)
+};
+
+//前视交通信号灯开关命令
+message CtrlCmd_FrontCamera_TrafficLight{
+    required bool type = 1;   	//交通信号灯状态信息
+    required bool posInf = 2; 	//方位信息包含(关键点坐标,附着车道 ID)
+};
+
+//环视车位信息开关命令
+message CtrlCmd_AroundCamera_Carpos{
+    required bool carAngle = 1;               		//车位角点坐标
+    required bool carWidhtDeep = 2;           	//车位宽度/深度
+    required bool carconfidenceDegree = 3;    	//置信度
+};
+
+//环视障碍物信息开关命令
+message CtrlCmd_AroundCamera_Obs{
+    required bool type = 1;			//障碍物类型信息
+    required bool posinf = 2;			//障碍物置信度
+    required bool confidenceDegree = 3;		//障碍物方位信息
+};
+
+//毫米波雷达障碍物信息开关命令
+message CtrlCmd_MWRader_Obs{
+    required bool ID = 1;			//障碍物ID
+    required bool confidenceDegree = 2;		//置信度
+    required bool state = 3;			//状态位
+    required bool lifeCycle = 4;			//生命周期
+    required bool courseAngle = 5;		//航向角
+    required bool bearing = 6;			//方位
+    required bool relativeSpeed = 7;		//相对速度
+    required bool absoluteSpeed = 8;		//绝对速度
+    required bool relativeAcceleration = 9;	//相对加速度
+    required bool obstacleMotionPrediction = 10;	//障碍物运动预测轨迹
+};
+
+//激光雷达车道线信息开关命令
+message CtrlCmd_LRader_Laneline{
+    required bool No = 1;		//车道线 ID
+    required bool geometryInf = 2;	//车道线空间坐标信息
+};
+
+//激光雷达障碍物信息开关命令
+message CtrlCmd_LRader_Obs{
+    required bool ID = 1;			//障碍物 ID
+    required bool type = 2;			//类型
+    required bool confidenceDegree = 3;		//置信度
+    required bool state = 4;			//状态位
+    required bool lifeCycle = 5;			//生命周期
+    required bool route = 6;			//所处车道标志
+    required bool courseAngle = 7;		//航向角
+    required bool size = 8;			//尺寸
+    required bool bearing_xyz = 9;		//方位（X/Y/Z 三轴信息）
+    required bool relativeSpeed = 10;		//相对速度
+    required bool absoluteSpeed = 11;		//绝对速度
+    required bool relativeAcceleration = 12;	//相对加速度
+    required bool referencePoint = 13;		//参考点（目标中心点）
+    required bool contourPoint = 14;		//轮廓点
+    required bool framePoint = 15;		//外包框顶点
+    required bool obstacleMotionPrediction = 16;	//障碍物运动预测轨迹
+};
+
+//感知融合毫米波激光雷达前视融合信息开关命令
+message CtrlCmd_MWLRaderFrontCamera{
+    required bool ID = 1;			//障碍物ID
+    required bool type = 2;			//类型
+    required bool confidenceDegree = 3;		//置信度
+    required bool state = 4;			//状态位
+    required bool lifeCycle = 5;		//生命周期
+    required bool route = 6;			//所处车道标志
+    required bool courseAngle = 7;		//航向角
+    required bool bearing = 8;			//方位
+    required bool size = 9;			//尺寸
+    required bool relativeSpeed = 10;		//相对速度（X/Y 两轴信息）
+    required bool absoluteSpeed = 11;		//绝对速度（X/Y两轴信息）
+    required bool relativeAcceleration = 12;	//相对加速度（X/Y 两轴信息）
+    required bool referencePoint = 13;		//参考点（目标中心点）
+    required bool contourPoint = 14;		//轮廓点
+    required bool framePoint = 15;		//外包框顶点
+    required bool obstacleMotionPrediction = 16;	//障碍物运动预测轨迹
+};
+
+//毫米波雷达前视融合信息开关命令
+message CtrlCmd_MWRaderFrontCamera{
+    required bool ID = 1;			//障碍物ID
+    required bool type = 2;			//类型
+    required bool confidenceDegree = 3;		//置信度
+    required bool state = 4;			//状态位
+    required bool lifeCycle = 5;		//生命周期
+    required bool route = 6;			//所处车道标志
+//    required bool courseAngle = 7;		//航向角
+    required bool bearing = 8;			//方位
+//    required bool size = 9;			//尺寸
+    required bool relativeSpeed = 10;		//相对速度（X/Y 两轴信息）
+    required bool absoluteSpeed = 11;		//绝对速度（X/Y两轴信息）
+    required bool relativeAcceleration = 12;	//相对加速度（X/Y 两轴信息）
+    required bool referencePoint = 13;		//参考点（目标中心点）
+    required bool contourPoint = 14;		//轮廓点
+    required bool framePoint = 15;		//外包框顶点
+    required bool obstacleMotionPrediction = 16;	//障碍物运动预测轨迹
+};
+
+//超声波雷达环视融合信息开关命令
+message CtrlCmd_URaderAroundCamera{
+    required bool carAngle = 1;		//车位角点坐标
+    required bool carWidhtDeep = 2;		//车位宽度/深度
+    required bool carconfidenceDegree = 3;	//车位置信度
+    required bool obstype = 4;			//障碍物类型信息
+    required bool obsposinf = 5;		//障碍物置信度
+    required bool obsconfidenceDegree = 6;	//障碍物方位信息
+};
+
+// 环境建模开关命令
+message CtrlCmd_EnvironmentModeling{
+    required bool grid = 1;	//栅格图
+};
+
+//融合定位模块开关命令
+message CtrlCmd_FusionPositioning{
+    required bool time = 1;		//时间信息
+    required bool state = 2;		//纬度、经度、海拔、定位状态
+    required bool distance = 3;		//距离左右侧车道线距离
+    required bool yawAngle = 4;	//偏航角
+    required bool angleOfPitch = 5;	//俯仰角
+    required bool rollAngle = 6;	//滚转角
+};
+
+//显示开关命令
+message  ToDispControlCmd{
+    required CtrlCmd_FrontCamera_Obs FrontCamera_Obs=1;                           		//前视障碍物开关命令
+    required CtrlCmd_FrontCamera_Laneline FrontCamera_Laneline=2;           		//前视车道线开关命令
+    required CtrlCmd_FrontCamera_Trafficline FrontCamera_Trafficline=3;      		//前视交通标线开关命令
+    required CtrlCmd_FrontCamera_DriveableArea FrontCamera_DriveableArea=4;  	//前视可行驶区域开关命令
+    required CtrlCmd_FrontCamera_TrafficSign FrontCamera_TrafficSign=5;      		//前视交通标志开关命令
+    required CtrlCmd_FrontCamera_TrafficLight FrontCamera_TrafficLight=6;    		//前视交通信号灯开关命令
+    required CtrlCmd_AroundCamera_Carpos AroundCamera_Carpos=7;              		//环视车位信息开关命令
+    required CtrlCmd_AroundCamera_Obs AroundCamera_Obs=8;                    		//环视障碍物信息开关命令
+    required CtrlCmd_MWRader_Obs MWRader_Obs=9;                              		//毫米波雷达障碍物信息开关命令
+    required CtrlCmd_LRader_Laneline LRader_Laneline=10;                      		//激光雷达车道线信息开关命令
+    required CtrlCmd_LRader_Obs LRader_Obs=11;                                			//激光雷达障碍物信息开关命令
+    required CtrlCmd_MWLRaderFrontCamera MWLRaderFrontCamera=12;              	//感知融合毫米波激光雷达前视融合信息开关命令
+    required CtrlCmd_MWRaderFrontCamera MWRaderFrontCamera=13;                	//毫米波雷达前视融合信息开关命令
+    required CtrlCmd_URaderAroundCamera URaderAroundCamera=14;                	//超声波雷达环视融合信息开关命令
+    required CtrlCmd_EnvironmentModeling EnvironmentModeling=15;              		//环境建模开关命令
+    required CtrlCmd_FusionPositioning FusionPositioning=16;                  		//融合定位模块开关命令
+};
+
+//数据解析层模块开关
+message dataAnalysisCmd
+{
+    required bool frontViewCameraDataAnalysisModule = 1;         	//前视三目摄像头数据解析模块
+    required bool ViewAroundCameraDataAnalysisModule = 2;        	//环视摄像头数据解析模块
+    required bool MWRDataAnalysisModule = 3;                     	//毫米波雷达数据解析模块
+    required bool lidarDataAnalysisModule = 4;                   		//激光雷达数据解析模块
+    required bool URDataAnalysisModule = 5;                      		//超声波雷达数据解析模块
+    required bool GpsImuDataAnalysisModule = 6;                  	//GPS/IMU 数据解析模块
+    required bool vehicleStatusDataAnalysisModule = 7;           	//车辆状态信息数据解析模块
+};
+
+//数据处理层模块开关
+message dataProcessingCmd
+{
+    required bool frontViewThreeEyeCamera = 1;                   //前视三目摄像头图像处理模块
+    required bool visualCamera = 2;                              	//环视摄像头图像处理模块
+    required bool MMWRadar = 3;                                  	//毫米波雷达数据处理模块
+    required bool laserRadar = 4;                                	//激光雷达点云数据处理模块
+};
+
+//感知融合层模块开关
+message perceptualFusion
+{
+    required bool  spaceRegistrationModule = 1;                 //空间配准模块
+    required bool  perceptualFusionModule = 2;                  //感知融合模块
+    required bool  fusionPositioningModule = 3;                  //融合定位模块
+    required bool  timeRegistrationModule = 4;                   //时间配准模块
+    required bool  environmentModelingModule = 5;         //环境建模模块
+    required bool  lidarAndMayConstructModule = 6;         //激光雷达即时定位与地图构建处理模块
+};
+
+//功能开关命令
+message fucSetControlCmd
+{
+    required dataAnalysisCmd DataAnalysis =1;         	//数据解析层模块
+    required dataProcessingCmd DataProcessing =2;     	//数据处理层模块
+    required perceptualFusion PerceptualFusion =3;    	//感知融合层模块
+};
+
+//系统设置命令
+message settingsCmd
+{
+    required fucSetControlCmd fucSetControl =1;   	//功能设置
+    required ToDispControlCmd dispSetControl =2;  	//显示设置
+};

src/tool/IVSysMan/IVSysMan.pro

@@ -45,6 +45,7 @@ contains(QMAKE_HOST.os, Windows){
 
 
 SOURCES += \
+    ../../include/msgtype/switch.pb.cc \
         main.cpp \
         mainwindow.cpp \
     sysman.cpp \
@@ -58,6 +59,7 @@ SOURCES += \
     progunit.cpp
 
 HEADERS += \
+    ../../include/msgtype/switch.pb.h \
         mainwindow.h \
     sysman.h \
     progmon.h \

src/tool/IVSysMan/mainwindow.cpp

@@ -1,6 +1,8 @@
 #include "mainwindow.h"
 #include "ui_mainwindow.h"
 
+#include <iostream>
+
 extern std::string gstrxmlpath;
 
 
@@ -10,6 +12,10 @@ extern iv::Ivlog  * ivlog;
 static bool gbupdate =  false;
 static std::string gstrivsyspath;
 
+static iv::Switch::settingsCmd  gsettingcmd;
+static bool gbupdatesetting = false;
+QMutex gMutexSetting;
+
 
 
 void ListenChange(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
@@ -23,6 +29,24 @@ void ListenChange(const char * strdata,const unsigned int nSize,const unsigned i
 
 }
 
+void ListenSysSwitch(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
+{
+    iv::Switch::settingsCmd x;
+    if(!x.ParseFromArray(strdata,nSize))
+    {
+        std::cout<<"ListenSysSwitch Parse error."<<std::endl;
+        return;
+    }
+    gMutexSetting.lock();
+    gsettingcmd.CopyFrom(x);
+    gbupdatesetting = true;
+    gMutexSetting.unlock();
+    (void)&index;
+    (void)dt;
+    (void)strmemname;
+
+}
+
 
 MainWindow::MainWindow(QWidget *parent) :
     QMainWindow(parent),
@@ -89,9 +113,14 @@ MainWindow::MainWindow(QWidget *parent) :
     mpivexit = iv::ivexit::RegIVExitCmd();
 
     mpa = iv::modulecomm::RegisterRecv("ivsyschange",ListenChange);
+    mpaswitch = iv::modulecomm::RegisterRecv("sys_switch",ListenSysSwitch);
     QTimer * timerchange = new QTimer(this);
     connect(timerchange,SIGNAL(timeout()),this,SLOT(onTimerChangeXML()));
     timerchange->start(100);
+
+    QTimer * timerswitch = new QTimer(this);
+    connect(timerswitch,SIGNAL(timeout()),this,SLOT(onTimerSysSwitch()));
+    timerswitch->start(50);
 }
 
 MainWindow::~MainWindow()
@@ -239,4 +268,79 @@ void MainWindow::ChangeXML(std::string strxmlpath)
 //    pLabel->setGeometry(30,30,100,30);
 }
 
+void MainWindow::ProcdataAnalysisCmd(const iv::Switch::dataAnalysisCmd *pdataAnalysisCmd)
+{
+    if(pdataAnalysisCmd->mwrdataanalysismodule())
+    {
+        mPM->StartProc("driver_can_nvidia_agx");
+    }
+    else
+    {
+        mPM->StopProc("driver_can_nvidia_agx");
+    }
+    if(pdataAnalysisCmd->lidardataanalysismodule())
+    {
+        mPM->StartProc("driver_lidar_vlp32c");
+        mPM->StartProc("driver_lidar_vlp16","-s vlp16_left.yaml -n driver_lidar_vlp32_l");
+        mPM->StartProc("driver_lidar_vlp16","-s vlp16_right.yaml -n driver_lidar_vlp32_r");
+    }
+    else
+    {
+        mPM->StopProc("driver_lidar_vlp32c");
+        mPM->StopProc("driver_lidar_vlp16","-s vlp16_left.yaml -n driver_lidar_vlp32_l");
+        mPM->StopProc("driver_lidar_vlp16","-s vlp16_right.yaml -n driver_lidar_vlp32_r");
+    }
+    if(pdataAnalysisCmd->gpsimudataanalysismodule())
+    {
+        mPM->StartProc("detection_state_delphi_ins500d");
+    }
+    else
+    {
+        mPM->StartProc("detection_state_delphi_ins500d");
+    }
+}
+
+void MainWindow::ProcperceptualFusion(const iv::Switch::perceptualFusion *pperceptualFusion)
+{
+    if(pperceptualFusion->spaceregistrationmodule())
+    {
+        mPM->StartProc("driver_lidar_merge");
+    }
+    else
+    {
+        mPM->StopProc("driver_lidar_merge");
+    }
+    if(pperceptualFusion->fusionpositioningmodule())
+    {
+        mPM->StartProc("detection_ndt_matching_gpu_multi");
+    }
+    else
+    {
+        mPM->StopProc("detection_ndt_matching_gpu_multi");
+    }
+    if(pperceptualFusion->lidarandmayconstructmodule())
+    {
+        mPM->StartProc("adcndtmultimapping");
+    }
+    else
+    {
+        mPM->StopProc("adcndtmultimapping");
+    }
+}
+
+void MainWindow::onTimerSysSwitch()
+{
+    if(gbupdatesetting == false)return;
+    iv::Switch::settingsCmd pswitch;
+
+    gMutexSetting.lock();
+    pswitch.CopyFrom(gsettingcmd);
+    gbupdatesetting = false;
+    gMutexSetting.unlock();
+
+    iv::Switch::fucSetControlCmd xcmd = pswitch.fucsetcontrol();
+    ProcdataAnalysisCmd(&xcmd.dataanalysis());
+    ProcperceptualFusion(&xcmd.perceptualfusion());
+}
+
 

src/tool/IVSysMan/mainwindow.h

@@ -18,6 +18,8 @@
 
 #include "modulecomm.h"
 
+#include "switch.pb.h"
+
 namespace Ui {
 class MainWindow;
 }
@@ -54,6 +56,8 @@ private slots:
 
     void onTimerChangeXML();
 
+    void onTimerSysSwitch();
+
 
 
 private:
@@ -69,6 +73,8 @@ private:
 
     void * mpivexit;
 
+    void * mpaswitch;
+
     int mnTestIndex = 0;
 
     void * mpa;
@@ -76,6 +82,9 @@ private:
 public:
     void ChangeXML(std::string strxmlpath);
 
+    void ProcdataAnalysisCmd(const iv::Switch::dataAnalysisCmd * pdataAnalysisCmd);
+    void ProcperceptualFusion(const iv::Switch::perceptualFusion * pperceptualFusion);
+
 
 
     //add tjc

src/tool/IVSysMan/progmon.cpp

@@ -318,8 +318,68 @@ void ProgMon::restartProc(ProgUnit *pu){
     ivlog->info("start program: AppGroup - %s; AppDir - %s; AppName - %s; StartArgs - %s;", pu->strgroup.c_str(), pu->strappdir.c_str(), pu->strappname.c_str(), pu->strargs.c_str());
 }
 
+ProgUnit * ProgMon::FindProcByName(std::string strappname, std::string strargs)
+{
+    ProgUnit * pu = 0;
+    int i;
+    int nsize =  mvectorprog.size();
+    for(i=0;i<nsize;i++)
+    {
+        ProgUnit * putem = &mvectorprog.at(i);
+        if(strargs.size()<1)
+        {
+            if(strappname == putem->strappname)
+            {
+                pu = putem;
+                break;
+            }
+        }
+        else
+        {
+            if((strappname == putem->strappname)&&(strargs == putem->strargs))
+            {
+                pu = putem;
+                break;
+            }
+        }
+    }
+
+    return pu;
+}
+
+void ProgMon::StartProc(std::string strappname, std::string strargs)
+{
+    ProgUnit * pu = FindProcByName(strappname,strargs);
+
+    if(pu == 0)
+    {
+        qDebug("StartProc can't find app = %s args = %s",strappname.data(),strargs.data());
+        return;
+    }
+
+    StartProc(pu);
+}
+
+void ProgMon::StopProc(std::string strappname, std::string strargs)
+{
+    ProgUnit * pu = FindProcByName(strappname,strargs);
+
+    if(pu == 0)
+    {
+        qDebug("StopProc can't find app = %s args = %s",strappname.data(),strargs.data());
+        return;
+    }
+
+    StopProc(pu);
+}
+
 void ProgMon::StartProc(ProgUnit *pu)
 {
+    if(pu->mbRun)
+    {
+        qDebug("process %s is running. not need start.",pu->strappname.data());
+        return;
+    }
     pu->mProcess = new QProcess(this);
 
     connect(pu->mProcess,SIGNAL(started()),this,SLOT(onProcessStarted()));
@@ -352,6 +412,11 @@ void ProgMon::StopProc(ProgUnit *pu)
 {
     if(pu == 0)return;
     if(pu->mProcess == 0)return;
+    if(!pu->mbRun)
+    {
+        qDebug("process %s is not running. not need stop.",pu->strappname.data());
+        return;
+    }
     pu->mProcess->kill();
     if(!checkStartState(pu)){
         return;

src/tool/IVSysMan/progmon.h

@@ -62,9 +62,14 @@ public:
 
     std::vector<ProgUnit> loadprogunit(std::string path);
 
+    void StartProc(std::string strappname,std::string strargs="");
+    void StopProc(std::string strappname,std::string strargs="");
+
 private:
     QMutex mMutex;
 
+    ProgUnit * FindProcByName(std::string strappname,std::string strargs);
+
 signals:
     /* Signal when proc started */
     void SigProcStarted(ProgUnit * pu);

src/tool/adciv_replay/mainwindow.cpp

@@ -162,7 +162,7 @@ void MainWindow::onTimerReplay()
     QDateTime dtx = QDateTime::currentDateTime();
 
     qint64 timediff = mdtrpstart.msecsTo(dtx);
-    qDebug("diff is %d",timediff);
+ //   qDebug("diff is %d",timediff);
 
     bool bR= true;
 

src/tool/tool_scheduler/.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
+

src/tool/tool_scheduler/Readme.md

@@ -0,0 +1,3 @@
+1.本工程用于调度。
+2.设置调度的方法有两种，一种时接收scheduler.proto格式的消息，另外一种是从界面里选取xml文件。
+3.调度是单元是一系列的经纬度和停留时间，参考本工程的xml文件。

src/tool/tool_scheduler/gnss_coordinate_convert.cpp

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

src/tool/tool_scheduler/gnss_coordinate_convert.h

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

src/tool/tool_scheduler/ivscheduler.cpp

@@ -0,0 +1,257 @@
+#include "ivscheduler.h"
+
+#include <iostream>
+
+
+static ivscheduler * givs;
+
+static QMutex gMutexGPSIMU;
+
+static iv::gps::gpsimu ggpsimu;
+static bool gbUpdate = false;
+
+class xodrobj
+{
+public:
+    double flatsrc;
+    double flonsrc;
+    double fhgdsrc;
+    double flat;
+    double flon;
+    int lane;
+};
+
+
+void Listengpsimu(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
+{
+//        ggpsimu->UpdateGPSIMU(strdata,nSize);
+
+    iv::gps::gpsimu xgpsimu;
+    if(!xgpsimu.ParseFromArray(strdata,nSize))
+    {
+        std::cout<<"Listengpsimu Parse error."<<std::endl;
+        return;
+    }
+    (void)&index;
+    (void)dt;
+    (void)strmemname;
+
+    gMutexGPSIMU.lock();
+    ggpsimu.CopyFrom(xgpsimu);
+    gbUpdate = true;
+    gMutexGPSIMU.unlock();
+}
+
+void Listenscheduler(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
+{
+//        ggpsimu->UpdateGPSIMU(strdata,nSize);
+
+    iv::scheduler pscheduler;
+    if(!pscheduler.ParseFromArray(strdata,nSize))
+    {
+        std::cout<<"Listenscheduler Parse error."<<std::endl;
+        return;
+    }
+
+    (void)&index;
+    (void)dt;
+    (void)strmemname;
+
+    givs->setscheduler(&pscheduler);
+
+}
+
+ivscheduler::ivscheduler(std::string strmemname,std::string strschedulername)
+{
+    givs = this;
+    mstrmemname = strmemname;
+    mstrschedulername = strschedulername;
+    iv::modulecomm::RegisterRecv(mstrmemname.data(),Listengpsimu);
+    iv::modulecomm::RegisterRecv(mstrschedulername.data(),Listenscheduler);
+    mpadst = iv::modulecomm::RegisterSend("xodrreq",1000,1);
+}
+
+void ivscheduler::run()
+{
+    iv::gps::gpsimu xgpsimu;
+    while(!QThread::isInterruptionRequested())
+    {
+        if(gbUpdate)
+        {
+            gMutexGPSIMU.lock();
+            xgpsimu.CopyFrom(ggpsimu);
+            gbUpdate = false;
+            gMutexGPSIMU.unlock();
+            emit updategps(xgpsimu.lon(),xgpsimu.lat(),xgpsimu.heading());
+            if(mpscheduler == 0)continue;
+            mMutex.lock();
+            if(mncycle<mncyclecount)
+            {
+                if(mnprocess<mpscheduler->mscheduler_unit_size())
+                {
+                    switch (mnstep) {
+                    case 0:
+                    {
+                        mfLatInit = xgpsimu.lat();
+                        mfLonInit = xgpsimu.lon();
+                        mfHeadingInit = xgpsimu.heading();
+                        mnTimeInit = QDateTime::currentMSecsSinceEpoch();
+
+                        //Sending Obj;
+                        iv::scheduler_unit * pscheduler_unit = mpscheduler->mutable_mscheduler_unit(mnprocess);
+                        mfLatObj = pscheduler_unit->mflat();
+                        mfLonObj = pscheduler_unit->mflon();
+                        mfHeadingObj = pscheduler_unit->mfheading();
+                        mnLastSendObj = QDateTime::currentMSecsSinceEpoch();
+                        SendObj(mfLonObj,mfLatObj);
+                        mnstep = 1;
+                        emit updatestep(mnstep);
+                    }
+                        break;
+                    case 1:
+                        if(IsVehicleMoving(&xgpsimu))
+                        {
+                            mnstep = 2;
+                            emit updatestep(mnstep);
+                        }
+                        else
+                        {
+                            if((QDateTime::currentMSecsSinceEpoch() - mnLastSendObj) > 3000)
+                            {
+                                //Send Obj
+                                SendObj(mfLonObj,mfLatObj);
+                                mnLastSendObj = QDateTime::currentMSecsSinceEpoch();
+                            }
+                        }
+                        if(IsVehcileArrivingStation(&xgpsimu))
+                        {
+                            mnArrivingTime = QDateTime::currentMSecsSinceEpoch();
+                            mnstep = 3;
+                            emit updatestep(mnstep);
+                        }
+                        //Decide Vechicle Running.
+                        break;
+                    case 2:
+                        if(IsVehcileArrivingStation(&xgpsimu))
+                        {
+                            mnArrivingTime = QDateTime::currentMSecsSinceEpoch();
+                            mnstep = 3;
+                            emit updatestep(mnstep);
+                        }
+                        //Decide Vehiclde Arrive End Point;
+                        break;
+                    case 3:
+                    {
+                        iv::scheduler_unit * pscheduler_unit = mpscheduler->mutable_mscheduler_unit(mnprocess);
+                        if((QDateTime::currentMSecsSinceEpoch() - mnArrivingTime)>= (pscheduler_unit->mstopsecond()*1000) )
+                        {
+                            mnstep = 4;
+                            emit updatestep(mnstep);
+                        }
+                    }
+                        //Count Stop Time;
+                        break;
+                    case 4:
+                        mnstep = 0;
+                        emit updatestep(mnstep);
+                        mnprocess++;
+                        break;
+                    default:
+                        break;
+                    }
+                }
+                else
+                {
+                    mncycle++;
+                    mnprocess = 0;
+                }
+            }
+            emit updatestate(mncycle,mncyclecount,mnprocess,mpscheduler->mscheduler_unit_size());
+            mMutex.unlock();
+
+
+        }
+        else
+        {
+            msleep(10);
+        }
+    }
+}
+
+void ivscheduler::setscheduler(iv::scheduler *pscheduler)
+{
+    mMutex.lock();
+    if(mpscheduler != 0)delete mpscheduler;
+    mpscheduler = new iv::scheduler;
+    mpscheduler->CopyFrom(*pscheduler);
+    mncyclecount = pscheduler->mcyble();
+    mncycle = 0;
+    mnprocess = 0;
+    mnstep = 0;
+    mMutex.unlock();
+
+}
+
+void ivscheduler::GetProcess(int &nProc, int &nProcTotal)
+{
+    if(mpscheduler == 0)
+    {
+        nProc = 0;
+        nProcTotal = 0;
+        return;
+    }
+
+    mMutex.lock();
+    nProcTotal = mpscheduler->mscheduler_unit_size();
+    nProc = mnprocess;
+    mMutex.unlock();
+}
+
+void ivscheduler::GetCycle(int &ncycle, int &ncyclecount)
+{
+    ncycle = mncycle;
+    ncyclecount = mncyclecount;
+
+}
+
+bool ivscheduler::IsVehicleMoving(iv::gps::gpsimu * pgpsimu)
+{
+    double dis;
+    double x1,y1;
+    double x2,y2;
+    GaussProjCal(mfLonInit,mfLatInit,&x1,&y1);
+    GaussProjCal(pgpsimu->lon(),pgpsimu->lat(),&x2,&y2);
+    dis = sqrt(pow(x1-x2,2)+pow(y1-y2,2));
+    if(dis > 10.0)return true;
+
+    return false;
+}
+
+bool ivscheduler::IsVehcileArrivingStation(iv::gps::gpsimu *pgpsimu)
+{
+    double dis;
+    double x1,y1;
+    double x2,y2;
+    GaussProjCal(mfLonObj,mfLatObj,&x1,&y1);
+    GaussProjCal(pgpsimu->lon(),pgpsimu->lat(),&x2,&y2);
+    dis = sqrt(pow(x1-x2,2)+pow(y1-y2,2));
+    if(dis < 10.0)
+    {
+        double fheaddiff = pgpsimu->heading() - mfHeadingObj;
+        if(fheaddiff< 0)fheaddiff = fheaddiff + 360.0;
+        if((fheaddiff>300)||(fheaddiff<60))
+            return true;
+    }
+
+    return false;
+}
+
+void ivscheduler::SendObj(double flon, double flat)
+{
+    xodrobj xo;
+    xo.flon = flon;
+    xo.flat = flat;
+    xo.lane = 3;
+
+    iv::modulecomm::ModuleSendMsg(mpadst,(char *)&xo,sizeof(xodrobj));
+}

src/tool/tool_scheduler/ivscheduler.h

@@ -0,0 +1,67 @@
+#ifndef IVSCHEDULER_H
+#define IVSCHEDULER_H
+
+#include <QThread>
+#include <QMutex>
+#include <QDateTime>
+
+#include "scheduler.pb.h"
+#include "gpsimu.pb.h"
+#include "gnss_coordinate_convert.h"
+#include "modulecomm.h"
+
+
+class ivscheduler : public QThread
+{
+        Q_OBJECT
+public:
+    ivscheduler(std::string strmemname = "hcp2_gpsimu",std::string strschedulername = "scheduler");
+
+
+public:
+    void setscheduler(iv::scheduler * pscheduler);
+    void run();
+
+    void GetProcess(int & nProc, int & nProcTotal);
+    void GetCycle(int & ncycle, int & ncyclecount);
+
+signals:
+    void updategps(double flon,double flat,double fheading);
+    void updatestate(int ncycle,int ncyclecount,int nprocess,int nprocesscount);
+    void updatestep(int nstep);
+
+
+private:
+    iv::scheduler * mpscheduler = 0;
+    int mnprocess = 0;
+    int mncycle = 0;
+
+    int mncyclecount = 1;
+    int mnstep = 0;
+    double mfLatInit;
+    double mfLonInit;
+    double mfHeadingInit;
+
+    double mfLatObj;
+    double mfLonObj;
+    double mfHeadingObj;
+
+    qint64 mnTimeInit;
+    qint64 mnLastSendObj;
+    qint64 mnArrivingTime;
+
+
+    QMutex mMutex;
+
+    std::string mstrmemname;
+    std::string mstrschedulername;
+    void * mpadst;
+
+private:
+    bool IsVehicleMoving(iv::gps::gpsimu * pgpsimu);
+    bool IsVehcileArrivingStation(iv::gps::gpsimu * pgpsimu);
+
+    void SendObj(double flon,double flat);
+};
+
+#endif // IVSCHEDULER_H

src/tool/tool_scheduler/main.cpp

@@ -0,0 +1,32 @@
+#include "mainwindow.h"
+
+#include <QApplication>
+
+#include "xmlparam.h"
+
+std::string gstrgpsmsg;
+std::string gstrschmsg;
+
+int main(int argc, char *argv[])
+{
+    QApplication a(argc, argv);
+
+    std::string strparapath;
+    if(argc<2)
+    {
+
+        strparapath = "./tool_scheduler.xml";
+    }
+    else
+    {
+
+        strparapath = argv[2];
+    }
+
+    iv::xmlparam::Xmlparam xp(strparapath);
+    gstrgpsmsg = xp.GetParam("gpsmsg","hcp2_gpsimu");
+    gstrschmsg = xp.GetParam("schmsg","scheduler");
+    MainWindow w;
+    w.show();
+    return a.exec();
+}

src/tool/tool_scheduler/mainwindow.cpp

@@ -0,0 +1,178 @@
+#include "mainwindow.h"
+#include "ui_mainwindow.h"
+
+#include <QFileDialog>
+#include <QMessageBox>
+
+extern std::string gstrgpsmsg;
+extern std::string gstrschmsg;
+
+MainWindow::MainWindow(QWidget *parent)
+    : QMainWindow(parent)
+    , ui(new Ui::MainWindow)
+{
+    ui->setupUi(this);
+
+    mpivsch = new ivscheduler(gstrgpsmsg,gstrschmsg);
+    mpivsch->start();
+
+    connect(mpivsch,SIGNAL(updategps(double,double,double)),this,SLOT(onUpdatePos(double,double,double)));
+    connect(mpivsch,SIGNAL(updatestate(int,int,int,int)),this,SLOT(onUpdateState(int,int,int,int)));
+    connect(mpivsch,SIGNAL(updatestep(int)),this,SLOT(onUpdateStep(int)));
+
+    setWindowTitle("Scheduler");
+}
+
+MainWindow::~MainWindow()
+{
+    delete ui;
+}
+
+
+void MainWindow::on_pushButton_Start_clicked()
+{
+    if(mpscheduler == 0)
+    {
+        QMessageBox::warning(this,"warning","no scheduler select.");
+        return;
+    }
+
+
+    int ncycle = ui->lineEdit_Cycle->text().toInt();
+    if(ncycle<1)ncycle = 1;
+    mpscheduler->set_mcyble(ncycle);
+    mpivsch->setscheduler(mpscheduler);
+}
+
+void MainWindow::on_pushButton_Load_clicked()
+{
+    QString str = QFileDialog::getOpenFileName(this,"Load Scheduler",".","*.xml");
+    if(str.isEmpty())return;
+
+    ui->lineEdit_FilePath->setText(str);
+
+    QDomDocument doc;
+
+
+    QFile file(str.toLatin1().data());
+    if (!file.open(QIODevice::ReadOnly))
+    {
+        QMessageBox::warning(this,"warning","Load File Fail.");
+        return;
+    }
+
+    if (!doc.setContent(&file)) {
+        QMessageBox::warning(this,"waring","Parse XML Fail.");
+        file.close();
+        return ;
+    }
+    file.close();
+
+    //遍历节点，从配置文件中获取当前有哪些模块，并拼接启动命令和启动参数_tjc
+    QDomElement docElem = doc.documentElement();
+    QDomNode n = docElem.firstChild();
+
+    iv::scheduler * pscheduler = new iv::scheduler;
+
+    while(!n.isNull())
+    {
+        QDomElement e = n.toElement(); // 尝试将节点转换为元素
+        std::string name = e.nodeName().toStdString();
+        if(name == "scheduler_unit")
+        {
+            double flat,flon,fheading,fstoptime;
+            flat = -1000;
+            flon = -1000;
+            fheading = -1000;
+            fstoptime = -1000;
+            QDomNode nodesch = e.firstChild();
+            while(!nodesch.isNull())
+            {
+                QDomElement esch= nodesch.toElement(); // 尝试将节点转换为元素
+                QString strname = nodesch.nodeName();
+                QString strvalue = esch.text();
+                if(strname.toStdString() == "lat")
+                {
+                    flat = atof(strvalue.toStdString().data());
+                }
+                if(strname.toStdString() == "lon")
+                {
+                    flon = atof(strvalue.toStdString().data());
+                }
+                if(strname.toStdString() == "heading")
+                {
+                    fheading = atof(strvalue.toStdString().data());
+                }
+                if(strname.toStdString() == "stoptime")
+                {
+                    fstoptime = atof(strvalue.toStdString().data());
+                }
+                nodesch = nodesch.nextSibling();
+            }
+
+            if((flat == -1000)||(flat == -1000)||(flat == -1000)||(flat == -1000))
+            {
+                qDebug("decode a unit error.");
+            }
+            else
+            {
+                iv::scheduler_unit * pscheduler_unit = pscheduler->add_mscheduler_unit();
+                pscheduler_unit->set_mfheading(fheading);
+                pscheduler_unit->set_mflat(flat);
+                pscheduler_unit->set_mflon(flon);
+                pscheduler_unit->set_mstopsecond(fstoptime);
+            }
+
+
+        }
+        n = n.nextSibling();
+    }
+
+    if(pscheduler->mscheduler_unit_size() < 1)
+    {
+        QMessageBox::warning(this,"waring"," can't load scheduler from xml.");
+        return;
+    }
+
+    mpscheduler = pscheduler;
+}
+
+void MainWindow::onUpdatePos(double flon, double flat, double fheading)
+{
+    ui->lineEdit_Lon->setText(QString::number(flon,'f',7));
+    ui->lineEdit_Lat->setText(QString::number(flat,'f',7));
+    ui->lineEdit_Head->setText(QString::number(fheading,'f',2));
+}
+
+void MainWindow::onUpdateState(int ncycle, int ncyclecount, int nprocess, int nprocesscount)
+{
+    char strout[100];
+    snprintf(strout,100,"%d/%d",ncycle,ncyclecount);
+    ui->lineEdit_CycleProcess->setText(strout);
+    snprintf(strout,100,"%d/%d",nprocess,nprocesscount);
+    ui->lineEdit_Process->setText(strout);
+}
+
+void MainWindow::onUpdateStep(int nstep)
+{
+    switch(nstep)
+    {
+    case 0:
+        ui->lineEdit_Step->setText("Init...");
+        break;
+    case 1:
+        ui->lineEdit_Step->setText("Decide Vehcile Moving...");
+        break;
+    case 2:
+        ui->lineEdit_Step->setText("Go to Destination...");
+        break;
+    case 3:
+        ui->lineEdit_Step->setText("Arrived. Stoping...");
+        break;
+    case 4:
+        ui->lineEdit_Step->setText("Complete...");
+        break;
+    }
+
+
+}

src/tool/tool_scheduler/mainwindow.h

@@ -0,0 +1,39 @@
+#ifndef MAINWINDOW_H
+#define MAINWINDOW_H
+
+#include <QMainWindow>
+#include <QtXml>
+
+#include "scheduler.pb.h"
+
+#include "ivscheduler.h"
+
+QT_BEGIN_NAMESPACE
+namespace Ui { class MainWindow; }
+QT_END_NAMESPACE
+
+class MainWindow : public QMainWindow
+{
+    Q_OBJECT
+
+public:
+    MainWindow(QWidget *parent = nullptr);
+    ~MainWindow();
+
+private slots:
+    void on_pushButton_Start_clicked();
+
+    void on_pushButton_Load_clicked();
+
+    void onUpdatePos(double flon,double flat,double fheading);
+    void onUpdateState(int ncycle,int ncyclecount,int nprocess,int nprocesscount);
+    void onUpdateStep(int nstep);
+
+private:
+    Ui::MainWindow *ui;
+
+    iv::scheduler * mpscheduler = 0;
+
+    ivscheduler * mpivsch;
+};
+#endif // MAINWINDOW_H

src/tool/tool_scheduler/mainwindow.ui

@@ -0,0 +1,229 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<ui version="4.0">
+ <class>MainWindow</class>
+ <widget class="QMainWindow" name="MainWindow">
+  <property name="geometry">
+   <rect>
+    <x>0</x>
+    <y>0</y>
+    <width>800</width>
+    <height>600</height>
+   </rect>
+  </property>
+  <property name="windowTitle">
+   <string>MainWindow</string>
+  </property>
+  <widget class="QWidget" name="centralwidget">
+   <widget class="QPushButton" name="pushButton_Load">
+    <property name="geometry">
+     <rect>
+      <x>110</x>
+      <y>40</y>
+      <width>89</width>
+      <height>25</height>
+     </rect>
+    </property>
+    <property name="text">
+     <string>Load</string>
+    </property>
+   </widget>
+   <widget class="QPushButton" name="pushButton_Start">
+    <property name="geometry">
+     <rect>
+      <x>420</x>
+      <y>330</y>
+      <width>89</width>
+      <height>25</height>
+     </rect>
+    </property>
+    <property name="text">
+     <string>Start</string>
+    </property>
+   </widget>
+   <widget class="QLineEdit" name="lineEdit_Lon">
+    <property name="geometry">
+     <rect>
+      <x>300</x>
+      <y>140</y>
+      <width>113</width>
+      <height>25</height>
+     </rect>
+    </property>
+   </widget>
+   <widget class="QLineEdit" name="lineEdit_Lat">
+    <property name="geometry">
+     <rect>
+      <x>300</x>
+      <y>190</y>
+      <width>113</width>
+      <height>25</height>
+     </rect>
+    </property>
+   </widget>
+   <widget class="QLineEdit" name="lineEdit_Head">
+    <property name="geometry">
+     <rect>
+      <x>300</x>
+      <y>240</y>
+      <width>113</width>
+      <height>25</height>
+     </rect>
+    </property>
+   </widget>
+   <widget class="QLineEdit" name="lineEdit_FilePath">
+    <property name="geometry">
+     <rect>
+      <x>230</x>
+      <y>40</y>
+      <width>541</width>
+      <height>25</height>
+     </rect>
+    </property>
+   </widget>
+   <widget class="QLineEdit" name="lineEdit_Cycle">
+    <property name="geometry">
+     <rect>
+      <x>240</x>
+      <y>330</y>
+      <width>113</width>
+      <height>25</height>
+     </rect>
+    </property>
+   </widget>
+   <widget class="QLineEdit" name="lineEdit_CycleProcess">
+    <property name="geometry">
+     <rect>
+      <x>240</x>
+      <y>400</y>
+      <width>113</width>
+      <height>25</height>
+     </rect>
+    </property>
+   </widget>
+   <widget class="QLineEdit" name="lineEdit_Process">
+    <property name="geometry">
+     <rect>
+      <x>240</x>
+      <y>453</y>
+      <width>113</width>
+      <height>25</height>
+     </rect>
+    </property>
+   </widget>
+   <widget class="QLabel" name="label">
+    <property name="geometry">
+     <rect>
+      <x>160</x>
+      <y>140</y>
+      <width>67</width>
+      <height>17</height>
+     </rect>
+    </property>
+    <property name="text">
+     <string>Lon</string>
+    </property>
+   </widget>
+   <widget class="QLabel" name="label_2">
+    <property name="geometry">
+     <rect>
+      <x>160</x>
+      <y>200</y>
+      <width>67</width>
+      <height>17</height>
+     </rect>
+    </property>
+    <property name="text">
+     <string>Lat</string>
+    </property>
+   </widget>
+   <widget class="QLabel" name="label_3">
+    <property name="geometry">
+     <rect>
+      <x>160</x>
+      <y>250</y>
+      <width>67</width>
+      <height>17</height>
+     </rect>
+    </property>
+    <property name="text">
+     <string>Head</string>
+    </property>
+   </widget>
+   <widget class="QLabel" name="label_4">
+    <property name="geometry">
+     <rect>
+      <x>110</x>
+      <y>330</y>
+      <width>67</width>
+      <height>17</height>
+     </rect>
+    </property>
+    <property name="text">
+     <string>Cycle</string>
+    </property>
+   </widget>
+   <widget class="QLabel" name="label_5">
+    <property name="geometry">
+     <rect>
+      <x>110</x>
+      <y>410</y>
+      <width>81</width>
+      <height>17</height>
+     </rect>
+    </property>
+    <property name="text">
+     <string>CycleNum</string>
+    </property>
+   </widget>
+   <widget class="QLabel" name="label_6">
+    <property name="geometry">
+     <rect>
+      <x>110</x>
+      <y>460</y>
+      <width>67</width>
+      <height>17</height>
+     </rect>
+    </property>
+    <property name="text">
+     <string>Process</string>
+    </property>
+   </widget>
+   <widget class="QLineEdit" name="lineEdit_Step">
+    <property name="geometry">
+     <rect>
+      <x>240</x>
+      <y>504</y>
+      <width>401</width>
+      <height>25</height>
+     </rect>
+    </property>
+   </widget>
+   <widget class="QLabel" name="label_7">
+    <property name="geometry">
+     <rect>
+      <x>110</x>
+      <y>508</y>
+      <width>67</width>
+      <height>17</height>
+     </rect>
+    </property>
+    <property name="text">
+     <string>Step</string>
+    </property>
+   </widget>
+  </widget>
+  <widget class="QMenuBar" name="menubar">
+   <property name="geometry">
+    <rect>
+     <x>0</x>
+     <y>0</y>
+     <width>800</width>
+     <height>28</height>
+    </rect>
+   </property>
+  </widget>
+  <widget class="QStatusBar" name="statusbar"/>
+ </widget>
+ <resources/>
+ <connections/>
+</ui>

src/tool/tool_scheduler/scheduler.xml

@@ -0,0 +1,14 @@
+<xml>	
+	<scheduler_unit>
+		<lat>39.1</lat>
+		<lon>117.2</lon>
+		<heading>360</heading>
+		<stoptime>30</stoptime>
+	</scheduler_unit>
+	<scheduler_unit>
+		<lat>39.2</lat>
+		<lon>117.2</lon>
+		<heading>360</heading>
+		<stoptime>10</stoptime>
+	</scheduler_unit>
+</xml>

src/tool/tool_scheduler/tool_scheduler.pro

@@ -0,0 +1,51 @@
+QT       += core gui xml
+
+greaterThan(QT_MAJOR_VERSION, 4): QT += widgets
+
+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 += \
+    ../../include/msgtype/gps.pb.cc \
+    ../../include/msgtype/gpsimu.pb.cc \
+    ../../include/msgtype/imu.pb.cc \
+    ../../include/msgtype/scheduler.pb.cc \
+    gnss_coordinate_convert.cpp \
+    ivscheduler.cpp \
+    main.cpp \
+    mainwindow.cpp
+
+HEADERS += \
+    ../../include/msgtype/gps.pb.h \
+    ../../include/msgtype/gpsimu.pb.h \
+    ../../include/msgtype/imu.pb.h \
+    ../../include/msgtype/scheduler.pb.h \
+    gnss_coordinate_convert.h \
+    ivscheduler.h \
+    mainwindow.h
+
+FORMS += \
+    mainwindow.ui
+
+# 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/common.pri ) {
+    error( "Couldn't find the common.pri file!" )
+}
+
+!include(../../../include/ivprotobuf.pri ) {
+    error( "Couldn't find the ivprotobuf.pri file!" )
+}

src/tool/tool_system_debug_command/tool_system_debug_command/README.md

@@ -0,0 +1 @@
+集成Ubuntu下的调试命令  图形化展示  半成品，来活了后面有时间再完善。

src/tool/tool_system_debug_command/tool_system_debug_command/main.cpp

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

src/tool/tool_system_debug_command/tool_system_debug_command/mainwindow.cpp

@@ -0,0 +1,91 @@
+#include "mainwindow.h"
+#include "ui_mainwindow.h"
+
+MainWindow::MainWindow(QWidget *parent) :
+    QMainWindow(parent),
+    ui(new Ui::MainWindow)
+{
+    ui->setupUi(this);
+    cmd = new QProcess(this);
+    connect(cmd , SIGNAL(readyReadStandardOutput()) , this , SLOT(on_readoutput()));
+    connect(cmd , SIGNAL(readyReadStandardError()) , this , SLOT(on_readerror()));
+
+}
+
+MainWindow::~MainWindow()
+{
+    delete ui;
+    if(cmd)
+    {
+        cmd->close();
+        cmd->waitForFinished();
+    }
+}
+
+void MainWindow::on_readoutput()
+{
+    ui->textBrowser_2->setText(cmd->readAllStandardOutput().data());   //将输出信息读取到编辑框
+}
+
+void MainWindow::on_readerror()
+{
+    QMessageBox::information(0, "标准错误输出", cmd->readAllStandardError().data());    //弹出信息框提示错误信息
+}
+
+void MainWindow::on_pushButton_clicked()
+{
+    ui->textBrowser->setText("打印本系统发行版的所有信息，比如发行版的ID，描述信息，具体版本，代号。\n");
+    cmd->start("bash");                           //启动终端(Windows下改为cmd)
+    cmd->waitForStarted();                        //等待启动完成
+    cmd->write("lsb_release -a\n");               //向终端写入“lsb_release -a”命令，注意尾部的“\n”不可省略
+}
+
+
+void MainWindow::on_pushButton_2_clicked()
+{
+    ui->textBrowser->setText("最后一行打印的是编译器版本号。\n");
+    cmd->start("bash");                   //启动终端(Windows下改为cmd)
+    cmd->waitForStarted();                //等待启动完成
+    cmd->write("g++ -v\n");               //向终端写入“g++ -v”命令，注意尾部的“\n”不可省略
+}
+
+void MainWindow::on_pushButton_3_clicked()
+{
+    ui->textBrowser->setText("返回公网出口IP和地区。\n");
+    cmd->start("bash");             //启动终端(Windows下改为cmd)
+    cmd->waitForStarted();          //等待启动完成
+    cmd->write("curl cip.cc\n");               //向终端写入“curl cip.cc”命令，注意尾部的“\n”不可省略
+}
+
+void MainWindow::on_pushButton_4_clicked()
+{
+    ui->textBrowser->setText("Destination 此列指示目标网络。\
+                             Gateway 此列指示网络的已定义网关。 如果在此列中看到*，则表示指定的网络不需要转发网关。\
+                             Genmask 此列指示网络的网络掩码。\
+                             Flags 此列中的U输出表示路线已启动。 G输出表示应该为此路由使用指定的网关。 D代表动态安装，M代表修改，R代表恢复。\
+                             MSS 此列指示此路由的TCP连接的默认最大段大小（MSS）。\
+                             Window 此列指示此路由上TCP连接的默认窗口大小。\
+                             Irtt 此列指示此路线的初始往返时间。\
+                             Iface Iface列显示网络接口。 如果您有多个接口，您会看到lo（用于环回），eth0（第一个以太网设备）和eth1（用于第二个以太网设备），依此类推您已安装的接口数量。\n");
+    cmd->start("bash");             //启动终端(Windows下改为cmd)
+    cmd->waitForStarted();          //等待启动完成
+    cmd->write("netstat -rn\n");               //向终端写入“netstat -rn”命令，注意尾部的“\n”不可省略
+}
+
+void MainWindow::on_pushButton_5_clicked()
+{
+    ui->textBrowser->setText("打印路由表信息。\n");
+    cmd->start("bash");             //启动终端(Windows下改为cmd)
+    cmd->waitForStarted();          //等待启动完成
+    cmd->write("ip route\n");               //向终端写入“ip route”命令，注意尾部的“\n”不可省略
+}
+
+void MainWindow::on_pushButton_6_clicked()
+{
+    //nc -l 1234
+}
+
+void MainWindow::on_pushButton_7_clicked()
+{
+    //nc -u -l 1234
+}

src/tool/tool_system_debug_command/tool_system_debug_command/mainwindow.h

@@ -0,0 +1,42 @@
+#ifndef MAINWINDOW_H
+#define MAINWINDOW_H
+
+#include <QMainWindow>
+#include <QProcess>
+#include <QMessageBox>
+
+namespace Ui {
+class MainWindow;
+}
+
+class MainWindow : public QMainWindow
+{
+    Q_OBJECT
+
+public:
+    explicit MainWindow(QWidget *parent = 0);
+    ~MainWindow();
+    QProcess *cmd;
+
+private slots:
+    void on_pushButton_clicked();
+    void on_readoutput();
+    void on_readerror();
+
+    void on_pushButton_2_clicked();
+
+    void on_pushButton_3_clicked();
+
+    void on_pushButton_4_clicked();
+
+    void on_pushButton_5_clicked();
+
+    void on_pushButton_6_clicked();
+
+    void on_pushButton_7_clicked();
+
+private:
+    Ui::MainWindow *ui;
+};
+
+#endif // MAINWINDOW_H

src/tool/tool_system_debug_command/tool_system_debug_command/mainwindow.ui

@@ -0,0 +1,588 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<ui version="4.0">
+ <class>MainWindow</class>
+ <widget class="QMainWindow" name="MainWindow">
+  <property name="geometry">
+   <rect>
+    <x>0</x>
+    <y>0</y>
+    <width>1135</width>
+    <height>600</height>
+   </rect>
+  </property>
+  <property name="windowTitle">
+   <string>MainWindow</string>
+  </property>
+  <widget class="QWidget" name="centralWidget">
+   <widget class="QGroupBox" name="groupBox">
+    <property name="geometry">
+     <rect>
+      <x>30</x>
+      <y>30</y>
+      <width>341</width>
+      <height>501</height>
+     </rect>
+    </property>
+    <property name="title">
+     <string>操作系统</string>
+    </property>
+    <widget class="QLabel" name="label">
+     <property name="geometry">
+      <rect>
+       <x>10</x>
+       <y>30</y>
+       <width>101</width>
+       <height>17</height>
+      </rect>
+     </property>
+     <property name="text">
+      <string>查看系统版本</string>
+     </property>
+    </widget>
+    <widget class="QPushButton" name="pushButton">
+     <property name="geometry">
+      <rect>
+       <x>160</x>
+       <y>30</y>
+       <width>111</width>
+       <height>25</height>
+      </rect>
+     </property>
+     <property name="text">
+      <string>lsb_release -a</string>
+     </property>
+    </widget>
+    <widget class="QPushButton" name="pushButton_2">
+     <property name="geometry">
+      <rect>
+       <x>160</x>
+       <y>60</y>
+       <width>89</width>
+       <height>25</height>
+      </rect>
+     </property>
+     <property name="text">
+      <string>g++ -v</string>
+     </property>
+    </widget>
+    <widget class="QLabel" name="label_3">
+     <property name="geometry">
+      <rect>
+       <x>10</x>
+       <y>60</y>
+       <width>91</width>
+       <height>17</height>
+      </rect>
+     </property>
+     <property name="text">
+      <string>查看编译器</string>
+     </property>
+    </widget>
+    <widget class="QLabel" name="label_12">
+     <property name="geometry">
+      <rect>
+       <x>10</x>
+       <y>90</y>
+       <width>91</width>
+       <height>17</height>
+      </rect>
+     </property>
+     <property name="text">
+      <string>查看pci设备</string>
+     </property>
+    </widget>
+    <widget class="QLabel" name="label_13">
+     <property name="geometry">
+      <rect>
+       <x>10</x>
+       <y>120</y>
+       <width>91</width>
+       <height>17</height>
+      </rect>
+     </property>
+     <property name="text">
+      <string>查看usb设备</string>
+     </property>
+    </widget>
+    <widget class="QLabel" name="label_14">
+     <property name="geometry">
+      <rect>
+       <x>10</x>
+       <y>150</y>
+       <width>101</width>
+       <height>17</height>
+      </rect>
+     </property>
+     <property name="text">
+      <string>查看CPU信息</string>
+     </property>
+    </widget>
+    <widget class="QLabel" name="label_15">
+     <property name="geometry">
+      <rect>
+       <x>10</x>
+       <y>180</y>
+       <width>101</width>
+       <height>17</height>
+      </rect>
+     </property>
+     <property name="text">
+      <string>查看环境变量</string>
+     </property>
+    </widget>
+    <widget class="QLabel" name="label_16">
+     <property name="geometry">
+      <rect>
+       <x>10</x>
+       <y>210</y>
+       <width>141</width>
+       <height>17</height>
+      </rect>
+     </property>
+     <property name="text">
+      <string>列出加载的内核模块</string>
+     </property>
+    </widget>
+    <widget class="QLabel" name="label_17">
+     <property name="geometry">
+      <rect>
+       <x>10</x>
+       <y>250</y>
+       <width>111</width>
+       <height>17</height>
+      </rect>
+     </property>
+     <property name="text">
+      <string>查看内核版本</string>
+     </property>
+    </widget>
+    <widget class="QLabel" name="label_18">
+     <property name="geometry">
+      <rect>
+       <x>10</x>
+       <y>280</y>
+       <width>67</width>
+       <height>17</height>
+      </rect>
+     </property>
+     <property name="text">
+      <string>TextLabel</string>
+     </property>
+    </widget>
+    <widget class="QLabel" name="label_19">
+     <property name="geometry">
+      <rect>
+       <x>10</x>
+       <y>310</y>
+       <width>67</width>
+       <height>17</height>
+      </rect>
+     </property>
+     <property name="text">
+      <string>TextLabel</string>
+     </property>
+    </widget>
+    <widget class="QPushButton" name="pushButton_10">
+     <property name="geometry">
+      <rect>
+       <x>160</x>
+       <y>90</y>
+       <width>89</width>
+       <height>25</height>
+      </rect>
+     </property>
+     <property name="text">
+      <string>lspci -tv</string>
+     </property>
+    </widget>
+    <widget class="QPushButton" name="pushButton_11">
+     <property name="geometry">
+      <rect>
+       <x>160</x>
+       <y>120</y>
+       <width>89</width>
+       <height>25</height>
+      </rect>
+     </property>
+     <property name="text">
+      <string>lsusb -tv</string>
+     </property>
+    </widget>
+    <widget class="QPushButton" name="pushButton_12">
+     <property name="geometry">
+      <rect>
+       <x>160</x>
+       <y>150</y>
+       <width>131</width>
+       <height>25</height>
+      </rect>
+     </property>
+     <property name="text">
+      <string>cat /proc/cpuinfo</string>
+     </property>
+    </widget>
+    <widget class="QPushButton" name="pushButton_13">
+     <property name="geometry">
+      <rect>
+       <x>160</x>
+       <y>180</y>
+       <width>89</width>
+       <height>25</height>
+      </rect>
+     </property>
+     <property name="text">
+      <string>env</string>
+     </property>
+    </widget>
+    <widget class="QPushButton" name="pushButton_14">
+     <property name="geometry">
+      <rect>
+       <x>160</x>
+       <y>210</y>
+       <width>89</width>
+       <height>25</height>
+      </rect>
+     </property>
+     <property name="text">
+      <string>lsmod</string>
+     </property>
+    </widget>
+    <widget class="QPushButton" name="pushButton_15">
+     <property name="geometry">
+      <rect>
+       <x>160</x>
+       <y>240</y>
+       <width>131</width>
+       <height>25</height>
+      </rect>
+     </property>
+     <property name="text">
+      <string>uname -r</string>
+     </property>
+    </widget>
+    <widget class="QPushButton" name="pushButton_16">
+     <property name="geometry">
+      <rect>
+       <x>160</x>
+       <y>270</y>
+       <width>89</width>
+       <height>25</height>
+      </rect>
+     </property>
+     <property name="text">
+      <string>PushButton</string>
+     </property>
+    </widget>
+    <widget class="QPushButton" name="pushButton_17">
+     <property name="geometry">
+      <rect>
+       <x>160</x>
+       <y>300</y>
+       <width>89</width>
+       <height>25</height>
+      </rect>
+     </property>
+     <property name="text">
+      <string>PushButton</string>
+     </property>
+    </widget>
+   </widget>
+   <widget class="QTextBrowser" name="textBrowser">
+    <property name="geometry">
+     <rect>
+      <x>810</x>
+      <y>50</y>
+      <width>311</width>
+      <height>91</height>
+     </rect>
+    </property>
+   </widget>
+   <widget class="QLabel" name="label_2">
+    <property name="geometry">
+     <rect>
+      <x>810</x>
+      <y>30</y>
+      <width>67</width>
+      <height>17</height>
+     </rect>
+    </property>
+    <property name="text">
+     <string>说明</string>
+    </property>
+   </widget>
+   <widget class="QGroupBox" name="groupBox_2">
+    <property name="geometry">
+     <rect>
+      <x>410</x>
+      <y>200</y>
+      <width>371</width>
+      <height>171</height>
+     </rect>
+    </property>
+    <property name="title">
+     <string>网络</string>
+    </property>
+    <widget class="QLabel" name="label_4">
+     <property name="geometry">
+      <rect>
+       <x>10</x>
+       <y>30</y>
+       <width>131</width>
+       <height>17</height>
+      </rect>
+     </property>
+     <property name="text">
+      <string>查看系统对外IP</string>
+     </property>
+    </widget>
+    <widget class="QPushButton" name="pushButton_3">
+     <property name="geometry">
+      <rect>
+       <x>130</x>
+       <y>30</y>
+       <width>111</width>
+       <height>25</height>
+      </rect>
+     </property>
+     <property name="text">
+      <string>curl cip.cc</string>
+     </property>
+    </widget>
+    <widget class="QPushButton" name="pushButton_4">
+     <property name="geometry">
+      <rect>
+       <x>130</x>
+       <y>60</y>
+       <width>89</width>
+       <height>25</height>
+      </rect>
+     </property>
+     <property name="text">
+      <string>netstat -rn</string>
+     </property>
+    </widget>
+    <widget class="QLabel" name="label_5">
+     <property name="geometry">
+      <rect>
+       <x>10</x>
+       <y>60</y>
+       <width>91</width>
+       <height>17</height>
+      </rect>
+     </property>
+     <property name="text">
+      <string>查看路由表</string>
+     </property>
+    </widget>
+    <widget class="QPushButton" name="pushButton_5">
+     <property name="geometry">
+      <rect>
+       <x>250</x>
+       <y>60</y>
+       <width>89</width>
+       <height>25</height>
+      </rect>
+     </property>
+     <property name="text">
+      <string>ip route</string>
+     </property>
+    </widget>
+    <widget class="QLabel" name="label_8">
+     <property name="geometry">
+      <rect>
+       <x>10</x>
+       <y>100</y>
+       <width>101</width>
+       <height>17</height>
+      </rect>
+     </property>
+     <property name="text">
+      <string>TCP监听端口</string>
+     </property>
+    </widget>
+    <widget class="QLabel" name="label_9">
+     <property name="geometry">
+      <rect>
+       <x>10</x>
+       <y>130</y>
+       <width>101</width>
+       <height>17</height>
+      </rect>
+     </property>
+     <property name="text">
+      <string>UDP监听端口</string>
+     </property>
+    </widget>
+    <widget class="QLineEdit" name="lineEdit">
+     <property name="geometry">
+      <rect>
+       <x>110</x>
+       <y>100</y>
+       <width>71</width>
+       <height>25</height>
+      </rect>
+     </property>
+    </widget>
+    <widget class="QLineEdit" name="lineEdit_2">
+     <property name="geometry">
+      <rect>
+       <x>110</x>
+       <y>130</y>
+       <width>71</width>
+       <height>25</height>
+      </rect>
+     </property>
+    </widget>
+    <widget class="QPushButton" name="pushButton_6">
+     <property name="geometry">
+      <rect>
+       <x>210</x>
+       <y>100</y>
+       <width>89</width>
+       <height>25</height>
+      </rect>
+     </property>
+     <property name="text">
+      <string>确定</string>
+     </property>
+    </widget>
+    <widget class="QPushButton" name="pushButton_7">
+     <property name="geometry">
+      <rect>
+       <x>210</x>
+       <y>130</y>
+       <width>89</width>
+       <height>25</height>
+      </rect>
+     </property>
+     <property name="text">
+      <string>确定</string>
+     </property>
+    </widget>
+   </widget>
+   <widget class="QLabel" name="label_6">
+    <property name="geometry">
+     <rect>
+      <x>30</x>
+      <y>0</y>
+      <width>541</width>
+      <height>17</height>
+     </rect>
+    </property>
+    <property name="text">
+     <string>所有命令的标准输出将从Text Brower显示，标准错误输出将从弹窗显示。</string>
+    </property>
+   </widget>
+   <widget class="QLabel" name="label_7">
+    <property name="geometry">
+     <rect>
+      <x>810</x>
+      <y>150</y>
+      <width>67</width>
+      <height>17</height>
+     </rect>
+    </property>
+    <property name="text">
+     <string>标准输出</string>
+    </property>
+   </widget>
+   <widget class="QTextBrowser" name="textBrowser_2">
+    <property name="geometry">
+     <rect>
+      <x>810</x>
+      <y>170</y>
+      <width>311</width>
+      <height>371</height>
+     </rect>
+    </property>
+   </widget>
+   <widget class="QGroupBox" name="groupBox_3">
+    <property name="geometry">
+     <rect>
+      <x>410</x>
+      <y>30</y>
+      <width>371</width>
+      <height>151</height>
+     </rect>
+    </property>
+    <property name="title">
+     <string>串口</string>
+    </property>
+    <widget class="QLabel" name="label_10">
+     <property name="geometry">
+      <rect>
+       <x>10</x>
+       <y>30</y>
+       <width>101</width>
+       <height>17</height>
+      </rect>
+     </property>
+     <property name="text">
+      <string>查看系统版本</string>
+     </property>
+    </widget>
+    <widget class="QPushButton" name="pushButton_8">
+     <property name="geometry">
+      <rect>
+       <x>110</x>
+       <y>30</y>
+       <width>111</width>
+       <height>25</height>
+      </rect>
+     </property>
+     <property name="text">
+      <string>lsb_release -a</string>
+     </property>
+    </widget>
+    <widget class="QPushButton" name="pushButton_9">
+     <property name="geometry">
+      <rect>
+       <x>110</x>
+       <y>60</y>
+       <width>89</width>
+       <height>25</height>
+      </rect>
+     </property>
+     <property name="text">
+      <string>g++ -v</string>
+     </property>
+    </widget>
+    <widget class="QLabel" name="label_11">
+     <property name="geometry">
+      <rect>
+       <x>10</x>
+       <y>60</y>
+       <width>91</width>
+       <height>17</height>
+      </rect>
+     </property>
+     <property name="text">
+      <string>查看编译器</string>
+     </property>
+    </widget>
+   </widget>
+  </widget>
+  <widget class="QMenuBar" name="menuBar">
+   <property name="geometry">
+    <rect>
+     <x>0</x>
+     <y>0</y>
+     <width>1135</width>
+     <height>22</height>
+    </rect>
+   </property>
+  </widget>
+  <widget class="QToolBar" name="mainToolBar">
+   <attribute name="toolBarArea">
+    <enum>TopToolBarArea</enum>
+   </attribute>
+   <attribute name="toolBarBreak">
+    <bool>false</bool>
+   </attribute>
+  </widget>
+  <widget class="QStatusBar" name="statusBar"/>
+ </widget>
+ <layoutdefault spacing="6" margin="11"/>
+ <resources/>
+ <connections/>
+</ui>

src/tool/tool_system_debug_command/tool_system_debug_command/tool_system_debug_command.pro

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

src/ui/ui_ads_hmi/ADCIntelligentVehicle.cpp

@@ -1062,6 +1062,7 @@ void ADCIntelligentVehicle::paintEvent(QPaintEvent *)
         scene->clear();
         scene_small->clear();
         scene->addPixmap(QPixmap::fromImage(*image));
+
         scene_small->addPixmap(QPixmap::fromImage(*image_small));
         myview->setScene(scene);
         myview_small->setScene(scene_small);

src/ui/ui_osgviewer/main.cpp

@@ -29,6 +29,7 @@
 #include "viewer.hpp"
 #include "RoadManager.hpp"
 #include "CommonMini.hpp"
+#include "alog.h"
 
 
 #define DEFAULT_SPEED   70  // km/h
@@ -62,10 +63,22 @@ typedef struct
 	int id;
 } Car;
 
+typedef struct
+{
+    double mrel_x;
+    double mrel_y;
+    viewer::EntityModel * model;
+} RadarObj;
+
 std::vector<Car*> cars;
 
+std::vector<RadarObj *> gradarobj;
+const int NRADARMX = 100;
+
 Car * gADCIV_car;
 
+viewer::EntityModel *  gtestRadar;
+
 // Car models used for populating the road network
 // path should be relative the OpenDRIVE file
 static const char* carModelsFiles_[] =
@@ -85,6 +98,7 @@ std::vector<osg::ref_ptr<osg::LOD>> carModels_;
 
 #include "modulecomm.h"
 #include "gpsimu.pb.h"
+#include "radarobjectarray.pb.h"
 #include "gnss_coordinate_convert.h"
 
 double glat0 = 39.1201777;
@@ -95,10 +109,9 @@ double gfrel_y = 0;
 double gfrel_z = 0;
 double gvehicle_hdg = 0;
 
-double gvehicleheight = 1.6;
+double gvehicleheight = 7.6;//1.6
 double gfspeed = 0;
 
-
 void Listengpsimu(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
 {
 //        ggpsimu->UpdateGPSIMU(strdata,nSize);
@@ -140,6 +153,42 @@ void Listengpsimu(const char * strdata,const unsigned int nSize,const unsigned i
     gvehicle_hdg = hdg;
 }
 
+void ListenRADAR(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
+{
+
+    static qint64 oldrecvtime;
+    iv::radar::radarobjectarray xradararray;
+    if(!xradararray.ParseFromArray(strdata,nSize))
+    {
+        std::cout<<"ListenRadar Parse Error."<<std::endl;
+        return;
+    }
+
+    int i;
+    for(i=0;i<xradararray.obj_size();i++)
+    {
+        iv::radar::radarobject * pobj = xradararray.mutable_obj(i);
+        if(i<gradarobj.size())
+        {
+            if(pobj->bvalid())
+            {
+            gradarobj.at(i)->mrel_x = pobj->x();
+            gradarobj.at(i)->mrel_y = pobj->y();
+            }
+            else
+            {
+                gradarobj.at(i)->mrel_x = 10000;
+                gradarobj.at(i)->mrel_y = 10000;
+            }
+        }
+    }
+    for(i=xradararray.obj_size();i<gradarobj.size();i++)
+    {
+        gradarobj.at(i)->mrel_x = 10000;
+        gradarobj.at(i)->mrel_y = 10000;
+    }
+}
+
 
 void log_callback(const char *str)
 {
@@ -230,6 +279,20 @@ int SetupADCIVCar( viewer::Viewer *viewer)
     return 0;
 }
 
+int SetupRadar(viewer::Viewer *viewer)
+{
+    int i;
+    for(i=0;i<NRADARMX;i++)
+    {
+        RadarObj * pRadar = new RadarObj;
+        pRadar->mrel_x = 10000;
+        pRadar->mrel_y = 10000;
+        pRadar->model = viewer->AddRadarModel(osg::Vec3(0.5, 0.5, 0.5),"");
+        gradarobj.push_back(pRadar);
+    }
+    return 0;
+}
+
 int SetupCars(roadmanager::OpenDrive *odrManager, viewer::Viewer *viewer)
 {
 	if (density < 1E-10)
@@ -365,6 +428,26 @@ void updateADCIVCar(Car * car)
                     head, osg::Vec3(0, 0, 1));
 
         car->model->txNode_->setAttitude(car->model->quat_);
+
+        int k;
+ //       head = head +M_PI/2.0;
+        for(k=0;k<gradarobj.size();k++)
+        {
+            double relx,rely;
+            relx = gradarobj.at(k)->mrel_y*cos(head) + gradarobj.at(k)->mrel_x * sin(head);
+            rely = gradarobj.at(k)->mrel_y*sin(head) - gradarobj.at(k)->mrel_x*cos(head);
+
+            gradarobj.at(k)->model->txNode_->setPosition(osg::Vec3(x+relx, y+rely, z));
+
+            gradarobj.at(k)->model->quat_.makeRotate(
+                        roll, osg::Vec3(1, 0, 0),
+                        pitch, osg::Vec3(0, 1, 0),
+                        head, osg::Vec3(0, 0, 1));
+
+            gradarobj.at(k)->model->txNode_->setAttitude(car->model->quat_);
+        }
+
+
     }
 
 }
@@ -410,10 +493,11 @@ int main(int argc, char** argv)
     argv = new char*[3];
     argv[0] = "testodrviewer";
     argv[1] = "--odr";
-    argv[2] = "/home/yuchuli/map/models/y.xodr";
+    argv[2] = "/home/nvidia/map/models/y.xodr";
 
 
-    void * pa = iv::modulecomm::RegisterRecv("hcp2_gpsimu",Listengpsimu);
+    void * pa = iv::modulecomm::RegisterRecv("ins550d_gpsimu",Listengpsimu);
+    pa = iv::modulecomm::RegisterRecv("radar",ListenRADAR);
     (void)pa;
 
 	// Use logger callback
@@ -547,6 +631,10 @@ int main(int argc, char** argv)
 			viewer->GetNodeMaskBit(viewer::NodeMask::NODE_MASK_OSI_POINTS) ? "on" : "off");
 
          SetupADCIVCar(viewer);
+         SetupRadar(viewer);
+//         gtestRadar = viewer->AddRadarModel(osg::Vec3(0.5, 0.5, 0.5),"");
+//         gtestRadar = viewer->AddEntityModel(carModelsFiles_[0], osg::Vec3(0.5, 0.5, 0.5),
+//                                             viewer::EntityModel::EntityType::ENTITY_TYPE_VEHICLE, false, "", 0);
 //        if (SetupCars(odrManager, viewer) == -1)
 //        {
 //            return 4;

src/ui/ui_osgviewer/ui_osgviewer.pro

@@ -21,6 +21,8 @@ SOURCES += \
         ../../include/msgtype/gps.pb.cc \
         ../../include/msgtype/gpsimu.pb.cc \
         ../../include/msgtype/imu.pb.cc \
+        ../../include/msgtype/radarobject.pb.cc \
+        ../../include/msgtype/radarobjectarray.pb.cc \
         gnss_coordinate_convert.cpp \
         main.cpp \
         ../../../thirdpartylib/esminilib/odrSpiral.cpp \
@@ -36,9 +38,9 @@ else: unix:!android: target.path = /opt/$${TARGET}/bin
 !isEmpty(target.path): INSTALLS += target
 
 
-INCLUDEPATH += /home/yuchuli/File/git/OpenSceneGraph/include
+INCLUDEPATH += $$PWD/../../../thirdpartylib/osglib/include
 
-INCLUDEPATH += /home/yuchuli/File/git/OpenSceneGraph/build/include
+INCLUDEPATH += $$PWD/../../../thirdpartylib/osglib/include/build/include
 
 INCLUDEPATH += $$PWD/../../../thirdpartylib/esminilib
 
@@ -56,17 +58,21 @@ INCLUDEPATH += $$PWD/../../../thirdpartylib/esminilib
 }
 
 
-LIBS += -L/home/yuchuli/File/git/OpenSceneGraph/build/lib -losg -losgDB -losgFX \
+LIBS += -L$$PWD/../../../thirdpartylib/osglib/lib -losg -losgDB -losgFX \
         -losgGA -losgViewer -losgSim -losgUtil -losgText
 
 
-LIBS += /home/yuchuli/File/git/OpenSceneGraph/build/lib/osgPlugins-3.7.0/osgdb_serializers_osg.so
-LIBS += /home/yuchuli/File/git/OpenSceneGraph/build/lib/osgPlugins-3.7.0/osgdb_osg.so
-LIBS += /home/yuchuli/File/git/OpenSceneGraph/build/lib/osgPlugins-3.7.0/osgdb_serializers_osgsim.so
-LIBS += /home/yuchuli/File/git/OpenSceneGraph/build/lib/osgPlugins-3.7.0/osgdb_jpeg.so
+LIBS += $$PWD/../../../thirdpartylib/osglib/lib/osgPlugins-3.7.0/osgdb_serializers_osg.so
+LIBS += $$PWD/../../../thirdpartylib/osglib/lib/osgPlugins-3.7.0/osgdb_osg.so
+LIBS += $$PWD/../../../thirdpartylib/osglib/lib/osgPlugins-3.7.0/osgdb_serializers_osgsim.so
+LIBS += $$PWD/../../../thirdpartylib/osglib/lib/osgPlugins-3.7.0/osgdb_jpeg.so
+
+
 
 HEADERS += \
     ../../include/msgtype/gps.pb.h \
     ../../include/msgtype/gpsimu.pb.h \
     ../../include/msgtype/imu.pb.h \
+    ../../include/msgtype/radarobject.pb.h \
+    ../../include/msgtype/radarobjectarray.pb.h \
     gnss_coordinate_convert.h

src/ui/ui_osgviewer/viewer.cpp

@@ -1047,6 +1047,101 @@ void Viewer::SetCameraMode(int mode)
 	rubberbandManipulator_->setMode(camMode_);
 }
 
+EntityModel* Viewer::AddRadarModel(osg::Vec3 trail_color,std::string name)
+{
+
+    osg::ref_ptr<osg::Group> group = 0;
+
+        osg::ref_ptr<osg::Geode> geode = new osg::Geode;
+        geode->addDrawable(new osg::ShapeDrawable(new osg::Box()));
+
+        osg::ref_ptr<osg::Group> bbGroup = new osg::Group;
+        osg::ref_ptr<osg::PositionAttitudeTransform> tx = new osg::PositionAttitudeTransform;
+
+        osg::Material* material = new osg::Material();
+
+        // Set color of vehicle based on its index
+        double* color;
+        double b = 1.5;  // brighness
+        color = color_blue;
+
+        material->setDiffuse(osg::Material::FRONT, osg::Vec4(b * color[0], b * color[1], b * color[2], 1.0));
+        material->setAmbient(osg::Material::FRONT, osg::Vec4(b * color[0], b * color[1], b * color[2], 1.0));
+
+        if (group == 0)
+        {
+            // If no model loaded, make a shaded copy of bounding box as model
+            osg::ref_ptr<osg::Geode> geodeCopy = dynamic_cast<osg::Geode*>(geode->clone(osg::CopyOp::DEEP_COPY_ALL));
+            group = new osg::Group;
+            tx->addChild(geodeCopy);
+            geodeCopy->setNodeMask(NodeMask::NODE_MASK_ENTITY_MODEL);
+        }
+
+        // Set dimensions of the entity "box"
+
+            // Scale to something car-ish
+            tx->setScale(osg::Vec3(0.5, 0.5, 1.5));
+            tx->setPosition(osg::Vec3(1.5, 0, 0.75));
+
+        tx->addChild(geode);
+        tx->getOrCreateStateSet()->setAttribute(material);
+        bbGroup->setName("BoundingBox");
+        bbGroup->addChild(tx);
+
+        group->addChild(bbGroup);
+        group->setName(name);
+
+    EntityModel* model;
+//	if (type == EntityModel::EntityType::ENTITY_TYPE_VEHICLE)
+//	{
+        model = new EntityModel(osgViewer_, group, rootnode_, trails_, dot_node_, trail_color, name);
+//        model = new CarModel(osgViewer_, group, rootnode_, trails_, dot_node_, trail_color, name);
+
+//	}
+//	else
+//	{
+//        model = new EntityModel(osgViewer_, group, rootnode_, trails_, dot_node_, trail_color, name);
+//	}
+
+    model->state_set_ = model->lod_->getOrCreateStateSet(); // Creating material
+    model->blend_color_ = new osg::BlendColor(osg::Vec4(1, 1, 1, 1));
+    model->state_set_->setAttributeAndModes(model->blend_color_);
+    model->blend_color_->setDataVariance(osg::Object::DYNAMIC);
+
+    osg::BlendFunc* bf = new osg::BlendFunc(osg::BlendFunc::CONSTANT_ALPHA, osg::BlendFunc::ONE_MINUS_CONSTANT_ALPHA);
+    model->state_set_->setAttributeAndModes(bf);
+    model->state_set_->setMode(GL_DEPTH_TEST, osg::StateAttribute::ON);
+    model->state_set_->setRenderingHint(osg::StateSet::TRANSPARENT_BIN);
+
+    entities_.push_back(model);
+
+//	// Focus on first added car
+//	if (entities_.size() == 1)
+//	{
+//		currentCarInFocus_ = 0;
+//		rubberbandManipulator_->setTrackNode(entities_.back()->txNode_,
+//			rubberbandManipulator_->getMode() == osgGA::RubberbandManipulator::CAMERA_MODE::RB_MODE_TOP ? false : true);
+//		nodeTrackerManipulator_->setTrackNode(entities_.back()->txNode_);
+//	}
+
+//	if (type == EntityModel::EntityType::ENTITY_TYPE_VEHICLE)
+//	{
+//		CarModel* vehicle = (CarModel*)entities_.back();
+//		CreateRoadSensors(vehicle);
+
+//		if (road_sensor)
+//		{
+//			vehicle->road_sensor_->Show();
+//		}
+//		else
+//		{
+//			vehicle->road_sensor_->Hide();
+//		}
+//	}
+
+    return entities_.back();
+}
+
 EntityModel* Viewer::AddEntityModel(std::string modelFilepath, osg::Vec3 trail_color, EntityModel::EntityType type, 
 	bool road_sensor, std::string name, OSCBoundingBox* boundingBox)
 {
@@ -1189,17 +1284,17 @@ EntityModel* Viewer::AddEntityModel(std::string modelFilepath, osg::Vec3 trail_c
 
 	if (type == EntityModel::EntityType::ENTITY_TYPE_VEHICLE)
 	{
-		CarModel* vehicle = (CarModel*)entities_.back();
-		CreateRoadSensors(vehicle);
+        CarModel* vehicle = (CarModel*)entities_.back();
+        CreateRoadSensors(vehicle);
 		
-		if (road_sensor)
-		{
-			vehicle->road_sensor_->Show();
-		}
-		else
-		{
-			vehicle->road_sensor_->Hide();
-		}
+        if (road_sensor)
+        {
+            vehicle->road_sensor_->Show();
+        }
+        else
+        {
+            vehicle->road_sensor_->Hide();
+        }
 	}
 
 	return entities_.back();

src/ui/ui_osgviewer/viewer.hpp

@@ -317,6 +317,7 @@ namespace viewer
 		int GetEntityInFocus() { return currentCarInFocus_; }
 		EntityModel* AddEntityModel(std::string modelFilepath, osg::Vec3 trail_color, EntityModel::EntityType type, 
 			bool road_sensor, std::string name, OSCBoundingBox *boundingBox);
+                EntityModel* AddRadarModel(osg::Vec3 trail_color,std::string name);
 		void RemoveCar(std::string name);
 		int LoadShadowfile(std::string vehicleModelFilename);
 		int AddEnvironment(const char* filename);