余工修改了driver_map_xodrload, 增加了起点终点模糊路段智能选择的算法， 增加了终点靠边停车的算法（需要设置sidelaneenable为true）

src/driver/driver_map_xodrload_new/boost.h

@@ -0,0 +1,51 @@
+#pragma once
+
+#ifndef _IV_BOOST_H_
+#define _IV_BOOST_H_
+
+#if defined __GNUC__
+#  pragma GCC system_header 
+#endif
+#ifndef __CUDACC__
+//https://bugreports.qt-project.org/browse/QTBUG-22829
+#ifndef Q_MOC_RUN
+#include <boost/version.hpp>
+#include <boost/numeric/conversion/cast.hpp>
+#include <boost/thread/mutex.hpp>
+#include <boost/thread/condition.hpp>
+#include <boost/thread.hpp>
+#include <boost/thread/thread.hpp>
+#include <boost/filesystem.hpp>
+#include <boost/bind.hpp>
+#include <boost/cstdint.hpp>
+#include <boost/function.hpp>
+#include <boost/tuple/tuple.hpp>
+#include <boost/shared_ptr.hpp>
+#include <boost/weak_ptr.hpp>
+#include <boost/mpl/fold.hpp>
+#include <boost/mpl/inherit.hpp>
+#include <boost/mpl/inherit_linearly.hpp>
+#include <boost/mpl/joint_view.hpp>
+#include <boost/mpl/transform.hpp>
+#include <boost/mpl/vector.hpp>
+#include <boost/algorithm/string.hpp>
+#ifndef Q_MOC_RUN
+#include <boost/date_time/posix_time/posix_time.hpp>
+#endif
+#if BOOST_VERSION >= 104700
+#include <boost/chrono.hpp>
+#endif
+#include <boost/tokenizer.hpp>
+#include <boost/foreach.hpp>
+#include <boost/shared_array.hpp>
+#include <boost/interprocess/sync/file_lock.hpp>
+#if BOOST_VERSION >= 104900
+#include <boost/interprocess/permissions.hpp>
+#endif
+#include <boost/iostreams/device/mapped_file.hpp>
+#define BOOST_PARAMETER_MAX_ARITY 7
+#include <boost/signals2.hpp>
+#include <boost/signals2/slot.hpp>
+#endif
+#endif
+#endif    // _IV_BOOST_H_

src/driver/driver_map_xodrload_new/const.cpp

@@ -0,0 +1,255 @@
+/*							const.c
+ *
+ *	Globally declared constants
+ *
+ *
+ *
+ * SYNOPSIS:
+ *
+ * extern double nameofconstant;
+ *
+ *
+ *
+ *
+ * DESCRIPTION:
+ *
+ * This file contains a number of mathematical constants and
+ * also some needed size parameters of the computer arithmetic.
+ * The values are supplied as arrays of hexadecimal integers
+ * for IEEE arithmetic; arrays of octal constants for DEC
+ * arithmetic; and in a normal decimal scientific notation for
+ * other machines.  The particular notation used is determined
+ * by a symbol (DEC, IBMPC, or UNK) defined in the include file
+ * mconf.h.
+ *
+ * The default size parameters are as follows.
+ *
+ * For DEC and UNK modes:
+ * MACHEP =  1.38777878078144567553E-17       2**-56
+ * MAXLOG =  8.8029691931113054295988E1       log(2**127)
+ * MINLOG = -8.872283911167299960540E1        log(2**-128)
+ * MAXNUM =  1.701411834604692317316873e38    2**127
+ *
+ * For IEEE arithmetic (IBMPC):
+ * MACHEP =  1.11022302462515654042E-16       2**-53
+ * MAXLOG =  7.09782712893383996843E2         log(2**1024)
+ * MINLOG = -7.08396418532264106224E2         log(2**-1022)
+ * MAXNUM =  1.7976931348623158E308           2**1024
+ *
+ * The global symbols for mathematical constants are
+ * PI     =  3.14159265358979323846           pi
+ * PIO2   =  1.57079632679489661923           pi/2
+ * PIO4   =  7.85398163397448309616E-1        pi/4
+ * SQRT2  =  1.41421356237309504880           sqrt(2)
+ * SQRTH  =  7.07106781186547524401E-1        sqrt(2)/2
+ * LOG2E  =  1.4426950408889634073599         1/log(2)
+ * SQ2OPI =  7.9788456080286535587989E-1      sqrt( 2/pi )
+ * LOGE2  =  6.93147180559945309417E-1        log(2)
+ * LOGSQ2 =  3.46573590279972654709E-1        log(2)/2
+ * THPIO4 =  2.35619449019234492885           3*pi/4
+ * TWOOPI =  6.36619772367581343075535E-1     2/pi
+ *
+ * These lists are subject to change.
+ */
+
+/*							const.c */
+
+/*
+Cephes Math Library Release 2.3:  March, 1995
+Copyright 1984, 1995 by Stephen L. Moshier
+*/
+
+#include "mconf.h"
+#include <stdlib.h>
+#include <stdio.h>
+
+#ifdef UNK
+#if 1
+double MACHEP =  1.11022302462515654042E-16;   /* 2**-53 */
+#else
+double MACHEP =  1.38777878078144567553E-17;   /* 2**-56 */
+#endif
+double UFLOWTHRESH =  2.22507385850720138309E-308; /* 2**-1022 */
+#ifdef DENORMAL
+double MAXLOG =  7.09782712893383996732E2;     /* log(MAXNUM) */
+/* double MINLOG = -7.44440071921381262314E2; */     /* log(2**-1074) */
+double MINLOG = -7.451332191019412076235E2;     /* log(2**-1075) */
+#else
+double MAXLOG =  7.08396418532264106224E2;     /* log 2**1022 */
+double MINLOG = -7.08396418532264106224E2;     /* log 2**-1022 */
+#endif
+double MAXNUM =  1.79769313486231570815E308;    /* 2**1024*(1-MACHEP) */
+double PI     =  3.14159265358979323846;       /* pi */
+double PIO2   =  1.57079632679489661923;       /* pi/2 */
+double PIO4   =  7.85398163397448309616E-1;    /* pi/4 */
+double SQRT2  =  1.41421356237309504880;       /* sqrt(2) */
+double SQRTH  =  7.07106781186547524401E-1;    /* sqrt(2)/2 */
+double LOG2E  =  1.4426950408889634073599;     /* 1/log(2) */
+double SQ2OPI =  7.9788456080286535587989E-1;  /* sqrt( 2/pi ) */
+double LOGE2  =  6.93147180559945309417E-1;    /* log(2) */
+double LOGSQ2 =  3.46573590279972654709E-1;    /* log(2)/2 */
+double THPIO4 =  2.35619449019234492885;       /* 3*pi/4 */
+double TWOOPI =  6.36619772367581343075535E-1; /* 2/pi */
+#ifdef INFINITIES
+//double INFINITY = 1.0/0.0;  /* 99e999; */
+double INFINITY = atof("infinity");  /* 99e999; */
+#else
+double INFINITY =  1.79769313486231570815E308;    /* 2**1024*(1-MACHEP) */
+#endif
+#ifdef NANS
+double NAN =atof("infinity") - atof("infinity");
+#else
+double NAN = 0.0;
+#endif
+#ifdef MINUSZERO
+double NEGZERO = -0.0;
+#else
+double NEGZERO = 0.0;
+#endif
+#endif
+
+#ifdef IBMPC
+			/* 2**-53 =  1.11022302462515654042E-16 */
+unsigned short MACHEP[4] = {0x0000,0x0000,0x0000,0x3ca0};
+unsigned short UFLOWTHRESH[4] = {0x0000,0x0000,0x0000,0x0010};
+#ifdef DENORMAL
+			/* log(MAXNUM) =  7.09782712893383996732224E2 */
+unsigned short MAXLOG[4] = {0x39ef,0xfefa,0x2e42,0x4086};
+			/* log(2**-1074) = - -7.44440071921381262314E2 */
+/*unsigned short MINLOG[4] = {0x71c3,0x446d,0x4385,0xc087};*/
+unsigned short MINLOG[4] = {0x3052,0xd52d,0x4910,0xc087};
+#else
+			/* log(2**1022) =   7.08396418532264106224E2 */
+unsigned short MAXLOG[4] = {0xbcd2,0xdd7a,0x232b,0x4086};
+			/* log(2**-1022) = - 7.08396418532264106224E2 */
+unsigned short MINLOG[4] = {0xbcd2,0xdd7a,0x232b,0xc086};
+#endif
+			/* 2**1024*(1-MACHEP) =  1.7976931348623158E308 */
+unsigned short MAXNUM[4] = {0xffff,0xffff,0xffff,0x7fef};
+unsigned short PI[4]     = {0x2d18,0x5444,0x21fb,0x4009};
+unsigned short PIO2[4]   = {0x2d18,0x5444,0x21fb,0x3ff9};
+unsigned short PIO4[4]   = {0x2d18,0x5444,0x21fb,0x3fe9};
+unsigned short SQRT2[4]  = {0x3bcd,0x667f,0xa09e,0x3ff6};
+unsigned short SQRTH[4]  = {0x3bcd,0x667f,0xa09e,0x3fe6};
+unsigned short LOG2E[4]  = {0x82fe,0x652b,0x1547,0x3ff7};
+unsigned short SQ2OPI[4] = {0x3651,0x33d4,0x8845,0x3fe9};
+unsigned short LOGE2[4]  = {0x39ef,0xfefa,0x2e42,0x3fe6};
+unsigned short LOGSQ2[4] = {0x39ef,0xfefa,0x2e42,0x3fd6};
+unsigned short THPIO4[4] = {0x21d2,0x7f33,0xd97c,0x4002};
+unsigned short TWOOPI[4] = {0xc883,0x6dc9,0x5f30,0x3fe4};
+#ifdef INFINITIES
+unsigned short INFINITY[4] = {0x0000,0x0000,0x0000,0x7ff0};
+#else
+unsigned short INFINITY[4] = {0xffff,0xffff,0xffff,0x7fef};
+#endif
+#ifdef NANS
+unsigned short NAN[4] = {0x0000,0x0000,0x0000,0x7ffc};
+#else
+unsigned short NAN[4] = {0x0000,0x0000,0x0000,0x0000};
+#endif
+#ifdef MINUSZERO
+unsigned short NEGZERO[4] = {0x0000,0x0000,0x0000,0x8000};
+#else
+unsigned short NEGZERO[4] = {0x0000,0x0000,0x0000,0x0000};
+#endif
+#endif
+
+#ifdef MIEEE
+			/* 2**-53 =  1.11022302462515654042E-16 */
+unsigned short MACHEP[4] = {0x3ca0,0x0000,0x0000,0x0000};
+unsigned short UFLOWTHRESH[4] = {0x0010,0x0000,0x0000,0x0000};
+#ifdef DENORMAL
+			/* log(2**1024) =   7.09782712893383996843E2 */
+unsigned short MAXLOG[4] = {0x4086,0x2e42,0xfefa,0x39ef};
+			/* log(2**-1074) = - -7.44440071921381262314E2 */
+/* unsigned short MINLOG[4] = {0xc087,0x4385,0x446d,0x71c3}; */
+unsigned short MINLOG[4] = {0xc087,0x4910,0xd52d,0x3052};
+#else
+			/* log(2**1022) =  7.08396418532264106224E2 */
+unsigned short MAXLOG[4] = {0x4086,0x232b,0xdd7a,0xbcd2};
+			/* log(2**-1022) = - 7.08396418532264106224E2 */
+unsigned short MINLOG[4] = {0xc086,0x232b,0xdd7a,0xbcd2};
+#endif
+			/* 2**1024*(1-MACHEP) =  1.7976931348623158E308 */
+unsigned short MAXNUM[4] = {0x7fef,0xffff,0xffff,0xffff};
+unsigned short PI[4]     = {0x4009,0x21fb,0x5444,0x2d18};
+unsigned short PIO2[4]   = {0x3ff9,0x21fb,0x5444,0x2d18};
+unsigned short PIO4[4]   = {0x3fe9,0x21fb,0x5444,0x2d18};
+unsigned short SQRT2[4]  = {0x3ff6,0xa09e,0x667f,0x3bcd};
+unsigned short SQRTH[4]  = {0x3fe6,0xa09e,0x667f,0x3bcd};
+unsigned short LOG2E[4]  = {0x3ff7,0x1547,0x652b,0x82fe};
+unsigned short SQ2OPI[4] = {0x3fe9,0x8845,0x33d4,0x3651};
+unsigned short LOGE2[4]  = {0x3fe6,0x2e42,0xfefa,0x39ef};
+unsigned short LOGSQ2[4] = {0x3fd6,0x2e42,0xfefa,0x39ef};
+unsigned short THPIO4[4] = {0x4002,0xd97c,0x7f33,0x21d2};
+unsigned short TWOOPI[4] = {0x3fe4,0x5f30,0x6dc9,0xc883};
+#ifdef INFINITIES
+unsigned short INFINITY[4] = {0x7ff0,0x0000,0x0000,0x0000};
+#else
+unsigned short INFINITY[4] = {0x7fef,0xffff,0xffff,0xffff};
+#endif
+#ifdef NANS
+unsigned short NAN[4] = {0x7ff8,0x0000,0x0000,0x0000};
+#else
+unsigned short NAN[4] = {0x0000,0x0000,0x0000,0x0000};
+#endif
+#ifdef MINUSZERO
+unsigned short NEGZERO[4] = {0x8000,0x0000,0x0000,0x0000};
+#else
+unsigned short NEGZERO[4] = {0x0000,0x0000,0x0000,0x0000};
+#endif
+#endif
+
+#ifdef DEC
+			/* 2**-56 =  1.38777878078144567553E-17 */
+unsigned short MACHEP[4] = {0022200,0000000,0000000,0000000};
+unsigned short UFLOWTHRESH[4] = {0x0080,0x0000,0x0000,0x0000};
+			/* log 2**127 = 88.029691931113054295988 */
+unsigned short MAXLOG[4] = {041660,007463,0143742,025733,};
+			/* log 2**-128 = -88.72283911167299960540 */
+unsigned short MINLOG[4] = {0141661,071027,0173721,0147572,};
+			/* 2**127 = 1.701411834604692317316873e38 */
+unsigned short MAXNUM[4] = {077777,0177777,0177777,0177777,};
+unsigned short PI[4]     = {040511,007732,0121041,064302,};
+unsigned short PIO2[4]   = {040311,007732,0121041,064302,};
+unsigned short PIO4[4]   = {040111,007732,0121041,064302,};
+unsigned short SQRT2[4]  = {040265,002363,031771,0157145,};
+unsigned short SQRTH[4]  = {040065,002363,031771,0157144,};
+unsigned short LOG2E[4]  = {040270,0125073,024534,013761,};
+unsigned short SQ2OPI[4] = {040114,041051,0117241,0131204,};
+unsigned short LOGE2[4]  = {040061,071027,0173721,0147572,};
+unsigned short LOGSQ2[4] = {037661,071027,0173721,0147572,};
+unsigned short THPIO4[4] = {040426,0145743,0174631,007222,};
+unsigned short TWOOPI[4] = {040042,0174603,067116,042025,};
+/* Approximate infinity by MAXNUM.  */
+unsigned short INFINITY[4] = {077777,0177777,0177777,0177777,};
+unsigned short NAN[4] = {0000000,0000000,0000000,0000000};
+#ifdef MINUSZERO
+unsigned short NEGZERO[4] = {0000000,0000000,0000000,0100000};
+#else
+unsigned short NEGZERO[4] = {0000000,0000000,0000000,0000000};
+#endif
+#endif
+
+#ifndef UNK
+extern unsigned short MACHEP[];
+extern unsigned short UFLOWTHRESH[];
+extern unsigned short MAXLOG[];
+extern unsigned short UNDLOG[];
+extern unsigned short MINLOG[];
+extern unsigned short MAXNUM[];
+extern unsigned short PI[];
+extern unsigned short PIO2[];
+extern unsigned short PIO4[];
+extern unsigned short SQRT2[];
+extern unsigned short SQRTH[];
+extern unsigned short LOG2E[];
+extern unsigned short SQ2OPI[];
+extern unsigned short LOGE2[];
+extern unsigned short LOGSQ2[];
+extern unsigned short THPIO4[];
+extern unsigned short TWOOPI[];
+extern unsigned short INFINITY[];
+extern unsigned short NAN[];
+extern unsigned short NEGZERO[];
+#endif

src/driver/driver_map_xodrload_new/driver_map_xodrload.xml

@@ -0,0 +1,6 @@
+<xml>	
+	<node name="driver_map_xodrload">
+		<param name="extendmap" value="true" />
+	</node>
+</xml>
+

src/driver/driver_map_xodrload_new/driver_map_xodrload_new.pro

@@ -0,0 +1,80 @@
+QT -= gui
+
+QT += xml dbus
+
+CONFIG += c++11 console
+CONFIG -= app_bundle
+
+
+# The following define makes your compiler emit warnings if you use
+# any feature of Qt which as been marked deprecated (the exact warnings
+# depend on your compiler). Please consult the documentation of the
+# deprecated API in order to know how to port your code away from it.
+DEFINES += QT_DEPRECATED_WARNINGS
+
+QMAKE_CXXFLAGS +=  -g
+
+# 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     \
+    fresnl.cpp \
+    const.cpp \
+    polevl.c \
+    globalplan.cpp \
+    dubins.c \
+    gnss_coordinate_convert.cpp \
+    service_roi_xodr.cpp \
+    xodrplan.cpp
+
+HEADERS += \
+    ../../../include/ivexit.h \
+    mconf.h \
+    globalplan.h \
+    gps_type.h \
+    dubins.h \
+    gnss_coordinate_convert.h \
+    planpoint.h \
+    service_roi_xodr.h \
+    xodrplan.h
+
+
+!include(../../../include/common.pri ) {
+    error( "Couldn't find the common.pri file!" )
+}
+
+!include(../../../include/ivprotobuf.pri ) {
+    error( "Couldn't find the ivprotobuf.pri file!" )
+}
+
+!include(../../../include/ivboost.pri ) {
+    error( "Couldn't find the ivboost.pri file!" )
+}
+
+!include(../../../include/iveigen.pri ) {
+    error( "Couldn't find the iveigen.pri file!" )
+}
+
+
+!include(../../common/common/xodr/OpenDrive/OpenDrive.pri ) {
+    error( "Couldn't find the OpenDrive.pri file!" )
+}
+
+!include(../../common/common/xodr/TinyXML/TinyXML.pri ) {
+    error( "Couldn't find the TinyXML.pri file!" )
+}
+
+!include(../../common/common/xodr/xodrfunc/xodrfunc.pri ) {
+    error( "Couldn't find the xodrfunc.pri file!" )
+}
+
+INCLUDEPATH += $$PWD/../../common/common/xodr
+INCLUDEPATH += $$PWD/../../common/common/xodr/xodrfunc
+
+DEFINES += INPILOT
+
+LIBS += -livprotoif
+
+

src/driver/driver_map_xodrload_new/dubins.c

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

src/driver/driver_map_xodrload_new/dubins.h

@@ -0,0 +1,170 @@
+/*
+ * Copyright (c) 2008-2018, Andrew Walker
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+#ifndef DUBINS_H
+#define DUBINS_H
+
+typedef enum 
+{
+    LSL = 0,
+    LSR = 1,
+    RSL = 2,
+    RSR = 3,
+    RLR = 4,
+    LRL = 5
+} DubinsPathType;
+
+typedef struct 
+{
+    /* the initial configuration */
+    double qi[3];        
+    /* the lengths of the three segments */
+    double param[3];     
+    /* model forward velocity / model angular velocity */
+    double rho;          
+    /* the path type described */
+    DubinsPathType type; 
+} DubinsPath;
+
+#define EDUBOK        (0)   /* No error */
+#define EDUBCOCONFIGS (1)   /* Colocated configurations */
+#define EDUBPARAM     (2)   /* Path parameterisitation error */
+#define EDUBBADRHO    (3)   /* the rho value is invalid */
+#define EDUBNOPATH    (4)   /* no connection between configurations with this word */
+
+/**
+ * Callback function for path sampling
+ *
+ * @note the q parameter is a configuration
+ * @note the t parameter is the distance along the path
+ * @note the user_data parameter is forwarded from the caller
+ * @note return non-zero to denote sampling should be stopped
+ */
+typedef int (*DubinsPathSamplingCallback)(double q[3], double t, void* user_data);
+
+/**
+ * Generate a path from an initial configuration to
+ * a target configuration, with a specified maximum turning
+ * radii
+ *
+ * A configuration is (x, y, theta), where theta is in radians, with zero
+ * along the line x = 0, and counter-clockwise is positive
+ *
+ * @param path  - the resultant path
+ * @param q0    - a configuration specified as an array of x, y, theta
+ * @param q1    - a configuration specified as an array of x, y, theta
+ * @param rho   - turning radius of the vehicle (forward velocity divided by maximum angular velocity)
+ * @return      - non-zero on error
+ */
+int dubins_shortest_path(DubinsPath* path, double q0[3], double q1[3], double rho);
+
+/**
+ * Generate a path with a specified word from an initial configuration to
+ * a target configuration, with a specified turning radius 
+ *
+ * @param path     - the resultant path
+ * @param q0       - a configuration specified as an array of x, y, theta
+ * @param q1       - a configuration specified as an array of x, y, theta
+ * @param rho      - turning radius of the vehicle (forward velocity divided by maximum angular velocity)
+ * @param pathType - the specific path type to use
+ * @return         - non-zero on error
+ */
+int dubins_path(DubinsPath* path, double q0[3], double q1[3], double rho, DubinsPathType pathType);
+
+/**
+ * Calculate the length of an initialised path
+ *
+ * @param path - the path to find the length of
+ */
+double dubins_path_length(DubinsPath* path);
+
+/**
+ * Return the length of a specific segment in an initialized path
+ *
+ * @param path - the path to find the length of
+ * @param i    - the segment you to get the length of (0-2)
+ */
+double dubins_segment_length(DubinsPath* path, int i);
+
+/**
+ * Return the normalized length of a specific segment in an initialized path
+ *
+ * @param path - the path to find the length of
+ * @param i    - the segment you to get the length of (0-2)
+ */
+double dubins_segment_length_normalized( DubinsPath* path, int i );
+
+/**
+ * Extract an integer that represents which path type was used
+ *
+ * @param path    - an initialised path
+ * @return        - one of LSL, LSR, RSL, RSR, RLR or LRL 
+ */
+DubinsPathType dubins_path_type(DubinsPath* path);
+
+/**
+ * Calculate the configuration along the path, using the parameter t
+ *
+ * @param path - an initialised path
+ * @param t    - a length measure, where 0 <= t < dubins_path_length(path)
+ * @param q    - the configuration result
+ * @returns    - non-zero if 't' is not in the correct range
+ */
+int dubins_path_sample(DubinsPath* path, double t, double q[3]);
+
+/**
+ * Walk along the path at a fixed sampling interval, calling the
+ * callback function at each interval
+ *
+ * The sampling process continues until the whole path is sampled, or the callback returns a non-zero value
+ *
+ * @param path      - the path to sample
+ * @param stepSize  - the distance along the path for subsequent samples
+ * @param cb        - the callback function to call for each sample
+ * @param user_data - optional information to pass on to the callback
+ *
+ * @returns - zero on successful completion, or the result of the callback
+ */
+int dubins_path_sample_many(DubinsPath* path, 
+                            double stepSize, 
+                            DubinsPathSamplingCallback cb, 
+                            void* user_data);
+
+/**
+ * Convenience function to identify the endpoint of a path
+ *
+ * @param path - an initialised path
+ * @param q    - the configuration result
+ */
+int dubins_path_endpoint(DubinsPath* path, double q[3]);
+
+/**
+ * Convenience function to extract a subset of a path
+ *
+ * @param path    - an initialised path
+ * @param t       - a length measure, where 0 < t < dubins_path_length(path)
+ * @param newpath - the resultant path
+ */
+int dubins_extract_subpath(DubinsPath* path, double t, DubinsPath* newpath);
+
+
+#endif /* DUBINS_H */
+

src/driver/driver_map_xodrload_new/fresnl.cpp

@@ -0,0 +1,518 @@
+/*							fresnl.c
+ *
+ *	Fresnel integral
+ *
+ *
+ *
+ * SYNOPSIS:
+ *
+ * double x, S, C;
+ * void fresnl();
+ *
+ * fresnl( x, _&S, _&C );
+ *
+ *
+ * DESCRIPTION:
+ *
+ * Evaluates the Fresnel integrals
+ *
+ *           x
+ *           -
+ *          | |
+ * C(x) =   |   cos(pi/2 t**2) dt,
+ *        | |
+ *         -
+ *          0
+ *
+ *           x
+ *           -
+ *          | |
+ * S(x) =   |   sin(pi/2 t**2) dt.
+ *        | |
+ *         -
+ *          0
+ *
+ *
+ * The integrals are evaluated by a power series for x < 1.
+ * For x >= 1 auxiliary functions f(x) and g(x) are employed
+ * such that
+ *
+ * C(x) = 0.5 + f(x) sin( pi/2 x**2 ) - g(x) cos( pi/2 x**2 )
+ * S(x) = 0.5 - f(x) cos( pi/2 x**2 ) - g(x) sin( pi/2 x**2 )
+ *
+ *
+ *
+ * ACCURACY:
+ *
+ *  Relative error.
+ *
+ * Arithmetic  function   domain     # trials      peak         rms
+ *   IEEE       S(x)      0, 10       10000       2.0e-15     3.2e-16
+ *   IEEE       C(x)      0, 10       10000       1.8e-15     3.3e-16
+ *   DEC        S(x)      0, 10        6000       2.2e-16     3.9e-17
+ *   DEC        C(x)      0, 10        5000       2.3e-16     3.9e-17
+ */
+
+/*
+Cephes Math Library Release 2.8:  June, 2000
+Copyright 1984, 1987, 1989, 2000 by Stephen L. Moshier
+*/
+
+#include "mconf.h"
+
+
+/* S(x) for small x */
+#ifdef UNK
+static double sn[6] = {
+-2.99181919401019853726E3,
+ 7.08840045257738576863E5,
+-6.29741486205862506537E7,
+ 2.54890880573376359104E9,
+-4.42979518059697779103E10,
+ 3.18016297876567817986E11,
+};
+static double sd[6] = {
+/* 1.00000000000000000000E0,*/
+ 2.81376268889994315696E2,
+ 4.55847810806532581675E4,
+ 5.17343888770096400730E6,
+ 4.19320245898111231129E8,
+ 2.24411795645340920940E10,
+ 6.07366389490084639049E11,
+};
+#endif
+#ifdef DEC
+static unsigned short sn[24] = {
+0143072,0176433,0065455,0127034,
+0045055,0007200,0134540,0026661,
+0146560,0035061,0023667,0127545,
+0050027,0166503,0002673,0153756,
+0151045,0002721,0121737,0102066,
+0051624,0013177,0033451,0021271,
+};
+static unsigned short sd[24] = {
+/*0040200,0000000,0000000,0000000,*/
+0042214,0130051,0112070,0101617,
+0044062,0010307,0172346,0152510,
+0045635,0160575,0143200,0136642,
+0047307,0171215,0127457,0052361,
+0050647,0031447,0032621,0013510,
+0052015,0064733,0117362,0012653,
+};
+#endif
+#ifdef IBMPC
+static unsigned short sn[24] = {
+0xb5c3,0x6d65,0x5fa3,0xc0a7,
+0x05b6,0x172c,0xa1d0,0x4125,
+0xf5ed,0x24f6,0x0746,0xc18e,
+0x7afe,0x60b7,0xfda8,0x41e2,
+0xf087,0x347b,0xa0ba,0xc224,
+0x2457,0xe6e5,0x82cf,0x4252,
+};
+static unsigned short sd[24] = {
+/*0x0000,0x0000,0x0000,0x3ff0,*/
+0x1072,0x3287,0x9605,0x4071,
+0xdaa9,0xfe9c,0x4218,0x40e6,
+0x17b4,0xb8d0,0xbc2f,0x4153,
+0xea9e,0xb5e5,0xfe51,0x41b8,
+0x22e9,0xe6b2,0xe664,0x4214,
+0x42b5,0x73de,0xad3b,0x4261,
+};
+#endif
+#ifdef MIEEE
+static unsigned short sn[24] = {
+0xc0a7,0x5fa3,0x6d65,0xb5c3,
+0x4125,0xa1d0,0x172c,0x05b6,
+0xc18e,0x0746,0x24f6,0xf5ed,
+0x41e2,0xfda8,0x60b7,0x7afe,
+0xc224,0xa0ba,0x347b,0xf087,
+0x4252,0x82cf,0xe6e5,0x2457,
+};
+static unsigned short sd[24] = {
+/*0x3ff0,0x0000,0x0000,0x0000,*/
+0x4071,0x9605,0x3287,0x1072,
+0x40e6,0x4218,0xfe9c,0xdaa9,
+0x4153,0xbc2f,0xb8d0,0x17b4,
+0x41b8,0xfe51,0xb5e5,0xea9e,
+0x4214,0xe664,0xe6b2,0x22e9,
+0x4261,0xad3b,0x73de,0x42b5,
+};
+#endif
+
+/* C(x) for small x */
+#ifdef UNK
+static double cn[6] = {
+-4.98843114573573548651E-8,
+ 9.50428062829859605134E-6,
+-6.45191435683965050962E-4,
+ 1.88843319396703850064E-2,
+-2.05525900955013891793E-1,
+ 9.99999999999999998822E-1,
+};
+static double cd[7] = {
+ 3.99982968972495980367E-12,
+ 9.15439215774657478799E-10,
+ 1.25001862479598821474E-7,
+ 1.22262789024179030997E-5,
+ 8.68029542941784300606E-4,
+ 4.12142090722199792936E-2,
+ 1.00000000000000000118E0,
+};
+#endif
+#ifdef DEC
+static unsigned short cn[24] = {
+0132126,0040141,0063733,0013231,
+0034037,0072223,0010200,0075637,
+0135451,0021020,0073264,0036057,
+0036632,0131520,0101316,0060233,
+0137522,0072541,0136124,0132202,
+0040200,0000000,0000000,0000000,
+};
+static unsigned short cd[28] = {
+0026614,0135503,0051776,0032631,
+0030573,0121116,0154033,0126712,
+0032406,0034100,0012442,0106212,
+0034115,0017567,0150520,0164623,
+0035543,0106171,0177336,0146351,
+0037050,0150073,0000607,0171635,
+0040200,0000000,0000000,0000000,
+};
+#endif
+#ifdef IBMPC
+static unsigned short cn[24] = {
+0x62d3,0x2cfb,0xc80c,0xbe6a,
+0x0f74,0x6210,0xee92,0x3ee3,
+0x8786,0x0ed6,0x2442,0xbf45,
+0xcc13,0x1059,0x566a,0x3f93,
+0x9690,0x378a,0x4eac,0xbfca,
+0x0000,0x0000,0x0000,0x3ff0,
+};
+static unsigned short cd[28] = {
+0xc6b3,0x6a7f,0x9768,0x3d91,
+0x75b9,0xdb03,0x7449,0x3e0f,
+0x5191,0x02a4,0xc708,0x3e80,
+0x1d32,0xfa2a,0xa3ee,0x3ee9,
+0xd99d,0x3fdb,0x718f,0x3f4c,
+0xfe74,0x6030,0x1a07,0x3fa5,
+0x0000,0x0000,0x0000,0x3ff0,
+};
+#endif
+#ifdef MIEEE
+static unsigned short cn[24] = {
+0xbe6a,0xc80c,0x2cfb,0x62d3,
+0x3ee3,0xee92,0x6210,0x0f74,
+0xbf45,0x2442,0x0ed6,0x8786,
+0x3f93,0x566a,0x1059,0xcc13,
+0xbfca,0x4eac,0x378a,0x9690,
+0x3ff0,0x0000,0x0000,0x0000,
+};
+static unsigned short cd[28] = {
+0x3d91,0x9768,0x6a7f,0xc6b3,
+0x3e0f,0x7449,0xdb03,0x75b9,
+0x3e80,0xc708,0x02a4,0x5191,
+0x3ee9,0xa3ee,0xfa2a,0x1d32,
+0x3f4c,0x718f,0x3fdb,0xd99d,
+0x3fa5,0x1a07,0x6030,0xfe74,
+0x3ff0,0x0000,0x0000,0x0000,
+};
+#endif
+
+/* Auxiliary function f(x) */
+#ifdef UNK
+static double fn[10] = {
+  4.21543555043677546506E-1,
+  1.43407919780758885261E-1,
+  1.15220955073585758835E-2,
+  3.45017939782574027900E-4,
+  4.63613749287867322088E-6,
+  3.05568983790257605827E-8,
+  1.02304514164907233465E-10,
+  1.72010743268161828879E-13,
+  1.34283276233062758925E-16,
+  3.76329711269987889006E-20,
+};
+static double fd[10] = {
+/*  1.00000000000000000000E0,*/
+  7.51586398353378947175E-1,
+  1.16888925859191382142E-1,
+  6.44051526508858611005E-3,
+  1.55934409164153020873E-4,
+  1.84627567348930545870E-6,
+  1.12699224763999035261E-8,
+  3.60140029589371370404E-11,
+  5.88754533621578410010E-14,
+  4.52001434074129701496E-17,
+  1.25443237090011264384E-20,
+};
+#endif
+#ifdef DEC
+static unsigned short fn[40] = {
+0037727,0152216,0106601,0016214,
+0037422,0154606,0112710,0071355,
+0036474,0143453,0154253,0166545,
+0035264,0161606,0022250,0073743,
+0033633,0110036,0024653,0136246,
+0032003,0036652,0041164,0036413,
+0027740,0174122,0046305,0036726,
+0025501,0125270,0121317,0167667,
+0023032,0150555,0076175,0047443,
+0020061,0133570,0070130,0027657,
+};
+static unsigned short fd[40] = {
+/*0040200,0000000,0000000,0000000,*/
+0040100,0063767,0054413,0151452,
+0037357,0061566,0007243,0065754,
+0036323,0005365,0033552,0133625,
+0035043,0101123,0000275,0165402,
+0033367,0146614,0110623,0023647,
+0031501,0116644,0125222,0144263,
+0027436,0062051,0117235,0001411,
+0025204,0111543,0056370,0036201,
+0022520,0071351,0015227,0122144,
+0017554,0172240,0112713,0005006,
+};
+#endif
+#ifdef IBMPC
+static unsigned short fn[40] = {
+0x2391,0xd1b0,0xfa91,0x3fda,
+0x0e5e,0xd2b9,0x5b30,0x3fc2,
+0x7dad,0x7b15,0x98e5,0x3f87,
+0x0efc,0xc495,0x9c70,0x3f36,
+0x7795,0xc535,0x7203,0x3ed3,
+0x87a1,0x484e,0x67b5,0x3e60,
+0xa7bb,0x4998,0x1f0a,0x3ddc,
+0xfdf7,0x1459,0x3557,0x3d48,
+0xa9e4,0xaf8f,0x5a2d,0x3ca3,
+0x05f6,0x0e0b,0x36ef,0x3be6,
+};
+static unsigned short fd[40] = {
+/*0x0000,0x0000,0x0000,0x3ff0,*/
+0x7a65,0xeb21,0x0cfe,0x3fe8,
+0x6d7d,0xc1d4,0xec6e,0x3fbd,
+0x56f3,0xa6ed,0x615e,0x3f7a,
+0xbd60,0x6017,0x704a,0x3f24,
+0x64f5,0x9232,0xf9b1,0x3ebe,
+0x5916,0x9552,0x33b4,0x3e48,
+0xa061,0x33d3,0xcc85,0x3dc3,
+0x0790,0x6b9f,0x926c,0x3d30,
+0xf48d,0x2352,0x0e5d,0x3c8a,
+0x6141,0x12b9,0x9e94,0x3bcd,
+};
+#endif
+#ifdef MIEEE
+static unsigned short fn[40] = {
+0x3fda,0xfa91,0xd1b0,0x2391,
+0x3fc2,0x5b30,0xd2b9,0x0e5e,
+0x3f87,0x98e5,0x7b15,0x7dad,
+0x3f36,0x9c70,0xc495,0x0efc,
+0x3ed3,0x7203,0xc535,0x7795,
+0x3e60,0x67b5,0x484e,0x87a1,
+0x3ddc,0x1f0a,0x4998,0xa7bb,
+0x3d48,0x3557,0x1459,0xfdf7,
+0x3ca3,0x5a2d,0xaf8f,0xa9e4,
+0x3be6,0x36ef,0x0e0b,0x05f6,
+};
+static unsigned short fd[40] = {
+/*0x3ff0,0x0000,0x0000,0x0000,*/
+0x3fe8,0x0cfe,0xeb21,0x7a65,
+0x3fbd,0xec6e,0xc1d4,0x6d7d,
+0x3f7a,0x615e,0xa6ed,0x56f3,
+0x3f24,0x704a,0x6017,0xbd60,
+0x3ebe,0xf9b1,0x9232,0x64f5,
+0x3e48,0x33b4,0x9552,0x5916,
+0x3dc3,0xcc85,0x33d3,0xa061,
+0x3d30,0x926c,0x6b9f,0x0790,
+0x3c8a,0x0e5d,0x2352,0xf48d,
+0x3bcd,0x9e94,0x12b9,0x6141,
+};
+#endif
+
+
+/* Auxiliary function g(x) */
+#ifdef UNK
+static double gn[11] = {
+  5.04442073643383265887E-1,
+  1.97102833525523411709E-1,
+  1.87648584092575249293E-2,
+  6.84079380915393090172E-4,
+  1.15138826111884280931E-5,
+  9.82852443688422223854E-8,
+  4.45344415861750144738E-10,
+  1.08268041139020870318E-12,
+  1.37555460633261799868E-15,
+  8.36354435630677421531E-19,
+  1.86958710162783235106E-22,
+};
+static double gd[11] = {
+/*  1.00000000000000000000E0,*/
+  1.47495759925128324529E0,
+  3.37748989120019970451E-1,
+  2.53603741420338795122E-2,
+  8.14679107184306179049E-4,
+  1.27545075667729118702E-5,
+  1.04314589657571990585E-7,
+  4.60680728146520428211E-10,
+  1.10273215066240270757E-12,
+  1.38796531259578871258E-15,
+  8.39158816283118707363E-19,
+  1.86958710162783236342E-22,
+};
+#endif
+#ifdef DEC
+static unsigned short gn[44] = {
+0040001,0021435,0120406,0053123,
+0037511,0152523,0037703,0122011,
+0036631,0134302,0122721,0110235,
+0035463,0051712,0043215,0114732,
+0034101,0025677,0147725,0057630,
+0032323,0010342,0067523,0002206,
+0030364,0152247,0110007,0054107,
+0026230,0057654,0035464,0047124,
+0023706,0036401,0167705,0045440,
+0021166,0154447,0105632,0142461,
+0016142,0002353,0011175,0170530,
+};
+static unsigned short gd[44] = {
+/*0040200,0000000,0000000,0000000,*/
+0040274,0145551,0016742,0127005,
+0037654,0166557,0076416,0015165,
+0036717,0140217,0030675,0050111,
+0035525,0110060,0076405,0070502,
+0034125,0176061,0060120,0031730,
+0032340,0001615,0054343,0120501,
+0030375,0041414,0070747,0107060,
+0026233,0031034,0160757,0074526,
+0023710,0003341,0137100,0144664,
+0021167,0126414,0023774,0015435,
+0016142,0002353,0011175,0170530,
+};
+#endif
+#ifdef IBMPC
+static unsigned short gn[44] = {
+0xcaca,0xb420,0x2463,0x3fe0,
+0x7481,0x67f8,0x3aaa,0x3fc9,
+0x3214,0x54ba,0x3718,0x3f93,
+0xb33b,0x48d1,0x6a79,0x3f46,
+0xabf3,0xf9fa,0x2577,0x3ee8,
+0x6091,0x4dea,0x621c,0x3e7a,
+0xeb09,0xf200,0x9a94,0x3dfe,
+0x89cb,0x8766,0x0bf5,0x3d73,
+0xa964,0x3df8,0xc7a0,0x3cd8,
+0x58a6,0xf173,0xdb24,0x3c2e,
+0xbe2b,0x624f,0x409d,0x3b6c,
+};
+static unsigned short gd[44] = {
+/*0x0000,0x0000,0x0000,0x3ff0,*/
+0x55c1,0x23bc,0x996d,0x3ff7,
+0xc34f,0xefa1,0x9dad,0x3fd5,
+0xaa09,0xe637,0xf811,0x3f99,
+0xae28,0x0fa0,0xb206,0x3f4a,
+0x067b,0x2c0a,0xbf86,0x3eea,
+0x7428,0xab1c,0x0071,0x3e7c,
+0xf1c6,0x8e3c,0xa861,0x3dff,
+0xef2b,0x9c3d,0x6643,0x3d73,
+0x1936,0x37c8,0x00dc,0x3cd9,
+0x8364,0x84ff,0xf5a1,0x3c2e,
+0xbe2b,0x624f,0x409d,0x3b6c,
+};
+#endif
+#ifdef MIEEE
+static unsigned short gn[44] = {
+0x3fe0,0x2463,0xb420,0xcaca,
+0x3fc9,0x3aaa,0x67f8,0x7481,
+0x3f93,0x3718,0x54ba,0x3214,
+0x3f46,0x6a79,0x48d1,0xb33b,
+0x3ee8,0x2577,0xf9fa,0xabf3,
+0x3e7a,0x621c,0x4dea,0x6091,
+0x3dfe,0x9a94,0xf200,0xeb09,
+0x3d73,0x0bf5,0x8766,0x89cb,
+0x3cd8,0xc7a0,0x3df8,0xa964,
+0x3c2e,0xdb24,0xf173,0x58a6,
+0x3b6c,0x409d,0x624f,0xbe2b,
+};
+static unsigned short gd[44] = {
+/*0x3ff0,0x0000,0x0000,0x0000,*/
+0x3ff7,0x996d,0x23bc,0x55c1,
+0x3fd5,0x9dad,0xefa1,0xc34f,
+0x3f99,0xf811,0xe637,0xaa09,
+0x3f4a,0xb206,0x0fa0,0xae28,
+0x3eea,0xbf86,0x2c0a,0x067b,
+0x3e7c,0x0071,0xab1c,0x7428,
+0x3dff,0xa861,0x8e3c,0xf1c6,
+0x3d73,0x6643,0x9c3d,0xef2b,
+0x3cd9,0x00dc,0x37c8,0x1936,
+0x3c2e,0xf5a1,0x84ff,0x8364,
+0x3b6c,0x409d,0x624f,0xbe2b,
+};
+#endif
+
+#ifdef ANSIPROT
+extern "C" {double fabs ( double );}
+extern "C" {double cos ( double );}
+extern "C" {double sin ( double );}
+extern "C" {double polevl ( double, double *, int );}
+extern "C" {double p1evl ( double, double *, int );}
+#else
+double fabs(), cos(), sin(), polevl(), p1evl();
+#endif
+extern "C" {extern double PI, PIO2, MACHEP;}
+
+//int fresnl( xxa, ssa, cca )
+//double xxa, *ssa, *cca;
+
+int fresnl( double xxa, double *ssa, double *cca )
+{
+double f, g, cc, ss, c, s, t, u;
+double x, x2;
+
+x = fabs(xxa);
+x2 = x * x;
+if( x2 < 2.5625 )
+	{
+	t = x2 * x2;
+	ss = x * x2 * polevl( t, sn, 5)/p1evl( t, sd, 6 );
+	cc = x * polevl( t, cn, 5)/polevl(t, cd, 6 );
+	goto done;
+	}
+
+
+
+
+
+
+if( x > 36974.0 )
+	{
+	cc = 0.5;
+	ss = 0.5;
+	goto done;
+	}
+
+
+/*		Asymptotic power series auxiliary functions
+ *		for large argument
+ */
+	x2 = x * x;
+	t = PI * x2;
+	u = 1.0/(t * t);
+	t = 1.0/t;
+	f = 1.0 - u * polevl( u, fn, 9)/p1evl(u, fd, 10);
+	g = t * polevl( u, gn, 10)/p1evl(u, gd, 11);
+
+	t = PIO2 * x2;
+	c = cos(t);
+	s = sin(t);
+	t = PI * x;
+	cc = 0.5  +  (f * s  -  g * c)/t;
+	ss = 0.5  -  (f * c  +  g * s)/t;
+
+done:
+if( xxa < 0.0 )
+	{
+	cc = -cc;
+	ss = -ss;
+	}
+
+*cca = cc;
+*ssa = ss;
+return(0);
+}

src/driver/driver_map_xodrload_new/globalplan.cpp

@@ -0,0 +1,3028 @@
+#include "globalplan.h"
+#include "limits"
+#include <iostream>
+
+#include <Eigen/Dense>
+#include <Eigen/Cholesky>
+#include <Eigen/LU>
+#include <Eigen/QR>
+#include <Eigen/SVD>
+
+#include <QDebug>
+#include <QPointF>
+
+
+using namespace  Eigen;
+
+extern "C"
+{
+#include "dubins.h"
+}
+
+
+static void AddSignalMark(Road * pRoad, int nlane,std::vector<PlanPoint> & xvectorPP);
+/**
+ * @brief GetLineDis 获得点到直线Geometry的距离。
+ * @param pline
+ * @param x
+ * @param y
+ * @param nearx
+ * @param neary
+ * @param nearhead
+ * @return
+ */
+static double GetLineDis(GeometryLine * pline,const double x,const double y,double & nearx,
+                         double & neary,double & nearhead)
+{
+    double fRtn = 1000.0;
+
+    double a1,a2,a3,a4,b1,b2;
+    double ratio = pline->GetHdg();
+    while(ratio >= 2.0* M_PI)ratio = ratio-2.0*M_PI;
+    while(ratio<0)ratio = ratio+2.0*M_PI;
+
+    double dis1,dis2,dis3;
+    double x1,x2,x3,y1,y2,y3;
+    x1 = pline->GetX();y1=pline->GetY();
+    if((ratio == 0)||(ratio == M_PI))
+    {
+        a1 = 0;a4=0;
+        a2 = 1;b1= pline->GetY();
+        a3 = 1;b2= x;
+    }
+    else
+    {
+        if((ratio == 0.5*M_PI)||(ratio == 1.5*M_PI))
+        {
+            a2=0;a3=0;
+            a1=1,b1=pline->GetX();
+            a4 = 1;b2 = y;
+        }
+        else
+        {
+            a1 = tan(ratio) *(-1.0);
+            a2 = 1;
+            a3 = tan(ratio+M_PI/2.0)*(-1.0);
+            a4 = 1;
+            b1 = a1*pline->GetX() + a2 * pline->GetY();
+            b2 = a3*x+a4*y;
+        }
+    }
+
+    y2 = y1 + pline->GetLength() * sin(ratio);
+    x2 = x1 + pline->GetLength() * cos(ratio);
+
+    Eigen::Matrix2d A;
+    A<<a1,a2,
+            a3,a4;
+    Eigen::Vector2d B(b1,b2);
+
+    Eigen::Vector2d opoint  = A.lu().solve(B);
+
+ //   std::cout<<opoint<<std::endl;
+
+    x3 = opoint(0);
+    y3 = opoint(1);
+
+    dis1 = sqrt(pow(x1-x,2)+pow(y1-y,2));
+    dis2 = sqrt(pow(x2-x,2)+pow(y2-y,2));
+    dis3 = sqrt(pow(x3-x,2)+pow(y3-y,2));
+
+//    std::cout<<" dis 1 is "<<dis1<<std::endl;
+//    std::cout<<" dis 2 is "<<dis2<<std::endl;
+//    std::cout<<" dis 3 is "<<dis3<<std::endl;
+
+    if((dis1>pline->GetLength())||(dis2>pline->GetLength()))  //Outoff line
+    {
+ //       std::cout<<" out line"<<std::endl;
+        if(dis1<dis2)
+        {
+            fRtn = dis1;
+            nearx = x1;neary=y1;nearhead = pline->GetHdg();
+        }
+        else
+        {
+            fRtn = dis2;
+            nearx = x2;neary=y2;nearhead = pline->GetHdg();
+        }
+    }
+    else
+    {
+        fRtn = dis3;
+        nearx = x3;neary=y3;nearhead = pline->GetHdg();
+    }
+
+
+//    std::cout<<"dis is "<<fRtn<<" x is "<<nearx<<" y is "<<neary<<" head is "<<nearhead<<std::endl;
+    return fRtn;
+
+
+
+}
+
+
+static int getcirclecrosspoint(QPointF startPos, QPointF endPos, QPointF agvPos, double R,QPointF & point1,QPointF & point2 )
+{
+     double m=0;
+     double k;
+     double b;
+
+      //计算分子
+      m=startPos.x()-endPos.x();
+
+      //求直线的方程
+      if(0==m)
+      {
+          k=100000;
+          b=startPos.y()-k*startPos.x();
+      }
+      else
+      {
+          k=(endPos.y()-startPos.y())/(endPos.x()-startPos.x());
+          b=startPos.y()-k*startPos.x();
+      }
+//      qDebug()<<k<<b;
+
+     double temp = fabs(k*agvPos.x()-agvPos.y()+b)/sqrt(k*k+b*b);
+      //求直线与圆的交点 前提是圆需要与直线有交点
+     if(fabs(k*agvPos.x()-agvPos.y()+b)/sqrt(k*k+b*b)<R)
+      {
+          point1.setX((2*agvPos.x()-2*k*(b-agvPos.y())+sqrt(pow((2*k*(b-agvPos.y())-2*agvPos.x()),2)-4*(k*k+1)*((b-agvPos.y())*(b-agvPos.y())+agvPos.x()*agvPos.x()-R*R)))/(2*k*k+2));
+          point2.setX((2*agvPos.x()-2*k*(b-agvPos.y())-sqrt(pow((2*k*(b-agvPos.y())-2*agvPos.x()),2)-4*(k*k+1)*((b-agvPos.y())*(b-agvPos.y())+agvPos.x()*agvPos.x()-R*R)))/(2*k*k+2));
+          point1.setY(k*point1.x()+b);
+          point2.setY(k*point2.x()+b);
+          return 0;
+      }
+
+     return -1;
+}
+
+
+/**
+  * @brief CalcHdg
+  * 计算点0到点1的航向
+  * @param p0        Point 0
+  * @param p1        Point 1
+**/
+static double CalcHdg(QPointF p0, QPointF p1)
+{
+    double x0,y0,x1,y1;
+    x0 = p0.x();
+    y0 = p0.y();
+    x1 = p1.x();
+    y1 = p1.y();
+    if(x0 == x1)
+    {
+        if(y0 < y1)
+        {
+            return M_PI/2.0;
+        }
+        else
+            return M_PI*3.0/2.0;
+    }
+
+    double ratio = (y1-y0)/(x1-x0);
+
+    double hdg = atan(ratio);
+
+    if(ratio > 0)
+    {
+        if(y1 > y0)
+        {
+
+        }
+        else
+        {
+            hdg = hdg + M_PI;
+        }
+    }
+    else
+    {
+        if(y1 > y0)
+        {
+            hdg = hdg + M_PI;
+        }
+        else
+        {
+            hdg = hdg + 2.0*M_PI;
+        }
+    }
+
+    return hdg;
+}
+
+
+
+static bool pointinarc(GeometryArc * parc,QPointF poingarc,QPointF point1)
+{
+    double hdg = CalcHdg(poingarc,point1);
+    if(parc->GetCurvature() >0)hdg = hdg + M_PI/2.0;
+    else hdg = hdg - M_PI/2.0;
+    if(hdg >= 2.0*M_PI)hdg = hdg - 2.0*M_PI;
+    if(hdg < 0)hdg = hdg + 2.0*M_PI;
+
+    double hdgrange = parc->GetLength()/(1.0/parc->GetCurvature());
+
+    double hdgdiff = hdg - parc->GetHdg();
+
+    if(hdgrange >= 0 )
+    {
+        if(hdgdiff < 0)hdgdiff = hdgdiff + M_PI*2.0;
+    }
+    else
+    {
+        if(hdgdiff > 0)hdgdiff = hdgdiff - M_PI*2.0;
+    }
+
+    if(fabs(hdgdiff ) < fabs(hdgrange))return true;
+    return false;
+
+}
+
+static inline double calcpointdis(QPointF p1,QPointF p2)
+{
+    return sqrt(pow(p1.x()-p2.x(),2)+pow(p1.y()-p2.y(),2));
+}
+
+/**
+  * @brief GetArcDis
+  * 计算点到圆弧的最短距离，首先用点和圆弧中心点的直线与圆的交点，计算点到交点的距离，如果交点在圆弧上，则最短距离是交点之一，否则计算点到圆弧两个端点的距离。
+  * @param parc        pointer to a arc geomery
+  * @param x           current x
+  * @param y           current y
+  * @param nearx       near x
+  * @param neary       near y
+  * @param nearhead    nearhead
+**/
+
+static double GetArcDis(GeometryArc * parc,double x,double y,double & nearx,
+                        double & neary,double & nearhead)
+{
+
+ //   double fRtn = 1000.0;
+    if(parc->GetCurvature() == 0.0)return 1000.0;
+
+    double R = fabs(1.0/parc->GetCurvature());
+
+//    if(parc->GetX() == 4.8213842690322309e+01)
+//    {
+//        int a = 1;
+//        a = 3;
+//    }
+//    if(parc->GetCurvature() == -0.0000110203)
+//    {
+//        int a = 1;
+//        a = 3;
+//    }
+
+    //calculate arc center
+    double x_center,y_center;
+    if(parc->GetCurvature() > 0)
+    {
+        x_center = parc->GetX() + R * cos(parc->GetHdg() + M_PI/2.0);
+        y_center = parc->GetY() + R * sin(parc->GetHdg()+ M_PI/2.0);
+    }
+    else
+    {
+        x_center = parc->GetX() + R * cos(parc->GetHdg() - M_PI/2.0);
+        y_center = parc->GetY() + R * sin(parc->GetHdg() - M_PI/2.0);
+    }
+
+    double hdgltoa = CalcHdg(QPointF(x,y),QPointF(x_center,y_center));
+
+
+    QPointF arcpoint;
+    arcpoint.setX(x_center);arcpoint.setY(y_center);
+
+    QPointF pointnow;
+    pointnow.setX(x);pointnow.setY(y);
+    QPointF point1,point2;
+    point1.setX(x_center + (R * cos(hdgltoa)));
+    point1.setY(y_center + (R * sin(hdgltoa)));
+    point2.setX(x_center + (R * cos(hdgltoa + M_PI)));
+    point2.setY(y_center + (R * sin(hdgltoa + M_PI)));
+
+    //calculat dis
+    bool bp1inarc,bp2inarc;
+    bp1inarc =pointinarc(parc,arcpoint,point1);
+    bp2inarc =pointinarc(parc,arcpoint,point2);
+    double fdis[4];
+    fdis[0] = calcpointdis(pointnow,point1);
+    fdis[1] = calcpointdis(pointnow,point2);
+    fdis[2] = calcpointdis(pointnow,QPointF(parc->GetX(),parc->GetY()));
+    QPointF pointend;
+    double hdgrange = parc->GetLength()*parc->GetCurvature();
+    double hdgend = parc->GetHdg() + hdgrange;
+    while(hdgend <0.0)hdgend = hdgend + 2.0 *M_PI;
+    while(hdgend >= 2.0*M_PI) hdgend = hdgend -2.0*M_PI;
+
+    if(parc->GetCurvature() >0)
+    {
+        pointend.setX(arcpoint.x() + R*cos(hdgend -M_PI/2.0 ));
+        pointend.setY(arcpoint.y() + R*sin(hdgend -M_PI/2.0) );
+    }
+    else
+    {
+        pointend.setX(arcpoint.x() + R*cos(hdgend +M_PI/2.0 ));
+        pointend.setY(arcpoint.y() + R*sin(hdgend +M_PI/2.0) );
+    }
+
+    fdis[3] = calcpointdis(pointnow,pointend);
+    int indexmin = -1;
+    double fdismin = 1000000.0;
+    if(bp1inarc)
+    {
+        indexmin = 0;fdismin = fdis[0];
+    }
+    if(bp2inarc)
+    {
+        if(indexmin == -1)
+        {
+            indexmin = 1;fdismin = fdis[1];
+        }
+        else
+        {
+            if(fdis[1]<fdismin)
+            {
+                indexmin = 1;fdismin = fdis[1];
+            }
+        }
+    }
+    if(indexmin == -1)
+    {
+        indexmin = 2;fdismin = fdis[2];
+    }
+    else
+    {
+        if(fdis[2]<fdismin)
+        {
+            indexmin = 2;fdismin = fdis[2];
+        }
+    }
+    if(fdis[3]<fdismin)
+    {
+        indexmin = 3;fdismin = fdis[3];
+    }
+
+    switch (indexmin) {
+    case 0:
+        nearx = point1.x();
+        neary = point1.y();
+        if(parc->GetCurvature()<0)
+        {
+            nearhead = CalcHdg(arcpoint,point1) - M_PI/2.0;
+        }
+        else
+        {
+            nearhead = CalcHdg(arcpoint,point1) + M_PI/2.0;
+        }
+        break;
+    case 1:
+        nearx = point2.x();
+        neary = point2.y();
+        if(parc->GetCurvature()<0)
+        {
+            nearhead = CalcHdg(arcpoint,point2) - M_PI/2.0;
+        }
+        else
+        {
+            nearhead = CalcHdg(arcpoint,point2) + M_PI/2.0;
+        }
+        break;
+    case 2:
+        nearx = parc->GetX();
+        neary = parc->GetY();
+        nearhead = parc->GetHdg();
+        break;
+    case 3:
+        nearx = pointend.x();
+        neary = pointend.y();
+        nearhead = hdgend;
+        break;
+    default:
+        std::cout<<"error in arcdis "<<std::endl;
+        break;
+    }
+
+    while(nearhead>2.0*M_PI)nearhead = nearhead -2.0*M_PI;
+    while(nearhead<0)nearhead = nearhead + 2.0*M_PI;
+    return fdismin;
+}
+
+
+static double GetSpiralDis(GeometrySpiral * pspiral,double xnow,double ynow,double & nearx,
+                        double & neary,double & nearhead)
+{
+
+    double x,y,hdg;
+    double s = 0.0;
+    double fdismin = 100000.0;
+    double s0 = pspiral->GetS();
+
+    while(s<pspiral->GetLength())
+    {
+        pspiral->GetCoords(s0+s,x,y,hdg);
+        s = s+0.1;
+
+        double fdis = calcpointdis(QPointF(x,y),QPointF(xnow,ynow));
+        if(fdis<fdismin)
+        {
+            fdismin = fdis;
+            nearhead = hdg;
+            nearx = x;
+            neary = y;
+        }
+    }
+
+    return fdismin;
+}
+
+/**
+ * @brief GetParamPoly3Dis 获得点到贝塞尔曲线的距离。
+ * @param parc
+ * @param xnow
+ * @param ynow
+ * @param nearx
+ * @param neary
+ * @param nearhead
+ * @return
+ */
+static double GetParamPoly3Dis(GeometryParamPoly3 * parc,double xnow,double ynow,double & nearx,
+                        double & neary,double & nearhead)
+{
+
+    double s = 0.1;
+    double fdismin = 100000.0;
+
+    double xold,yold;
+    xold = parc->GetX();
+    yold = parc->GetY();
+
+    double fdis = calcpointdis(QPointF(parc->GetX(),parc->GetY()),QPointF(xnow,ynow));
+    if(fdis<fdismin)
+    {
+        fdismin = fdis;
+        nearhead = parc->GetHdg();
+        nearx = parc->GetX();
+        neary = parc->GetY();
+    }
+
+    double s_start = parc->GetS();
+
+    while(s < parc->GetLength())
+    {
+
+        double x, y,hdg;//xtem,ytem;
+        parc->GetCoords(s_start+s,x,y,hdg);
+        if(fdis<fdismin)
+        {
+            fdismin = fdis;
+            nearhead = hdg;
+            nearx = x;
+            neary = y;
+        }
+
+//        xold = x;
+//        yold = y;
+        s = s+ 0.1;
+    }
+
+    return fdismin;
+
+}
+
+
+static double GetPoly3Dis(GeometryPoly3 * ppoly,double xnow,double ynow,double & nearx,
+                        double & neary,double & nearhead)
+{
+    double x,y,hdg;
+//    double s = 0.0;
+    double fdismin = 100000.0;
+ //   double s0 = ppoly->GetS();
+
+    x = ppoly->GetX();
+    y = ppoly->GetY();
+    double A,B,C,D;
+    A = ppoly->GetA();
+    B = ppoly->GetB();
+    C = ppoly->GetC();
+    D = ppoly->GetD();
+    const double steplim = 0.3;
+    double du = steplim;
+    double u = 0;
+    double v = 0;
+    double oldx,oldy;
+    oldx = x;
+    oldy = y;
+    double xstart,ystart;
+    xstart = x;
+    ystart = y;
+
+
+    double hdgstart = ppoly->GetHdg();
+    double flen = 0;
+    u = u+du;
+    while(flen < ppoly->GetLength())
+    {
+        double fdis = 0;
+        v = A + B*u + C*u*u + D*u*u*u;
+        x = xstart + u*cos(hdgstart) - v*sin(hdgstart);
+        y = ystart + u*sin(hdgstart) + v*cos(hdgstart);
+        fdis = sqrt(pow(x- oldx,2)+pow(y-oldy,2));
+
+        if(fdis>(steplim*2.0))du = du/2.0;
+        flen = flen + fdis;
+        u = u + du;
+        hdg = CalcHdg(QPointF(oldx,oldy),QPointF(x,y));
+
+        double fdisnow = calcpointdis(QPointF(x,y),QPointF(xnow,ynow));
+        if(fdisnow<fdismin)
+        {
+            fdismin = fdisnow;
+            nearhead = hdg;
+            nearx = x;
+            neary = y;
+        }
+
+        oldx = x;
+        oldy = y;
+    }
+
+    return fdismin;
+}
+
+
+/**
+  * @brief GetNearPoint
+  * 计算点到圆弧的最短距离，首先用点和圆弧中心点的直线与圆的交点，计算点到交点的距离，如果交点在圆弧上，则最短距离是交点之一，否则计算点到圆弧两个端点的距离。
+  * @param x           current x
+  * @param y           current y
+  * @param pxodr       OpenDrive
+  * @param pObjRoad    Road
+  * @param pgeo        Geometry of road
+  * @param nearx       near x
+  * @param neary       near y
+  * @param nearhead    nearhead
+  * @param nearthresh  near threshhold
+  * @retval            if == 0 successfull  <0 fail.
+**/
+int GetNearPoint(const double x,const double y,OpenDrive * pxodr,Road ** pObjRoad,GeometryBlock ** pgeo, double & s,double & nearx,
+                 double & neary,double & nearhead,const double nearthresh,double &froads)
+{
+
+    double dismin = std::numeric_limits<double>::infinity();
+    s = dismin;
+    int i;
+    double frels;
+    for(i=0;i<pxodr->GetRoadCount();i++)
+    {
+        int j;
+        Road * proad = pxodr->GetRoad(i);
+        double nx,ny,nh;
+
+        for(j=0;j<proad->GetGeometryBlockCount();j++)
+        {
+            GeometryBlock * pgb = proad->GetGeometryBlock(j);
+            double dis;
+            RoadGeometry * pg;
+            pg = pgb->GetGeometryAt(0);
+
+            switch (pg->GetGeomType()) {
+            case 0:   //line
+                dis = xodrfunc::GetLineDis((GeometryLine *) pg,x,y,nx,ny,nh,frels);
+ //               dis = GetLineDis((GeometryLine *) pg,x,y,nx,ny,nh);
+                break;
+            case 1:
+                dis = xodrfunc::GetSpiralDis((GeometrySpiral *)pg,x,y,nx,ny,nh,frels);
+                break;
+            case 2:  //arc
+                dis = xodrfunc::GetArcDis((GeometryArc *)pg,x,y,nx,ny,nh,frels);
+                break;
+
+            case 3:
+                dis = xodrfunc::GetPoly3Dis((GeometryPoly3 *)pg,x,y,nx,ny,nh,frels);
+                break;
+            case 4:
+                dis = xodrfunc::GetParamPoly3Dis((GeometryParamPoly3 *)pg,x,y,nx,ny,nh,frels);
+                break;
+            default:
+                dis = 100000.0;
+                break;
+            }
+
+            if(dis < dismin)
+            {
+                dismin = dis;
+                nearx = nx;
+                neary = ny;
+                nearhead = nh;
+                s = dis;
+                froads = frels;
+                *pObjRoad = proad;
+                *pgeo = pgb;
+            }
+
+
+//            if(pgb->CheckIfLine())
+//            {
+//                GeometryLine * pline = (GeometryLine *)pgb->GetGeometryAt(0);
+
+//                double dis = GetLineDis(pline,x,y,nx,ny,nh);
+//                if(dis < dismin)
+//                {
+//                    dismin = dis;
+//                    nearx = nx;
+//                    neary = ny;
+//                    nearhead = nh;
+//                    s = dis;
+//                    *pObjRoad = proad;
+//                    *pgeo = pgb;
+//                }
+//            }
+//            else
+//            {
+//                GeometryLine * pline1 = (GeometryLine *)pgb->GetGeometryAt(0);
+//                double dis = GetLineDis(pline1,x,y,nx,ny,nh);
+//                if(dis < dismin)
+//                {
+//                    dismin = dis;
+//                    nearx = nx;
+//                    neary = ny;
+//                    nearhead = nh;
+//                    s = dis;
+//                    *pObjRoad = proad;
+//                    *pgeo = pgb;
+//                }
+//                GeometryArc * parc = (GeometryArc *)pgb->GetGeometryAt(1);
+//                dis = GetArcDis(parc,x,y,nx,ny,nh);
+//                if(dis < dismin)
+//                {
+//                    dismin = dis;
+//                    nearx = nx;
+//                    neary = ny;
+//                    nearhead = nh;
+//                    s = dis;
+//                    *pObjRoad = proad;
+//                    *pgeo = pgb;
+//                }
+//                pline1 = (GeometryLine *)pgb->GetGeometryAt(2);
+//                dis = GetLineDis(pline1,x,y,nx,ny,nh);
+//                if(dis < dismin)
+//                {
+//                    dismin = dis;
+//                    nearx = nx;
+//                    neary = ny;
+//                    nearhead = nh;
+//                    s = dis;
+//                    *pObjRoad = proad;
+//                    *pgeo = pgb;
+//                }
+//            }
+
+        }
+    }
+    if(s > nearthresh)return -1;
+    return 0;
+}
+
+
+namespace iv {
+struct nearroad
+{
+Road * pObjRoad;
+GeometryBlock * pgeob;
+double nearx;
+double neary;
+double nearhead;
+double frels;
+double fdis;
+int lr = 2; //1 left 2 right;
+int nmode = 0; //0 same direciion dis zero   1 oposite dis = 0 2 same direction dis<5 3 oposite  dis <5   4 same direction > 5 5 oposite direction;
+};
+}
+
+int GetNearPointWithHead(const double x,const double y,const double hdg,OpenDrive * pxodr,std::vector<iv::nearroad> & xvectornear,
+                         const double nearthresh,bool bhdgvalid = true)
+{
+    int i;
+    double frels;
+    int nminmode = 5;
+    for(i=0;i<pxodr->GetRoadCount();i++)
+    {
+        int j;
+        Road * proad = pxodr->GetRoad(i);
+        double nx,ny,nh;
+
+        bool bchange = false;
+        double localdismin = std::numeric_limits<double>::infinity();
+
+        double localnearx = 0;
+        double localneary = 0;
+        double localnearhead = 0;
+        double locals = 0;
+        double localfrels = 0;
+        GeometryBlock * pgeolocal;
+
+        for(j=0;j<proad->GetGeometryBlockCount();j++)
+        {
+            localdismin = std::numeric_limits<double>::infinity();
+            GeometryBlock * pgb = proad->GetGeometryBlock(j);
+            double dis;
+            RoadGeometry * pg;
+            pg = pgb->GetGeometryAt(0);
+
+            switch (pg->GetGeomType()) {
+            case 0:   //line
+                dis = xodrfunc::GetLineDis((GeometryLine *) pg,x,y,nx,ny,nh,frels);
+                break;
+            case 1:
+                dis = xodrfunc::GetSpiralDis((GeometrySpiral *)pg,x,y,nx,ny,nh,frels);
+                break;
+            case 2:  //arc
+                dis = xodrfunc::GetArcDis((GeometryArc *)pg,x,y,nx,ny,nh,frels);
+                break;
+
+            case 3:
+                dis = xodrfunc::GetPoly3Dis((GeometryPoly3 *)pg,x,y,nx,ny,nh,frels);
+                break;
+            case 4:
+                dis = xodrfunc::GetParamPoly3Dis((GeometryParamPoly3 *)pg,x,y,nx,ny,nh,frels);
+                break;
+            default:
+                dis = 100000.0;
+                break;
+            }
+
+
+            if(dis<localdismin)
+            {
+                localdismin = dis;
+                localnearx = nx;
+                localneary = ny;
+                localnearhead = nh;
+                locals = dis;
+                localfrels = frels;
+                pgeolocal = pgb;
+                bchange = true;
+            }
+
+
+        }
+
+        if(localdismin < nearthresh)
+        {
+            iv::nearroad xnear;
+            xnear.pObjRoad = proad;
+            xnear.pgeob = pgeolocal;
+            xnear.nearx = localnearx;
+            xnear.neary = localneary;
+            xnear.fdis = localdismin;
+            xnear.nearhead = localnearhead;
+            xnear.frels = localfrels;
+            if(xnear.fdis>0)
+            {
+                double fcalhdg = xodrfunc::CalcHdg(QPointF(xnear.nearx,xnear.neary),QPointF(x,y));
+                double fhdgdiff = fcalhdg - xnear.nearhead;
+                while(fhdgdiff<0)fhdgdiff = fhdgdiff + 2.0*M_PI;
+                while(fhdgdiff>= 2.0*M_PI)fhdgdiff = fhdgdiff - 2.0*M_PI;
+                if(fhdgdiff<M_PI)
+                {
+                    double fdisroad = proad->GetRoadLeftWidth(xnear.frels);
+                    if(fdisroad>xnear.fdis)
+                    {
+                        xnear.fdis = 0;
+                    }
+                    else
+                    {
+                        xnear.fdis = xnear.fdis - fdisroad;
+                    }
+                    xnear.lr = 1;
+                }
+                else
+                {
+                    double fdisroad = proad->GetRoadRightWidth(xnear.frels);
+                    if(fdisroad>xnear.fdis)
+                    {
+                        xnear.fdis = 0;
+                    }
+                    else
+                    {
+                        xnear.fdis = xnear.fdis - fdisroad;
+                    }
+                    xnear.lr = 2;
+                }
+
+
+            }
+            else
+            {
+                if(bhdgvalid == false)
+                {
+                    if(xnear.pObjRoad->GetLaneSectionCount()>0)
+                    {
+                        LaneSection * pLS = xnear.pObjRoad->GetLaneSection(0);
+                        if(pLS->GetRightLaneCount()>0)
+                        {
+                            xnear.lr = 2;
+                        }
+                        else
+                        {
+                            xnear.lr = 1;
+                        }
+                    }
+                    else
+                    {
+                        xnear.lr = 2;
+                    }
+                }
+            }
+            LaneSection * pLS = xnear.pObjRoad->GetLaneSection(0);
+            if(pLS != NULL)
+            {
+            if(xnear.fdis < 0.00001)
+            {
+                if(bhdgvalid == false)
+                {
+                    xnear.nmode = 0;
+                }
+                else
+                {
+                    double headdiff = hdg - xnear.nearhead;
+                    while(headdiff < 2.0*M_PI) headdiff = headdiff + 2.0*M_PI;
+                    while(headdiff >= 2.0*M_PI)headdiff = headdiff - 2.0*M_PI;
+                    if(((headdiff <M_PI/2.0)||(headdiff > M_PI*3.0/2.0))&&(pLS->GetRightLaneCount()>0))
+                    {
+                       xnear.nmode = 0;
+                    }
+                    else
+                    {
+                        xnear.nmode = 1;
+                    }
+                }
+
+            }
+            else
+            {
+                if(xnear.fdis<5)
+                {
+                    if(bhdgvalid == false)
+                    {
+                        xnear.nmode = 2;
+                    }
+                    else
+                    {
+                        double headdiff = hdg - xnear.nearhead;
+                        while(headdiff < 2.0*M_PI) headdiff = headdiff + 2.0*M_PI;
+                        while(headdiff >= 2.0*M_PI)headdiff = headdiff - 2.0*M_PI;
+                        if(((headdiff <M_PI/2.0)||(headdiff > M_PI*3.0/2.0))&&(pLS->GetRightLaneCount()>0))
+                        {
+                            xnear.nmode = 2;
+                        }
+                        else
+                        {
+                            xnear.nmode = 3;
+                        }
+                    }
+
+                }
+                else
+                {
+                    if(bhdgvalid == false)
+                    {
+                        xnear.nmode = 4;
+                    }
+                    else
+                    {
+                        double headdiff = hdg - xnear.nearhead;
+                        while(headdiff < 2.0*M_PI) headdiff = headdiff + 2.0*M_PI;
+                        while(headdiff >= 2.0*M_PI)headdiff = headdiff - 2.0*M_PI;
+                        if(((headdiff <M_PI/2.0)||(headdiff > M_PI*3.0/2.0))&&(pLS->GetRightLaneCount()>0))
+                        {
+                            xnear.nmode = 4;
+                        }
+                        else
+                        {
+                            xnear.nmode = 5;
+                        }
+                    }
+                }
+            }
+            if(xnear.nmode < nminmode)nminmode = xnear.nmode;
+            xvectornear.push_back(xnear);
+            }
+        }
+
+    }
+
+    if(xvectornear.size() == 0)return -1;
+
+
+    if(xvectornear.size() > 1)
+    {
+        int i;
+        for(i=0;i<(int)xvectornear.size();i++)
+        {
+            if(xvectornear[i].nmode > nminmode)
+            {
+                xvectornear.erase(xvectornear.begin() + i);
+                i = i-1;
+            }
+        }
+    }
+    if((xvectornear.size()>1)&&(nminmode>=4))  //if dis > 5 select small dis
+    {
+        int i;
+        while(xvectornear.size()>1)
+        {
+            if(xvectornear[1].fdis < xvectornear[0].fdis)
+            {
+                xvectornear.erase(xvectornear.begin());
+            }
+            else
+            {
+                xvectornear.erase(xvectornear.begin()+1);
+            }
+        }
+    }
+    return 0;
+
+}
+
+
+
+/**
+  * @brief SelectRoadLeftRight
+  * 选择左侧道路或右侧道路
+  * 1.选择角度差小于90度的道路一侧，即同侧道路
+  * 2.单行路，选择存在的那一侧道路。
+  * @param pRoad       道路
+  * @param head1       车的航向或目标点的航向
+  * @param head_road   目标点的航向
+  * @retval            1 left   2 right
+**/
+static int SelectRoadLeftRight(Road * pRoad,const double head1,const double  head_road)
+{
+    int nrtn = 2;
+
+
+    double headdiff = head1 - head_road;
+    while(headdiff < 2.0*M_PI) headdiff = headdiff + 2.0*M_PI;
+    while(headdiff >= 2.0*M_PI)headdiff = headdiff - 2.0*M_PI;
+
+    bool bSel = false;
+    if((headdiff >=M_PI/2.0)&&(headdiff <= M_PI*3.0/2.0))  //if diff is
+    {
+        if(pRoad->GetLaneSection(0)->GetLeftLaneCount()>0)
+        {
+
+            nrtn = 1;
+            bSel = true;
+        }
+    }
+    else
+    {
+        if(pRoad->GetLaneSection(0)->GetRightLaneCount()>0)
+        {
+            nrtn = 2;
+            bSel = true;
+        }
+    }
+
+    if(bSel == false)
+    {
+        if(pRoad->GetLaneSection(0)->GetLeftLaneCount() >0)
+        {
+            nrtn = 1;
+        }
+        else
+        {
+            nrtn = 2;
+        }
+    }
+
+    return nrtn;
+
+}
+
+
+
+//static double getlinereldis(GeometryLine * pline,double x,double y)
+//{
+//    double x0,y0;
+//    x0 = pline->GetX();
+//    y0 = pline->GetY();
+//    double dis = sqrt(pow(x-x0,2) + pow(y-y0,2));
+//    if(dis > pline->GetS())
+//    {
+//        std::cout<<"getlinereldis exceed s S is "<<pline->GetS()<<" dis is "<<dis<<std::endl;
+//        return dis;
+//    }
+//    return dis;
+//}
+
+//static double getarcreldis(GeometryArc * parc,double x,double y)
+//{
+//    double x0,y0;
+//    x0 = parc->GetX();
+//    y0 = parc->GetY();
+//    double dis = sqrt(pow(x-x0,2) + pow(y-y0,2));
+//    double R = fabs(1.0/parc->GetCurvature());
+//    double ang = 2.0* asin(dis/(2.0*R));
+//    double frtn = ang * R;
+//    return frtn;
+//}
+
+//static double getparampoly3reldis(GeometryParamPoly3 *parc, double xnow, double ynow)
+//{
+//    double s = 0.1;
+
+//    double xold,yold;
+//    xold = parc->GetX();
+//    yold = parc->GetY();
+
+
+//    while(s < parc->GetLength())
+//    {
+
+//        double x, y,xtem,ytem;
+//        xtem = parc->GetuA() + parc->GetuB() * s + parc->GetuC() * s*s + parc->GetuD() * s*s*s;
+//        ytem = parc->GetvA() + parc->GetvB() * s + parc->GetvC() * s*s + parc->GetvD() * s*s*s;
+//        x = xtem*cos(parc->GetHdg()) - ytem * sin(parc->GetHdg()) + parc->GetX();
+//        y = xtem*sin(parc->GetHdg()) + ytem * cos(parc->GetHdg()) + parc->GetY();
+
+//        if(sqrt(pow(xnow - x,2) + pow(ynow -y,2))<0.3)
+//        {
+//            break;
+//        }
+
+//        xold = x;
+//        yold = y;
+//        s = s+ 0.1;
+//    }
+
+//    return s;
+//}
+
+//static double getreldis(RoadGeometry * prg,double x,double y)
+//{
+//    int ngeotype = prg->GetGeomType();
+//    double frtn = 0;
+//    switch (ngeotype) {
+//    case 0:
+//        frtn = getlinereldis((GeometryLine *)prg,x,y);
+//        break;
+//    case 1:
+//        break;
+//    case 2:
+//        frtn = getarcreldis((GeometryArc *)prg,x,y);
+//        break;
+//    case 3:
+//        break;
+//    case 4:
+//        frtn = getparampoly3reldis((GeometryParamPoly3 *)prg,x,y);
+//        break;
+//    default:
+//        break;
+//    }
+
+//    return frtn;
+//}
+
+
+
+
+//static double getposinroad(Road * pRoad,GeometryBlock * pgeob,double x,double y)
+//{
+//    RoadGeometry* prg = pgeob->GetGeometryAt(0);
+//    double s1 = prg->GetS();
+//    double srtn = s1;
+
+//    return s1 + getreldis(prg,x,y);
+//}
+
+
+static std::vector<PlanPoint> getlinepoint(GeometryLine * pline,const double fspace = 0.1)
+{
+    std::vector<PlanPoint> xvectorPP;
+    int i;
+    double s = pline->GetLength();
+    int nsize = s/fspace;
+    if(nsize ==0)nsize = 1;
+
+    for(i=0;i<nsize;i++)
+    {
+        double x,y;
+        x = pline->GetX() + i *fspace * cos(pline->GetHdg());
+        y = pline->GetY() + i *fspace * sin(pline->GetHdg());
+        PlanPoint pp;
+        pp.x = x;
+        pp.y = y;
+        pp.dis = i*fspace;
+        pp.hdg = pline->GetHdg();
+        pp.mS = pline->GetS() + i*fspace;
+        xvectorPP.push_back(pp);
+    }
+    return xvectorPP;
+}
+
+static std::vector<PlanPoint> getarcpoint(GeometryArc * parc,const double fspace = 0.1)
+{
+    std::vector<PlanPoint> xvectorPP;
+
+    //   double fRtn = 1000.0;
+    if(parc->GetCurvature() == 0.0)return xvectorPP;
+
+    double R = fabs(1.0/parc->GetCurvature());
+
+    //calculate arc center
+    double x_center = parc->GetX() + (1.0/parc->GetCurvature()) * cos(parc->GetHdg() + M_PI/2.0);
+    double y_center = parc->GetY() + (1.0/parc->GetCurvature()) * sin(parc->GetHdg()+ M_PI/2.0);
+
+    double arcdiff = fspace/R;
+    int nsize = parc->GetLength()/fspace;
+    if(nsize == 0)nsize = 1;
+    int i;
+    for(i=0;i<nsize;i++)
+    {
+        double x,y;
+        PlanPoint pp;
+        if(parc->GetCurvature() > 0)
+        {
+            x = x_center + R * cos(parc->GetHdg() + i*arcdiff - M_PI/2.0);
+            y = y_center + R * sin(parc->GetHdg() + i*arcdiff - M_PI/2.0);
+            pp.hdg = parc->GetHdg() + i*arcdiff;
+        }
+        else
+        {
+            x = x_center + R * cos(parc->GetHdg() -i*arcdiff + M_PI/2.0);
+            y = y_center + R * sin(parc->GetHdg() -i*arcdiff + M_PI/2.0);
+            pp.hdg = parc->GetHdg() - i*arcdiff;
+        }
+
+        pp.x = x;
+        pp.y = y;
+        pp.dis = i*fspace;
+        pp.mS = parc->GetS() + i*fspace;
+        xvectorPP.push_back(pp);
+    }
+    return xvectorPP;
+}
+
+
+static std::vector<PlanPoint> getspiralpoint(GeometrySpiral * pspiral,const double fspace = 0.1)
+{
+
+    double x,y,hdg;
+    double s = 0.0;
+    double s0 = pspiral->GetS();
+
+    std::vector<PlanPoint> xvectorPP;
+    PlanPoint pp;
+
+    while(s<pspiral->GetLength())
+    {
+        pspiral->GetCoords(s0+s,x,y,hdg);
+
+        pp.x = x;
+        pp.y = y;
+        pp.hdg = hdg;
+        pp.dis = s;
+        pp.mS = pspiral->GetS() + s;
+        xvectorPP.push_back(pp);
+
+        s = s+fspace;
+
+    }
+    return xvectorPP;
+}
+
+static std::vector<PlanPoint> getpoly3dpoint(GeometryPoly3 * ppoly,const double fspace = 0.1)
+{
+    double x,y;
+    x = ppoly->GetX();
+    y = ppoly->GetY();
+    double A,B,C,D;
+    A = ppoly->GetA();
+    B = ppoly->GetB();
+    C = ppoly->GetC();
+    D = ppoly->GetD();
+    const double steplim = fspace;
+    double du = steplim;
+    double u = 0;
+    double v = 0;
+    double oldx,oldy;
+    oldx = x;
+    oldy = y;
+    double xstart,ystart;
+    xstart = x;
+    ystart = y;
+    double hdgstart = ppoly->GetHdg();
+    double flen = 0;
+    std::vector<PlanPoint> xvectorPP;
+    PlanPoint pp;
+    pp.x = xstart;
+    pp.y = ystart;
+    pp.hdg = hdgstart;
+    pp.dis = 0;
+    pp.mS = ppoly->GetS();
+    xvectorPP.push_back(pp);
+    u = du;
+    while(flen < ppoly->GetLength())
+    {
+        double fdis = 0;
+        v = A + B*u + C*u*u + D*u*u*u;
+        x = xstart + u*cos(hdgstart) - v*sin(hdgstart);
+        y = ystart + u*sin(hdgstart) + v*cos(hdgstart);
+        fdis = sqrt(pow(x- oldx,2)+pow(y-oldy,2));
+        double hdg = CalcHdg(QPointF(oldx,oldy),QPointF(x,y));
+        oldx = x;
+        oldy = y;
+        if(fdis>(steplim*2.0))du = du/2.0;
+        flen = flen + fdis;
+        u = u + du;
+
+
+        pp.x = x;
+        pp.y = y;
+        pp.hdg = hdg;
+
+ //       s = s+ dt;
+        pp.dis = flen;;
+        pp.mS = ppoly->GetS() + flen;
+        xvectorPP.push_back(pp);
+    }
+
+    return xvectorPP;
+
+}
+
+static std::vector<PlanPoint> getparampoly3dpoint(GeometryParamPoly3 * parc,const double fspace = 0.1)
+{
+    double s = 0.1;
+    std::vector<PlanPoint> xvectorPP;
+    PlanPoint pp;
+
+    double xold,yold;
+    xold = parc->GetX();
+    yold = parc->GetY();
+    double hdg0 = parc->GetHdg();
+    pp.x = xold;
+    pp.y = yold;
+    pp.hdg = hdg0;
+    pp.dis = 0;
+//    xvectorPP.push_back(pp);
+
+    double dt = 1.0;
+    double tcount = parc->GetLength()/0.1;
+    if(tcount > 0)
+    {
+        dt = 1.0/tcount;
+    }
+    double t;
+    s = dt;
+    s = 0;
+
+    double ua,ub,uc,ud,va,vb,vc,vd;
+    ua = parc->GetuA();ub= parc->GetuB();uc= parc->GetuC();ud = parc->GetuD();
+    va = parc->GetvA();vb= parc->GetvB();vc= parc->GetvC();vd = parc->GetvD();
+
+    double s_start = parc->GetS();
+    while(s < parc->GetLength())
+    {
+        double x, y,hdg;
+        parc->GetCoords(s_start+s,x,y,hdg);
+        pp.x = x;
+        pp.y = y;
+        pp.hdg = hdg;
+        s = s+ fspace;
+        pp.dis = pp.dis + fspace;;
+        pp.mS = s_start + s;
+        xvectorPP.push_back(pp);
+    }
+    return xvectorPP;
+}
+
+
+std::vector<PlanPoint> GetPoint(pathsection xpath,const double fspace = 0.1)
+{
+    Road * pRoad = xpath.mpRoad;
+    //s_start  s_end for select now section data.
+    double s_start,s_end;
+    LaneSection * pLS = pRoad->GetLaneSection(xpath.msectionid);
+    s_start = pLS->GetS();
+    if(xpath.msectionid == (pRoad->GetLaneSectionCount()-1))s_end = pRoad->GetRoadLength();
+    else
+    {
+        s_end = pRoad->GetLaneSection(xpath.msectionid+1)->GetS();
+    }
+
+//    if(xpath.mroadid == 10190)
+//    {
+//        int a = 1;
+//        a++;
+//    }
+
+    std::vector<PlanPoint> xvectorPPS;
+    double s = 0;
+
+    int i;
+    for(i=0;i<pRoad->GetGeometryBlockCount();i++)
+    {
+        GeometryBlock * pgb =  pRoad->GetGeometryBlock(i);
+        RoadGeometry * prg = pgb->GetGeometryAt(0);
+        std::vector<PlanPoint> xvectorPP;
+        if(i<(pRoad->GetGeometryBlockCount() -0))
+        {
+            if(prg->GetS()>s_end)
+            {
+                continue;
+            }
+            if((prg->GetS() + prg->GetLength())<s_start)
+            {
+                continue;
+            }
+        }
+
+
+        double s1 = prg->GetLength();
+
+        switch (prg->GetGeomType()) {
+        case 0:
+            xvectorPP = getlinepoint((GeometryLine *)prg,fspace);
+
+            break;
+        case 1:
+            xvectorPP = getspiralpoint((GeometrySpiral *)prg,fspace);
+            break;
+        case 2:
+            xvectorPP = getarcpoint((GeometryArc *)prg,fspace);
+            break;
+        case 3:
+
+            xvectorPP = getpoly3dpoint((GeometryPoly3 *)prg,fspace);
+            break;
+        case 4:
+            xvectorPP = getparampoly3dpoint((GeometryParamPoly3 *)prg,fspace);
+            break;
+        default:
+            break;
+        }
+        int j;
+        if(prg->GetS()<s_start)
+        {
+            s1 = prg->GetLength() -(s_start - prg->GetS());
+        }
+        if((prg->GetS() + prg->GetLength())>s_end)
+        {
+            s1 = s_end - prg->GetS();
+        }
+        for(j=0;j<xvectorPP.size();j++)
+        {
+            PlanPoint pp = xvectorPP.at(j);
+
+            pp.mfCurvature = pRoad->GetRoadCurvature(pp.mS);
+
+            if(((pp.dis+prg->GetS()) >= s_start) &&((pp.dis+prg->GetS()) <= s_end))
+            {
+                if(s_start > prg->GetS())
+                {
+                    pp.dis = s + pp.dis -(s_start - prg->GetS());
+                }
+                else
+                {
+                    pp.dis = s+ pp.dis;
+                }
+                pp.nRoadID = atoi(pRoad->GetRoadId().data());
+                xvectorPPS.push_back(pp);
+            }
+//            if((prg->GetS()>s_start)&&((prg->GetS() + prg->GetLength())<s_end))
+//            {
+//                pp.dis = s + pp.dis;
+//                pp.nRoadID = atoi(pRoad->GetRoadId().data());
+//                xvectorPPS.push_back(pp);
+//            }
+//            else
+//            {
+//                if(prg->GetS()<s_start)
+//                {
+
+//                }
+//                else
+//                {
+//                    if(pp.dis<(s_end -prg->GetS()))
+//                    {
+//                        pp.dis = s + pp.dis;
+//                        pp.nRoadID = atoi(pRoad->GetRoadId().data());
+//                        xvectorPPS.push_back(pp);
+//                    }
+//                }
+//            }
+        }
+        s = s+ s1;
+
+    }
+    return xvectorPPS;
+}
+
+std::vector<PlanPoint> GetPoint(Road * pRoad)
+{
+    std::vector<PlanPoint> xvectorPPS;
+    double s = 0;
+
+    int i;
+    for(i=0;i<pRoad->GetGeometryBlockCount();i++)
+    {
+        GeometryBlock * pgb =  pRoad->GetGeometryBlock(i);
+        RoadGeometry * prg = pgb->GetGeometryAt(0);
+        std::vector<PlanPoint> xvectorPP;
+        double s1 = prg->GetLength();
+        switch (prg->GetGeomType()) {
+        case 0:
+            xvectorPP = getlinepoint((GeometryLine *)prg);
+            break;
+        case 1:
+            xvectorPP = getspiralpoint((GeometrySpiral *)prg);
+            break;
+        case 2:
+            xvectorPP = getarcpoint((GeometryArc *)prg);
+
+            break;
+        case 3:
+            xvectorPP = getpoly3dpoint((GeometryPoly3 *)prg);
+            break;
+        case 4:
+            xvectorPP = getparampoly3dpoint((GeometryParamPoly3 *)prg);
+            break;
+        default:
+            break;
+        }
+        int j;
+        for(j=0;j<xvectorPP.size();j++)
+        {
+            PlanPoint pp = xvectorPP.at(j);
+            pp.dis = s + pp.dis;
+            pp.nRoadID = atoi(pRoad->GetRoadId().data());
+            xvectorPPS.push_back(pp);
+        }
+        s = s+ s1;
+
+    }
+    return xvectorPPS;
+
+}
+
+int indexinroadpoint(std::vector<PlanPoint> xvectorPP,double x,double y)
+{
+    int nrtn = 0;
+    int i;
+    int dismin = 1000;
+    for(i=0;i<xvectorPP.size();i++)
+    {
+        double dis = sqrt(pow(xvectorPP.at(i).x - x,2) + pow(xvectorPP.at(i).y -y,2));
+        if(dis <dismin)
+        {
+            dismin = dis;
+            nrtn = i;
+        }
+    }
+
+    if(dismin > 10.0)
+    {
+        std::cout<<"indexinroadpoint near point is error. dis is "<<dismin<<std::endl;
+    }
+    return nrtn;
+}
+
+
+double getwidth(Road * pRoad, int nlane)
+{
+    double frtn = 0;
+
+    int i;
+    int nlanecount = pRoad->GetLaneSection(0)->GetLaneCount();
+    for(i=0;i<nlanecount;i++)
+    {
+        if(nlane == pRoad->GetLaneSection(0)->GetLane(i)->GetId())
+        {
+            LaneWidth* pLW = pRoad->GetLaneSection(0)->GetLane(i)->GetLaneWidth(0);
+            if(pLW != 0)
+            {
+            frtn = pRoad->GetLaneSection(0)->GetLane(i)->GetLaneWidth(0)->GetA();
+            break;
+            }
+        }
+    }
+    return frtn;
+}
+
+double getoff(Road * pRoad,int nlane)
+{
+    double off = getwidth(pRoad,nlane)/2.0;
+    if(nlane < 0)off = off * (-1.0);
+ //   int nlanecount = pRoad->GetLaneSection(0)->GetLaneCount();
+    int i;
+    if(nlane > 0)
+        off = off + getwidth(pRoad,0)/2.0;
+    else
+        off = off - getwidth(pRoad,0)/2.0;
+    if(nlane > 0)
+    {
+        for(i=1;i<nlane;i++)
+        {
+            off = off + getwidth(pRoad,i);
+        }
+    }
+    else
+    {
+        for(i=-1;i>nlane;i--)
+        {
+            off = off - getwidth(pRoad,i);
+        }
+    }
+//    return 0;
+    return off;
+
+}
+
+
+namespace  iv {
+
+struct lanewidthabcd
+{
+    int nlane;
+    double A;
+    double B;
+    double C;
+    double D;
+};
+}
+
+double getwidth(Road * pRoad, int nlane,std::vector<iv::lanewidthabcd> xvectorlwa,double s)
+{
+    double frtn = 0;
+
+
+    int i;
+    int nlanecount = xvectorlwa.size();
+    for(i=0;i<nlanecount;i++)
+    {
+        if(nlane == xvectorlwa.at(i).nlane)
+        {
+            iv::lanewidthabcd * plwa = &xvectorlwa.at(i);
+            frtn = plwa->A + plwa->B*s + plwa->C *s*s + plwa->D *s*s*s;
+            break;
+        }
+    }
+    return frtn;
+}
+
+
+
+std::vector<iv::lanewidthabcd> GetLaneWidthABCD(Road * pRoad)
+{
+    std::vector<iv::lanewidthabcd> xvectorlwa;
+    if(pRoad->GetLaneSectionCount()<1)return xvectorlwa;
+    int i;
+
+
+    LaneSection * pLS = pRoad->GetLaneSection(0);
+
+//    if(pRoad->GetLaneSectionCount() > 1)
+//    {
+//        for(i=0;i<(pRoad->GetLaneSectionCount()-1);i++)
+//        {
+//            if(s>pRoad->GetLaneSection(i+1)->GetS())
+//            {
+//                pLS = pRoad->GetLaneSection(i+1);
+//            }
+//            else
+//            {
+//                break;
+//            }
+//        }
+//    }
+
+    int nlanecount = pLS->GetLaneCount();
+
+    for(i=0;i<nlanecount;i++)
+    {
+        iv::lanewidthabcd xlwa;
+
+
+        LaneWidth* pLW = pLS->GetLane(i)->GetLaneWidth(0);
+        xlwa.nlane = pLS->GetLane(i)->GetId();
+        if(pLW != 0)
+        {
+
+            xlwa.A = pLW->GetA();
+            xlwa.B = pLW->GetB();
+            xlwa.C = pLW->GetC();
+            xlwa.D = pLW->GetD();
+
+        }
+        else
+        {
+            xlwa.A = 0;
+            xlwa.B = 0;
+            xlwa.C = 0;
+            xlwa.D = 0;
+        }
+
+ //       if(strncmp(pLS->GetLane(i)->GetType().data(),"driving",256) == 0)
+        xvectorlwa.push_back(xlwa);   //Calculate Road Width, not driving need add in.
+
+    }
+    return xvectorlwa;
+
+}
+
+
+inline double getoff(int nlane,double s,std::vector<iv::lanewidthabcd> xvectorLWA,std::vector<int> xvectorIndex)
+{
+    int i;
+    double off = 0;
+    double a,b,c,d;
+    if(xvectorIndex.size() == 0)return off;
+
+    for(i=0;i<(xvectorIndex.size()-1);i++)
+    {
+
+        a = xvectorLWA[xvectorIndex[i]].A;
+        b = xvectorLWA[xvectorIndex[i]].B;
+        c = xvectorLWA[xvectorIndex[i]].C;
+        d = xvectorLWA[xvectorIndex[i]].D;
+        if(xvectorLWA[xvectorIndex[i]].nlane != 0)
+        {
+            off = off + a + b*s + c*s*s + d*s*s*s;
+        }
+        else
+        {
+            off = off + (a + b*s + c*s*s + d*s*s*s)/2.0;
+        }
+    }
+    i = xvectorIndex.size()-1;
+    a = xvectorLWA[xvectorIndex[i]].A;
+    b = xvectorLWA[xvectorIndex[i]].B;
+    c = xvectorLWA[xvectorIndex[i]].C;
+    d = xvectorLWA[xvectorIndex[i]].D;
+    off = off + (a + b*s + c*s*s + d*s*s*s)/2.0;
+    if(nlane < 0) off = off*(-1.0);
+//    std::cout<<"s is "<<s<<std::endl;
+//    std::cout<<" off is "<<off<<std::endl;
+    return off;
+
+}
+
+double GetRoadWidth(std::vector<iv::lanewidthabcd> xvectorLWA,int nlane,double s)
+{
+    double fwidth = 0;
+    int i;
+    double a,b,c,d;
+
+    int nsize = xvectorLWA.size();
+    for(i=0;i<nsize;i++)
+    {
+        if(nlane*(xvectorLWA[i].nlane)>0)
+        {
+            a = xvectorLWA[i].A;
+            b = xvectorLWA[i].B;
+            c = xvectorLWA[i].C;
+            d = xvectorLWA[i].D;
+            fwidth = fwidth + a + b*s +c*s*s + d*s*s*s;
+        }
+    }
+    return fwidth;
+
+}
+
+int GetLaneOriTotal(Road * pRoad, int nlane,double s,int & nori, int & ntotal,double & fSpeed,bool & bNoavoid)
+{
+    if(pRoad->GetLaneSectionCount() < 1)return -1;
+
+    LaneSection * pLS = pRoad->GetLaneSection(0);
+
+    int i;
+
+    if(pRoad->GetLaneSectionCount() > 1)
+    {
+        for(i=0;i<(pRoad->GetLaneSectionCount()-1);i++)
+        {
+            if(s>pRoad->GetLaneSection(i+1)->GetS())
+            {
+                pLS = pRoad->GetLaneSection(i+1);
+            }
+            else
+            {
+                break;
+            }
+        }
+    }
+
+    nori = 0;
+    ntotal = 0;
+    fSpeed = -1; //if -1 no speedlimit for map
+
+    pRoad->GetRoadSpeedMax(s,fSpeed);   //Get Road Speed Limit.
+
+    pRoad->GetRoadNoavoid(s,bNoavoid);
+
+    int nlanecount = pLS->GetLaneCount();
+    for(i=0;i<nlanecount;i++)
+    {
+        int nlaneid = pLS->GetLane(i)->GetId();
+
+        if(nlaneid == nlane)
+        {
+            Lane * pLane = pLS->GetLane(i);
+            if(pLane->GetLaneSpeedCount() > 0)
+            {
+                LaneSpeed * pLSpeed = pLane->GetLaneSpeed(0);
+                int j;
+                int nspeedcount = pLane->GetLaneSpeedCount();
+                for(j=0;j<(nspeedcount -1);j++)
+                {
+                    if((s-pLS->GetS())>=pLane->GetLaneSpeed(j+1)->GetS())
+                    {
+                        pLSpeed = pLane->GetLaneSpeed(j+1);
+                    }
+                    else
+                    {
+                        break;
+                    }
+                }
+                if(pLSpeed->GetS()<=(s-pLS->GetS()))fSpeed = pLSpeed->GetMax();
+
+            }
+        }
+        if(nlane<0)
+        {
+
+            if(nlaneid < 0)
+            {
+                if(strncmp(pLS->GetLane(i)->GetType().data(),"driving",256) == 0)
+                {
+                    ntotal++;
+                    if(nlaneid<nlane)nori++;
+                }
+            }
+
+
+        }
+        else
+        {
+            if(nlaneid > 0)
+            {
+                if(strncmp(pLS->GetLane(i)->GetType().data(),"driving",256) == 0)
+                {
+                    ntotal++;
+                    if(nlaneid > nlane)nori++;
+                }
+            }
+        }
+    }
+
+    return 0;
+}
+
+std::vector<int> GetLWIndex(std::vector<iv::lanewidthabcd> xvectorLWA,int nlane)
+{
+    std::vector<int> xvectorindex;
+    int i;
+    if(nlane >= 0)
+    {
+    for(i=0;i<=nlane;i++)
+    {
+       int j;
+       int nsize = xvectorLWA.size();
+       for(j=0;j<nsize;j++)
+       {
+           if(i == xvectorLWA.at(j).nlane )
+           {
+               xvectorindex.push_back(j);
+               break;
+           }
+       }
+    }
+    }
+    else
+    {
+        for(i=0;i>=nlane;i--)
+        {
+           int j;
+           int nsize = xvectorLWA.size();
+           for(j=0;j<nsize;j++)
+           {
+               if(i == xvectorLWA.at(j).nlane )
+               {
+                   xvectorindex.push_back(j);
+                   break;
+               }
+           }
+        }
+    }
+    return xvectorindex;
+}
+
+void CalcBorringRoad(pathsection xps,std::vector<PlanPoint> & xvectorPP)
+{
+    if(xps.mpRoad->GetRoadBorrowCount() == 0)
+    {
+        return;
+    }
+
+    Road * pRoad = xps.mpRoad;
+    unsigned int nborrowsize = pRoad->GetRoadBorrowCount();
+    unsigned int i;
+    unsigned int nPPCount = xvectorPP.size();
+    for(i=0;i<nborrowsize;i++)
+    {
+        RoadBorrow * pborrow = pRoad->GetRoadBorrow(i);
+        if(pborrow == NULL)
+        {
+            std::cout<<"warning:can't get borrow"<<std::endl;
+            continue;
+        }
+        if((pborrow->GetMode() == "All")||((pborrow->GetMode()=="R2L")&&(xps.mainsel<0))||((pborrow->GetMode()=="L2R")&&(xps.mainsel>0)))
+        {
+            unsigned int j;
+            double soffset = pborrow->GetS();
+            double borrowlen = pborrow->GetLength();
+            for(j=0;j<xvectorPP.size();j++)
+            {
+                if((xvectorPP[j].mS>=soffset)&&(xvectorPP[j].mS<=(soffset + borrowlen)))
+                {
+                    xvectorPP[j].mbBoringRoad = true;
+                }
+            }
+        }
+    }
+}
+
+void CalcInLaneAvoid(pathsection xps,std::vector<PlanPoint> & xvectorPP,const double fvehiclewidth,
+                     const int nchang1,const int nchang2,const int nchangpoint)
+{
+    if(xvectorPP.size()<2)return;
+    bool bInlaneavoid = false;
+    int i;
+    if((xps.mbStartToMainChange == false) && (xps.mbMainToEndChange == false))
+    {
+        if(xps.mpRoad->GetRoadLength()<30)
+        {
+            double hdgdiff = xvectorPP.at(xvectorPP.size()-1).hdg - xvectorPP.at(0).hdg;
+            if(hdgdiff<0)hdgdiff = hdgdiff + 2.0*M_PI;
+            if(hdgdiff>(M_PI/3.0)&&(hdgdiff<(5.0*M_PI/3.0)))
+                bInlaneavoid = false;
+            else
+                bInlaneavoid = true;
+        }
+        else
+        {
+            bInlaneavoid = true;
+        }
+    }
+    else
+    {
+        if(xps.mpRoad->GetRoadLength()>100)bInlaneavoid = true;
+    }
+
+    if(bInlaneavoid == false)
+    {
+        int nvpsize = xvectorPP.size();
+        for(i=0;i<nvpsize;i++)
+        {
+            xvectorPP.at(i).bInlaneAvoid = false;
+            xvectorPP.at(i).mx_left = xvectorPP.at(i).x;
+            xvectorPP.at(i).my_left = xvectorPP.at(i).y;
+            xvectorPP.at(i).mx_right = xvectorPP.at(i).x;
+            xvectorPP.at(i).my_right = xvectorPP.at(i).y;
+        }
+    }
+    else
+    {
+      int nvpsize = xvectorPP.size();
+      for(i=0;i<nvpsize;i++)xvectorPP.at(i).bInlaneAvoid = true;
+      if((xps.mbStartToMainChange == true)||(xps.mbMainToEndChange == true))
+      {
+          if(xps.mbStartToMainChange == true)
+          {
+              for(i=(nchang1 - nchangpoint/2);i<(nchang1 + nchangpoint/2);i++)
+              {
+                  if((i>=0)&&(i<nvpsize))
+                    xvectorPP.at(i).bInlaneAvoid = false;
+              }
+
+          }
+          if(xps.mbMainToEndChange)
+          {
+              for(i=(nchang2 - nchangpoint/2);i<(nchang2 + nchangpoint/2);i++)
+              {
+                  if((i>=0)&&(i<nvpsize))
+                    xvectorPP.at(i).bInlaneAvoid = false;
+              }
+          }
+      }
+
+      for(i=0;i<nvpsize;i++)
+      {
+          if(xvectorPP.at(i).bInlaneAvoid)
+          {
+              double foff = xvectorPP.at(i).mfDisToLaneLeft -0.3-fvehiclewidth*0.5; //30cm + 0.5vehcilewidth
+              if(foff < 0.3)
+              {
+                  xvectorPP.at(i).bInlaneAvoid = false;
+                  xvectorPP.at(i).mx_left = xvectorPP.at(i).x;
+                  xvectorPP.at(i).my_left = xvectorPP.at(i).y;
+                  xvectorPP.at(i).mx_right = xvectorPP.at(i).x;
+                  xvectorPP.at(i).my_right = xvectorPP.at(i).y;
+              }
+              else
+              {
+                  xvectorPP.at(i).mx_left = xvectorPP.at(i).x + foff * cos(xvectorPP.at(i).hdg + M_PI/2.0);
+                  xvectorPP.at(i).my_left = xvectorPP.at(i).y + foff * sin(xvectorPP.at(i).hdg + M_PI/2.0);
+                  xvectorPP.at(i).mx_right = xvectorPP.at(i).x + foff * cos(xvectorPP.at(i).hdg - M_PI/2.0);
+                  xvectorPP.at(i).my_right = xvectorPP.at(i).y + foff * sin(xvectorPP.at(i).hdg - M_PI/2.0);
+              }
+          }
+      }
+    }
+
+}
+
+
+double getoff(Road * pRoad,int nlane,const double s)
+{
+    int i;
+    int nLSCount = pRoad->GetLaneSectionCount();
+    double s_section = 0;
+
+    double foff = 0;
+
+    for(i=0;i<nLSCount;i++)
+    {
+
+        LaneSection * pLS = pRoad->GetLaneSection(i);
+        if(i<(nLSCount -1))
+        {
+            if(pRoad->GetLaneSection(i+1)->GetS()<s)
+            {
+                continue;
+            }
+        }
+        s_section = pLS->GetS();
+        int nlanecount = pLS->GetLaneCount();
+        int j;
+        for(j=0;j<nlanecount;j++)
+        {
+            Lane * pLane = pLS->GetLane(j);
+
+            int k;
+            double s_lane = s-s_section;
+
+
+            if(pLane->GetId() != 0)
+            {
+
+                for(k=0;k<pLane->GetLaneWidthCount();k++)
+                {
+                    if(k<(pLane->GetLaneWidthCount()-1))
+                    {
+                        if((pLane->GetLaneWidth(k+1)->GetS()+s_section)<s)
+                        {
+                            continue;
+                        }
+                    }
+                    s_lane = pLane->GetLaneWidth(k)->GetS();
+                    break;
+                }
+                LaneWidth * pLW  = pLane->GetLaneWidth(k);
+                if(pLW == 0)
+                {
+                    std::cout<<"not find LaneWidth"<<std::endl;
+                    break;
+                }
+
+                double fds = s - s_lane - s_section;
+                double fwidth= pLW->GetA() + pLW->GetB() * fds
+                        +pLW->GetC() * pow(fds,2) + pLW->GetD() * pow(fds,3);
+
+//                if((pRoad->GetRoadId() == "211210") &&(nlane == -1) &&(pLane->GetId() == -1))
+//                {
+//                    qDebug("fs is %f width is %f",fds,fwidth);
+//                }
+                if(nlane == pLane->GetId())
+                {
+                    foff = foff + fwidth/2.0;
+                }
+                if((nlane*(pLane->GetId())>0)&&(abs(nlane)>abs(pLane->GetId())))
+                {
+                    foff = foff + fwidth;
+                }
+
+            }
+
+        }
+
+
+        break;
+
+    }
+
+    if(pRoad->GetLaneOffsetCount()>0)
+    {
+
+        int nLOCount = pRoad->GetLaneOffsetCount();
+        int isel = -1;
+        for(i=0;i<(nLOCount-1);i++)
+        {
+            if(pRoad->GetLaneOffset(i+1)->GetS()>s)
+            {
+                isel = i;
+                break;
+            }
+        }
+        if(isel < 0)isel = nLOCount-1;
+        LaneOffset * pLO = pRoad->GetLaneOffset(isel);
+        double s_off = s - pLO->GetS();
+        double off1 = pLO->Geta() + pLO->Getb()*s_off + pLO->Getc() * s_off * s_off
+                +pLO->Getd() * s_off * s_off * s_off;
+        foff = foff - off1;
+    }
+
+    if(nlane<0)foff = foff*(-1);
+    return foff;
+}
+
+
+std::vector<PlanPoint> GetLanePoint(pathsection xps,std::vector<PlanPoint> xvPP,const double fvehiclewidth)
+{
+    std::vector<PlanPoint> xvectorPP;
+
+    std::vector<iv::lanewidthabcd> xvectorLWA = GetLaneWidthABCD(xps.mpRoad);
+    std::vector<int> xvectorindex1,xvectorindex2,xvectorindex3;
+    int nchange1,nchange2;
+    int nlane1,nlane2,nlane3;
+    if(xps.mnStartLaneSel == xps.mnEndLaneSel)
+    {
+        xps.mainsel = xps.mnStartLaneSel;
+        xps.mbStartToMainChange = false;
+        xps.mbMainToEndChange = false;
+    }
+    else
+    {
+        if(xps.mpRoad->GetRoadLength() < 100)
+        {
+            xps.mainsel = xps.mnEndLaneSel;
+            xps.mbStartToMainChange = true;
+            xps.mbMainToEndChange = false;
+        }
+    }
+
+    double off1,off2,off3;
+    if(xps.mnStartLaneSel < 0)
+    {
+    off1 = getoff(xps.mpRoad,xps.mnStartLaneSel);
+    off2 = getoff(xps.mpRoad,xps.mainsel);
+    off3 = getoff(xps.mpRoad,xps.mnEndLaneSel);
+    xvectorindex1 = GetLWIndex(xvectorLWA,xps.mnStartLaneSel);
+    xvectorindex2 = GetLWIndex(xvectorLWA,xps.mainsel);
+    xvectorindex3 = GetLWIndex(xvectorLWA,xps.mnEndLaneSel);
+    nlane1 = xps.mnStartLaneSel;
+    nlane2 = xps.mainsel;
+    nlane3 = xps.mnEndLaneSel;
+    }
+    else
+    {
+        off3 = getoff(xps.mpRoad,xps.mnStartLaneSel);
+        off2 = getoff(xps.mpRoad,xps.mainsel);
+        off1 = getoff(xps.mpRoad,xps.mnEndLaneSel);
+        xvectorindex3 = GetLWIndex(xvectorLWA,xps.mnStartLaneSel);
+        xvectorindex2 = GetLWIndex(xvectorLWA,xps.mainsel);
+        xvectorindex1 = GetLWIndex(xvectorLWA,xps.mnEndLaneSel);
+        nlane3 = xps.mnStartLaneSel;
+        nlane2 = xps.mainsel;
+        nlane1 = xps.mnEndLaneSel;
+    }
+
+    int nchangepoint = 300;
+    if((nchangepoint * 3) > xvPP.size())
+    {
+        nchangepoint = xvPP.size()/3;
+    }
+    int nsize = xvPP.size();
+
+    nchange1 = nsize/3;
+    if(nchange1>500)nchange1 = 500;
+    nchange2 = nsize*2/3;
+    if( (nsize-nchange2)>500)nchange2 = nsize-500;
+//    if(nsize < 1000)
+//    {
+//        std::cout<<"GetLanePoint Error. road id is "<<xps.mpRoad->GetRoadId()<<std::endl;
+//        return xvectorPP;
+//    }
+    double x,y;
+    int i;
+    if(xps.mainsel < 0)
+    {
+
+        for(i=0;i<nsize;i++)
+        {
+            PlanPoint pp = xvPP.at(i);
+    //            off1 = getoff(nlane1,pp.mS,xvectorLWA,xvectorindex1);
+
+            off1 = getoff(xps.mpRoad,nlane2,pp.mS);
+            pp.x = pp.x + off1 *cos(pp.hdg + M_PI/2.0);
+            pp.y = pp.y + off1 *sin(pp.hdg + M_PI/2.0);
+
+            pp.nlrchange = 1;
+
+            if(xps.mainsel != xps.secondsel)
+            {
+                off1 = getoff(xps.mpRoad,xps.secondsel,pp.mS);
+                pp.mfSecx = xvPP.at(i).x + off1 *cos(pp.hdg + M_PI/2.0);
+                pp.mfSecy = xvPP.at(i).y + off1 *sin(pp.hdg + M_PI/2.0);
+
+                if(xps.mainsel >xps.secondsel)
+                {
+                    pp.nlrchange = 1;
+                }
+                else
+                {
+                    pp.nlrchange = 2;
+                }
+            }
+            else
+            {
+                pp.mfSecx = pp.x ;
+                pp.mfSecy = pp.y ;
+            }
+
+            pp.mWidth = getwidth(xps.mpRoad,xps.mainsel,xvectorLWA,pp.mS);
+            pp.mfDisToLaneLeft = pp.mWidth/2.0;
+            pp.lanmp = 0;
+            pp.mfDisToRoadLeft = off1*(-1);
+            pp.mfRoadWidth = GetRoadWidth(xvectorLWA,xps.mainsel,pp.mS);
+            GetLaneOriTotal(xps.mpRoad,xps.mainsel,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed,pp.mbNoavoid);
+
+            xvectorPP.push_back(pp);
+        }
+
+
+    }
+    else
+    {
+
+        for(i=0;i<nsize;i++)
+        {
+            PlanPoint pp = xvPP.at(i);
+    //            off1 = getoff(nlane1,pp.mS,xvectorLWA,xvectorindex1);
+
+
+            off1 = getoff(xps.mpRoad,nlane2,pp.mS);
+            pp.x = pp.x + off1 *cos(pp.hdg + M_PI/2.0);
+            pp.y = pp.y + off1 *sin(pp.hdg + M_PI/2.0);
+
+            pp.nlrchange = 1;
+
+            if(xps.mainsel != xps.secondsel)
+            {
+                off1 = getoff(xps.mpRoad,xps.secondsel,pp.mS);
+                pp.mfSecx = xvPP.at(i).x + off1 *cos(pp.hdg + M_PI/2.0);
+                pp.mfSecy = xvPP.at(i).y + off1 *sin(pp.hdg + M_PI/2.0);
+
+                if(xps.mainsel >xps.secondsel)
+                {
+                    pp.nlrchange = 2;
+                }
+                else
+                {
+                    pp.nlrchange = 1;
+                }
+            }
+            else
+            {
+                pp.mfSecx = pp.x ;
+                pp.mfSecy = pp.y ;
+            }
+
+            pp.mWidth = getwidth(xps.mpRoad,xps.mainsel,xvectorLWA,pp.mS);
+            pp.mfDisToLaneLeft = pp.mWidth/2.0;
+            pp.lanmp = 0;
+            pp.mfDisToRoadLeft = off1;
+            pp.mfRoadWidth = GetRoadWidth(xvectorLWA,xps.mainsel,pp.mS);
+            GetLaneOriTotal(xps.mpRoad,xps.mainsel,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed,pp.mbNoavoid);
+
+            pp.hdg = pp.hdg + M_PI;
+            xvectorPP.push_back(pp);
+        }
+
+//        for(i=0;i<(nchange1- nchangepoint/2);i++)
+//        {
+//            PlanPoint pp = xvPP.at(i);
+//            off1 = getoff(nlane1,pp.mS,xvectorLWA,xvectorindex1);
+//            pp.x = pp.x + off1 *cos(pp.hdg + M_PI/2.0);
+//            pp.y = pp.y + off1 *sin(pp.hdg + M_PI/2.0);
+//            pp.hdg = pp.hdg + M_PI;
+
+//            pp.mWidth = getwidth(xps.mpRoad,nlane1,xvectorLWA,pp.mS);
+//            pp.mfDisToLaneLeft = pp.mWidth/2.0;
+//            pp.lanmp = 0;
+//            pp.mfDisToRoadLeft = off1;
+//            pp.mfRoadWidth = GetRoadWidth(xvectorLWA,nlane1,pp.mS);
+//            GetLaneOriTotal(xps.mpRoad,nlane1,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed);
+
+
+//            xvectorPP.push_back(pp);
+
+//        }
+//        for(i=(nchange1 - nchangepoint/2);i<(nchange1+nchangepoint/2);i++)
+//        {
+
+//            PlanPoint pp = xvPP.at(i);
+//            off1 = getoff(nlane1,pp.mS,xvectorLWA,xvectorindex1);
+//            off2 = getoff(nlane2,pp.mS,xvectorLWA,xvectorindex2);
+//            double offx = off1 + (off2 - off1) *(i-nchange1 + nchangepoint/2)/nchangepoint;
+//            pp.x = pp.x + offx *cos(pp.hdg + M_PI/2.0);
+//            pp.y = pp.y + offx *sin(pp.hdg + M_PI/2.0);
+//            pp.hdg = pp.hdg + M_PI;
+
+//            double flanewidth1 = getwidth(xps.mpRoad,nlane1,xvectorLWA,pp.mS);
+
+//            pp.mfDisToRoadLeft = offx;
+//            pp.mfRoadWidth = GetRoadWidth(xvectorLWA,nlane1,pp.mS);
+//            if(nlane1 == nlane2)
+//            {
+//                pp.mWidth = flanewidth1;
+//                pp.mfDisToLaneLeft = pp.mWidth/2.0;
+//                pp.lanmp = 0;
+//                GetLaneOriTotal(xps.mpRoad,nlane1,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed);
+//            }
+//            else
+//            {
+//                if(nlane1 < nlane2)
+//                {
+//                    pp.lanmp = -1;
+//                }
+//                else
+//                {
+//                    pp.lanmp = 1;
+//                }
+
+//                if(i<nchange1)
+//                {
+//                    pp.mWidth = getwidth(xps.mpRoad,nlane1,xvectorLWA,pp.mS);
+//                    double fmove = pp.mWidth * (i-(nchange1 - nchangepoint/2))/nchangepoint;
+//                    if(nlane1<nlane2)pp.mfDisToLaneLeft = pp.mWidth/2.0  - fmove;
+//                    else pp.mfDisToLaneLeft = pp.mWidth/2.0 + fmove;
+//                    GetLaneOriTotal(xps.mpRoad,nlane1,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed);
+//                }
+//                else
+//                {
+//                    pp.mWidth = getwidth(xps.mpRoad,nlane2,xvectorLWA,pp.mS);
+//                    double fmove = pp.mWidth * (nchange1+nchangepoint/2 -i)/nchangepoint;
+//                    if(nlane1<nlane2)pp.mfDisToLaneLeft = pp.mWidth/2.0  + fmove;
+//                    else pp.mfDisToLaneLeft = pp.mWidth/2.0 - fmove;
+//                    GetLaneOriTotal(xps.mpRoad,nlane2,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed);
+//                }
+
+//            }
+
+//            xvectorPP.push_back(pp);
+
+//        }
+//        for(i=(nchange1 + nchangepoint/2);i<(nchange2-nchangepoint/2);i++)
+//        {
+//            PlanPoint pp = xvPP.at(i);
+//            off2 = getoff(nlane2,pp.mS,xvectorLWA,xvectorindex2);
+//            pp.x = pp.x + off2 *cos(pp.hdg + M_PI/2.0);
+//            pp.y = pp.y + off2 *sin(pp.hdg + M_PI/2.0);
+//            pp.hdg = pp.hdg + M_PI;
+
+//            pp.mWidth = getwidth(xps.mpRoad,nlane2,xvectorLWA,pp.mS);
+//            pp.mfDisToLaneLeft = pp.mWidth/2.0;
+//            pp.lanmp = 0;
+//            pp.mfDisToRoadLeft = off2;
+//            pp.mfRoadWidth = GetRoadWidth(xvectorLWA,nlane2,pp.mS);
+//            GetLaneOriTotal(xps.mpRoad,nlane2,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed);
+
+
+//            xvectorPP.push_back(pp);
+
+//        }
+//        for(i=(nchange2 - nchangepoint/2);i<(nchange2+nchangepoint/2);i++)
+//        {
+
+//            PlanPoint pp = xvPP.at(i);
+//            off2 = getoff(nlane2,pp.mS,xvectorLWA,xvectorindex2);
+//            off3 = getoff(nlane3,pp.mS,xvectorLWA,xvectorindex3);
+//            double offx = off2 + (off3 - off2) *(i-nchange2 + nchangepoint/2)/nchangepoint;
+//            pp.x = pp.x + offx *cos(pp.hdg + M_PI/2.0);
+//            pp.y = pp.y + offx *sin(pp.hdg + M_PI/2.0);
+//            pp.hdg = pp.hdg + M_PI;
+
+//            double flanewidth1 = getwidth(xps.mpRoad,nlane2,xvectorLWA,pp.mS);
+
+//            pp.mfDisToRoadLeft = offx;
+//            pp.mfRoadWidth = GetRoadWidth(xvectorLWA,nlane2,pp.mS);
+//            if(nlane2 == nlane3)
+//            {
+//                pp.mWidth = flanewidth1;
+//                pp.mfDisToLaneLeft = pp.mWidth/2.0;
+//                pp.lanmp = 0;
+//                GetLaneOriTotal(xps.mpRoad,nlane2,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed);
+//            }
+//            else
+//            {
+//                if(nlane2 < nlane3)
+//                {
+//                    pp.lanmp = -1;
+//                }
+//                else
+//                {
+//                    pp.lanmp = 1;
+//                }
+
+//                if(i<nchange2)
+//                {
+//                    pp.mWidth = getwidth(xps.mpRoad,nlane2,xvectorLWA,pp.mS);
+//                    double fmove = pp.mWidth * (i-(nchange2 - nchangepoint/2))/nchangepoint;
+//                    if(nlane2<nlane3)pp.mfDisToLaneLeft = pp.mWidth/2.0  - fmove;
+//                    else pp.mfDisToLaneLeft = pp.mWidth/2.0 + fmove;
+//                    GetLaneOriTotal(xps.mpRoad,nlane2,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed);
+//                }
+//                else
+//                {
+//                    pp.mWidth = getwidth(xps.mpRoad,nlane3,xvectorLWA,pp.mS);
+//                    double fmove = pp.mWidth * (nchange2+nchangepoint/2 -i)/nchangepoint;
+//                    if(nlane2<nlane3)pp.mfDisToLaneLeft = pp.mWidth/2.0  + fmove;
+//                    else pp.mfDisToLaneLeft = pp.mWidth/2.0 - fmove;
+//                    GetLaneOriTotal(xps.mpRoad,nlane3,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed);
+//                }
+
+//            }
+
+//            xvectorPP.push_back(pp);
+
+//        }
+//        for(i=(nchange2+ nchangepoint/2);i<nsize;i++)
+//        {
+//            PlanPoint pp = xvPP.at(i);
+//            off3 = getoff(nlane3,pp.mS,xvectorLWA,xvectorindex3);
+//            pp.x = pp.x + off3 *cos(pp.hdg + M_PI/2.0);
+//            pp.y = pp.y + off3 *sin(pp.hdg + M_PI/2.0);
+//            pp.hdg = pp.hdg + M_PI;
+
+//            pp.mWidth = getwidth(xps.mpRoad,nlane3,xvectorLWA,pp.mS);
+//            pp.mfDisToLaneLeft = pp.mWidth/2.0;
+//            pp.lanmp = 0;
+//            pp.mfDisToRoadLeft = off3;
+//            pp.mfRoadWidth = GetRoadWidth(xvectorLWA,nlane3,pp.mS);
+//            GetLaneOriTotal(xps.mpRoad,nlane3,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed);
+
+
+//            xvectorPP.push_back(pp);
+
+//        }
+
+    }
+
+
+
+    CalcInLaneAvoid(xps,xvectorPP,fvehiclewidth,nchange1,nchange2,nchangepoint);
+
+    if(xps.mpRoad->GetRoadBorrowCount()>0)
+    {
+        CalcBorringRoad(xps,xvectorPP);
+    }
+
+
+    if(xps.mnStartLaneSel > 0)
+    {
+        std::vector<PlanPoint> xvvPP;
+        int nvsize = xvectorPP.size();
+        for(i=0;i<nvsize;i++)
+        {
+            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,
+                                           double nearx,double neary, double nearhead,
+                                           double nearx_obj,double  neary_obj,const double fvehiclewidth,const double flen = 1000)
+{
+
+  std::vector<PlanPoint> xvectorPPs;
+
+  double fspace = 0.1;
+
+  if(flen<2000)fspace = 0.1;
+  else
+  {
+      if(flen<5000)fspace = 0.3;
+      else
+      {
+          if(flen<10000)fspace = 0.5;
+          else
+              fspace = 1.0;
+      }
+  }
+
+  int i;
+
+
+//  std::vector<PlanPoint> xvectorPP = GetPoint( xpathsection[0].mpRoad);
+  std::vector<PlanPoint> xvectorPP = GetPoint( xpathsection[0],fspace);
+
+
+  int indexstart = indexinroadpoint(xvectorPP,nearx,neary);
+
+  std::vector<PlanPoint> xvPP = GetLanePoint(xpathsection[0],xvectorPP,fvehiclewidth);
+
+  if(xpathsection[0].mainsel < 0)
+  {
+  for(i=indexstart;i<xvPP.size();i++)
+  {
+      xvectorPPs.push_back(xvPP.at(i));
+  }
+  }
+  else
+  {
+
+      for(i=(xvPP.size() -indexstart);i<xvPP.size();i++)
+      {
+          PlanPoint pp;
+          pp = xvPP.at(i);
+//          pp.hdg = pp.hdg +M_PI;
+          xvectorPPs.push_back(pp);
+      }
+  }
+
+  int npathlast = xpathsection.size() - 1;
+//  while(npathlast >= 0)
+//  {
+//    if(npathlast == 0)break;
+//    if(xpathsection[npathlast].mpRoad != xpathsection[npathlast - 1].mpRoad)break;
+//  }
+  for(i=1;i<(npathlast);i++)
+  {
+//      if(xpathsection[i].mpRoad == xpathsection[i-1].mpRoad)
+//      {
+//          if(xpathsection[i].mainsel * xpathsection[i-1].mainsel >0)
+//          {
+//            continue;
+//          }
+//      }
+//      if(xpathsection[i].mpRoad == xpathsection[npathlast].mpRoad)
+//      {
+//          if(xpathsection[i].mainsel * xpathsection[npathlast].mainsel > 0)
+//          {
+//            break;
+//          }
+//      }
+ //     xvectorPP = GetPoint( xpathsection[i].mpRoad);
+      xvectorPP = GetPoint( xpathsection[i],fspace);
+      xvPP = GetLanePoint(xpathsection[i],xvectorPP,fvehiclewidth);
+//      std::cout<<" road id: "<<xpathsection[i].mroadid<<" section: "
+//              <<xpathsection[i].msectionid<<" size is "<<xvectorPP.size()<<std::endl;
+//      std::cout<<" road id is "<<xpathsection[i].mpRoad->GetRoadId().data()<<" size is "<<xvPP.size()<<std::endl;
+      int j;
+      for(j=0;j<xvPP.size();j++)
+      {
+
+          PlanPoint pp;
+          pp = xvPP.at(j);
+//          pp.hdg = pp.hdg +M_PI;
+          xvectorPPs.push_back(pp);
+      }
+  }
+
+  xvectorPP = GetPoint(xpathsection[npathlast],fspace);
+
+
+//  xvectorPP = GetPoint(xpathsection[npathlast].mpRoad);
+  int indexend = indexinroadpoint(xvectorPP,nearx_obj,neary_obj);
+  xvPP = GetLanePoint(xpathsection[npathlast],xvectorPP,fvehiclewidth);
+  int nlastsize;
+  if(xpathsection[npathlast].mainsel<0)
+  {
+      nlastsize = indexend;
+  }
+  else
+  {
+      if(indexend>0) nlastsize = xvPP.size() - indexend;
+      else nlastsize = xvPP.size();
+  }
+  for(i=0;i<nlastsize;i++)
+  {
+      xvectorPPs.push_back(xvPP.at(i));
+
+  }
+
+  //Find StartPoint
+//  if
+
+//  int a = 1;
+
+
+
+
+  return xvectorPPs;
+}
+
+std::vector<PlanPoint> gTempPP;
+int getPoint(double q[3], double x, void* user_data) {
+//    printf("%f, %f, %f, %f\n", q[0], q[1], q[2], x);
+    PlanPoint pp;
+    pp.x = q[0];
+    pp.y = q[1];
+    pp.hdg = q[2];
+    pp.dis = x;
+    pp.speed = *((double *)user_data);
+    gTempPP.push_back(pp);
+//    std::cout<<pp.x<<" "<<" "<<pp.y<<" "<<pp.hdg<<std::endl;
+    return 0;
+}
+
+/**
+ * @brief getlanesel
+ * @param nSel
+ * @param pLaneSection
+ * @param nlr
+ * @return
+ */
+int getlanesel(int nSel,LaneSection * pLaneSection,int nlr)
+{
+    int nlane = nSel;
+    int mainselindex = 0;
+    if(nlr == 2)nlane = nlane*(-1);
+    int nlanecount = pLaneSection->GetLaneCount();
+
+    int i;
+    int nTrueSel = nSel;
+    if(nTrueSel <= 1)nTrueSel= 1;
+    int nCurSel = 1;
+    if(nlr ==  2)nCurSel = nCurSel *(-1);
+    bool bOK = false;
+    int nxsel = 1;
+    int nlasttid = 0;
+    bool bfindlast = false;
+    while(bOK == false)
+    {
+        bool bHaveDriving = false;
+        int ncc = 0;
+        for(i=0;i<nlanecount;i++)
+        {
+            Lane * pLane = pLaneSection->GetLane(i);
+            if(strncmp(pLane->GetType().data(),"driving",255) == 0 )
+            {
+                if((nlr == 1)&&(pLane->GetId()>0))
+                {
+                    ncc++;
+                }
+                if((nlr == 2)&&(pLane->GetId()<0))
+                {
+                    ncc++;
+                }
+                if(ncc == nxsel)
+                {
+                    bHaveDriving = true;
+                    bfindlast = true;
+                    nlasttid = pLane->GetId();
+                    break;
+                }
+            }
+        }
+        if(bHaveDriving == true)
+        {
+            if(nxsel == nTrueSel)
+            {
+                return nlasttid;
+            }
+            else
+            {
+                nxsel++;
+            }
+        }
+        else
+        {
+            if(bfindlast)
+            {
+                return nlasttid;
+            }
+            else
+            {
+                std::cout<<"can't find lane."<<std::endl;
+                return 0;
+            }
+            //Use Last
+        }
+
+    }
+
+    return 0;
+
+    int k;
+    bool bFind = false;
+    while(bFind == false)
+    {
+        for(k=0;k<nlanecount;k++)
+        {
+            Lane * pLane = pLaneSection->GetLane(k);
+            if((nlane == pLane->GetId())&&(strncmp(pLane->GetType().data(),"driving",255) == 0))
+            {
+                bFind = true;
+                mainselindex = k;
+                break;
+            }
+        }
+        if(bFind)continue;
+        if(nlr == 1)nlane--;
+        else nlane++;
+        if(nlane == 0)
+        {
+            std::cout<<" Fail. can't find lane"<<std::endl;
+            break;
+        }
+    }
+    return nlane;
+}
+
+
+
+void checktrace(std::vector<PlanPoint> & xPlan)
+{
+    int i;
+    int nsize = xPlan.size();
+    for(i=1;i<nsize;i++)
+    {
+        double dis = sqrt(pow(xPlan.at(i).x - xPlan.at(i-1).x,2) + pow(xPlan.at(i).y - xPlan.at(i-1).y,2));
+        if(dis > 0.3)
+        {
+            double hdg =  CalcHdg(QPointF(xPlan.at(i-1).x,xPlan.at(i-1).y),QPointF(xPlan.at(i).x,xPlan.at(i).y));
+
+            int ncount = dis/0.1;
+            int j;
+            for(j=1;j<ncount;j++)
+            {
+                double x, y;
+
+                PlanPoint pp;
+                pp.x = xPlan.at(i-1).x + j*0.1 *cos(hdg);
+                pp.y = xPlan.at(i-1).y + j*0.1 *sin(hdg);
+                pp.hdg = hdg;
+                xPlan.insert(xPlan.begin()+i+j-1,pp);
+
+            }
+            qDebug("dis is %f",dis);
+        }
+    }
+}
+
+
+
+void ChangeLane(std::vector<PlanPoint> & xvectorPP)
+{
+    int i = 0;
+    int nsize = xvectorPP.size();
+    for(i=0;i<nsize;i++)
+    {
+        if((xvectorPP[i].mfSecx == xvectorPP[i].x)&&(xvectorPP[i].mfSecy == xvectorPP[i].y))
+        {
+
+        }
+        else
+        {
+            int k;
+            for(k=i;k<(nsize-1);k++)
+            {
+                if((xvectorPP[k].mfSecx == xvectorPP[k].x)&&(xvectorPP[k].mfSecy == xvectorPP[k].y))
+                {
+                    break;
+                }
+            }
+            int nnum = k-i;
+            int nchangepoint = 300;
+            double froadlen = sqrt(pow(xvectorPP[k].x - xvectorPP[i].x,2)
+                                   +pow(xvectorPP[k].y - xvectorPP[i].y,2));
+            const double fMAXCHANGE = 100;
+            if(froadlen<fMAXCHANGE)
+            {
+                nchangepoint = nnum;
+            }
+            else
+            {
+                nchangepoint = (fMAXCHANGE/froadlen) * nnum;
+            }
+
+            qDebug(" road %d len is %f changepoint is %d nnum is %d",
+                   xvectorPP[k-1].nRoadID,froadlen,nchangepoint,nnum);
+
+            int nstart = i + nnum/2 -nchangepoint/2;
+            int nend = i+nnum/2 + nchangepoint/2;
+            if(nnum<300)
+            {
+                nstart = i;
+                nend = k;
+            }
+            int j;
+            for(j=i;j<nstart;j++)
+            {
+                xvectorPP[j].x = xvectorPP[j].mfSecx;
+                xvectorPP[j].y = xvectorPP[j].mfSecy;
+            }
+            nnum = nend - nstart;
+            for(j=nstart;j<nend;j++)
+            {
+                double fdis = sqrt(pow(xvectorPP[j].x - xvectorPP[j].mfSecx,2)
+                                   +pow(xvectorPP[j].y - xvectorPP[j].mfSecy,2));
+                double foff = fdis *(j-nstart)/nnum;
+                if(xvectorPP[j].nlrchange == 1)
+                {
+//                    std::cout<<"foff is "<<foff<<std::endl;
+                    xvectorPP[j].x = xvectorPP[j].mfSecx + foff *cos(xvectorPP[j].hdg + M_PI/2.0);
+                    xvectorPP[j].y = xvectorPP[j].mfSecy + foff *sin(xvectorPP[j].hdg + M_PI/2.0);
+                    xvectorPP[j].mfDisToRoadLeft = xvectorPP[j].mfDisToRoadLeft + fdis - foff;
+                }
+                else
+                {
+                    xvectorPP[j].x = xvectorPP[j].mfSecx + foff *cos(xvectorPP[j].hdg - M_PI/2.0);
+                    xvectorPP[j].y = xvectorPP[j].mfSecy + foff *sin(xvectorPP[j].hdg - M_PI/2.0);
+                    xvectorPP[j].mfDisToRoadLeft = xvectorPP[j].mfDisToRoadLeft  - fdis +foff;
+                }
+            }
+            i =k;
+
+        }
+    }
+}
+
+#include <QFile>
+
+/**
+ * @brief MakePlan         全局路径规划
+ * @param pxd              有向图
+ * @param pxodr            OpenDrive地图
+ * @param x_now            当前x坐标
+ * @param y_now            当前y坐标
+ * @param head             当前航向
+ * @param x_obj            目标点x坐标
+ * @param y_obj            目标点y坐标
+ * @param obj_dis          目标点到路径的距离
+ * @param srcnearthresh    当前点离最近路径的阈值，如果不在这个范围，寻找失败
+ * @param dstnearthresh    目标点离最近路径的阈值，如果不在这个范围，寻找失败
+ * @param nlanesel         车道偏好，1为内车道，数值越大越偏外，如果数值不满足，会选择最靠外的。
+ * @param xPlan            返回的轨迹点
+ * @return                 0 成功  <0 失败
+ */
+int MakePlan(xodrdijkstra * pxd,OpenDrive * pxodr,const double x_now,const double y_now,const double head,
+             const double x_obj,const double y_obj,const double & obj_dis,
+             const double srcnearthresh,const double dstnearthresh,
+             const int nlanesel,std::vector<PlanPoint> & xPlan,const double fvehiclewidth )
+{
+    double  s;double nearx;
+    double  neary;double  nearhead;
+    Road * pRoad;GeometryBlock * pgeob;
+    Road * pRoad_obj;GeometryBlock * pgeob_obj;
+    double  s_obj;double nearx_obj;
+    double  neary_obj;double  nearhead_obj;
+    int lr_end = 2;
+    int lr_start = 2;
+    double fs1,fs2;
+//    int nfind = GetNearPoint(x_now,y_now,pxodr,&pRoad,&pgeob,s,nearx,neary,nearhead,srcnearthresh);
+
+    std::vector<iv::nearroad> xvectornearstart;
+    std::vector<iv::nearroad> xvectornearend;
+
+    GetNearPointWithHead(x_now,y_now,head,pxodr,xvectornearstart,srcnearthresh,true);
+    GetNearPointWithHead(x_obj,y_obj,0,pxodr,xvectornearend,dstnearthresh,false);
+
+    if(xvectornearstart.size()<1)
+    {
+        std::cout<<" plan fail. can't find start point."<<std::endl;
+        return -1;
+    }
+    if(xvectornearend.size() < 1)
+    {
+        std::cout<<" plan fail. can't find end point."<<std::endl;
+        return -2;
+    }
+
+    std::vector<std::vector<PlanPoint>> xvectorplans;
+    std::vector<double> xvectorroutedis;
+
+    int i;
+    int j;
+    for(i=0;i<(int)xvectornearstart.size();i++)
+    {
+        for(j=0;j<(int)xvectornearend.size();j++)
+        {
+            iv::nearroad * pnearstart = &xvectornearstart[i];
+            iv::nearroad * pnearend = &xvectornearend[j];
+            pRoad = pnearstart->pObjRoad;
+            pgeob = pnearstart->pgeob;
+            pRoad_obj = pnearend->pObjRoad;
+            pgeob_obj = pnearend->pgeob;
+            lr_start = pnearstart->lr;
+            lr_end = pnearend->lr;
+
+            nearx = pnearstart->nearx;
+            neary = pnearstart->neary;
+
+            nearx_obj = pnearend->nearx;
+            neary_obj = pnearend->neary;
+
+            bool bNeedDikstra = true;
+            if((atoi(pRoad->GetRoadId().data()) == atoi(pRoad_obj->GetRoadId().data()))&&(lr_start == lr_end) &&(pRoad->GetLaneSectionCount() == 1))
+            {
+                std::vector<PlanPoint> xvPP = GetPoint(pRoad);
+                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;
+                }
+                if((nindexend <nindexstart)&&(lr_start == 1))
+                {
+                    bNeedDikstra = false;
+                }
+                if(bNeedDikstra == false)
+                {
+
+                    std::vector<iv::lanewidthabcd> xvectorLWA = GetLaneWidthABCD(pRoad);
+                    std::vector<int> xvectorindex1;
+
+                    xvectorindex1 = GetLWIndex(xvectorLWA,nlane);
+
+                    double foff = getoff(pRoad,nlane);
+
+                    foff = foff + 0.0;
+                    std::vector<PlanPoint> xvectorPP;
+                    int i = nindexstart;
+                    while(i!=nindexend)
+                    {
+
+                        if(lr_start == 2)
+                        {
+                            PlanPoint pp = xvPP.at(i);
+                            foff = getoff(nlane,pp.mS,xvectorLWA,xvectorindex1);
+                            pp.x = pp.x + foff *cos(pp.hdg + M_PI/2.0);
+                            pp.y = pp.y + foff *sin(pp.hdg + M_PI/2.0);
+                            pp.mWidth = getwidth(pRoad,nlane,xvectorLWA,pp.mS);
+                            pp.mfDisToLaneLeft = pp.mWidth/2.0;
+                            pp.lanmp = 0;
+                            pp.mfDisToRoadLeft = foff *(-1.0);
+                            pp.mfRoadWidth = GetRoadWidth(xvectorLWA,nlane,pp.mS);
+                            GetLaneOriTotal(pRoad,nlane,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed,pp.mbNoavoid);
+
+                            xvectorPP.push_back(pp);
+                            i++;
+                        }
+                        else
+                        {
+                            PlanPoint pp = xvPP.at(i);
+                            foff = getoff(nlane,pp.mS,xvectorLWA,xvectorindex1);
+                            pp.x = pp.x + foff *cos(pp.hdg + M_PI/2.0);
+                            pp.y = pp.y + foff *sin(pp.hdg + M_PI/2.0);
+                            pp.hdg = pp.hdg + M_PI;
+
+                            pp.mWidth = getwidth(pRoad,nlane,xvectorLWA,pp.mS);
+                            pp.mfDisToLaneLeft = pp.mWidth/2.0;
+                            pp.lanmp = 0;
+                            pp.mfDisToRoadLeft = foff;
+                            pp.mfRoadWidth = GetRoadWidth(xvectorLWA,nlane,pp.mS);
+                            GetLaneOriTotal(pRoad,nlane,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed,pp.mbNoavoid);
+
+
+                            xvectorPP.push_back(pp);
+                            i--;
+                        }
+                    }
+
+                    for(i=0;i<(int)xvectorPP.size();i++)
+                    {
+                        xvectorPP[i].mfCurvature = pRoad->GetRoadCurvature(xvectorPP[i].mS);
+                    }
+
+                    pathsection xps;
+                    xps.mbStartToMainChange = false;
+                    xps.mbMainToEndChange = false;
+         //           CalcInLaneAvoid(xps,xvectorPP,fvehiclewidth,0,0,0);
+                    xPlan = xvectorPP;
+
+                    xvectorplans.push_back(xvectorPP);
+                    xvectorroutedis.push_back(xvectorPP.size() * 0.1);
+
+                }
+            }
+
+            if(bNeedDikstra)
+            {
+                bool bSuc = false;
+            std::vector<int> xpath = pxd->getpath(atoi(pRoad->GetRoadId().data()),lr_start,atoi(pRoad_obj->GetRoadId().data()),lr_end,bSuc,fs1,fs2);
+            if(xpath.size()<1)
+            {
+                continue;
+            }
+            if(bSuc == false)
+            {
+                continue;
+            }
+            double flen = pxd->getpathlength(xpath);
+            std::vector<pathsection> xpathsection = pxd->getgpspoint(atoi(pRoad->GetRoadId().data()),lr_start,atoi(pRoad_obj->GetRoadId().data()),lr_end,xpath,nlanesel);
+
+            std::vector<PlanPoint> xvectorPP = GetPlanPoint(xpathsection,pRoad,pgeob,pRoad_obj,pgeob_obj,x_now,y_now,
+                         head,nearx,neary,nearhead,nearx_obj,neary_obj,fvehiclewidth,flen);
+
+            ChangeLane(xvectorPP);
+            xvectorplans.push_back(xvectorPP);
+            xvectorroutedis.push_back(flen);
+
+            }
+
+        }
+    }
+
+    if(xvectorplans.size() > 0)
+    {
+        if(xvectorplans.size() == 1)
+        {
+            xPlan = xvectorplans[0];
+        }
+        else
+        {
+            int nsel = 0;
+            double fdismin = xvectorroutedis[0];
+            for(i=1;i<(int)xvectorroutedis.size();i++)
+            {
+                if(xvectorroutedis[i]<fdismin)
+                {
+                    nsel = i;
+                }
+            }
+            xPlan = xvectorplans[nsel];
+        }
+        return 0;
+    }
+    std::cout<<" plan fail. can't find path."<<std::endl;
+    return -1000;
+
+}

src/driver/driver_map_xodrload_new/globalplan.h

@@ -0,0 +1,26 @@
+#ifndef GLOBALPLAN_H
+#define GLOBALPLAN_H
+#include "OpenDrive/OpenDrive.h"
+
+#include <vector>
+
+#include "xodrfunc.h"
+
+#include "xodrdijkstra.h"
+
+#include "planpoint.h"
+
+class LaneChangePoint
+{
+    int nRoadID = -1;
+    double mS = 0;
+};
+
+int GetNearPoint(const double x,const double y,OpenDrive * pxodr,Road ** pObjRoad,GeometryBlock ** pgeo, double & s,double & nearx,
+                 double & neary,double & nearhead,const double nearthresh);
+
+int MakePlan(xodrdijkstra * pxd,OpenDrive * pxodr,const double x_now,const double y_now,const double head,
+             const double x_obj,const double y_obj,const double & obj_dis,
+             const double srcnearthresh,const double dstnearthresh,
+             const int nlanesel,std::vector<PlanPoint> & xPlan,const double fvehiclewidth = 2.0);
+#endif // GLOBALPLAN_H

src/driver/driver_map_xodrload_new/gnss_coordinate_convert.cpp

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

src/driver/driver_map_xodrload_new/gnss_coordinate_convert.h

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

src/driver/driver_map_xodrload_new/gps_type.h

@@ -0,0 +1,119 @@
+#pragma once
+/*
+*GPS 惯导数据
+*/
+#ifndef _IV_COMMON_GPS_TYPE_
+#define _IV_COMMON_GPS_TYPE_
+
+#include "boost.h"
+namespace iv {
+    struct GPS_INS
+    {
+        int valid = 0xff;
+        int index = 0;	//gps点序号
+
+        double gps_lat = 0;//纬度
+        double gps_lng = 0;//经度
+
+        double gps_x = 0;
+        double gps_y = 0;
+        double gps_z = 0;
+
+        double frenet_s=0;
+        double frenet_s_dot=0;
+        double frenet_s_dot_dot=0;
+        double frenet_d=0;
+        double frenet_d_dot=0;
+        double frenet_d_dot_dot=0;
+
+        double ins_roll_angle = 0;	//横滚角 一般定义载体的右、前、上三个方向构成右手系，绕向前的轴旋转就是横滚角，绕向右的轴旋转就是俯仰角，绕向上的轴旋转就是航向角
+        double ins_pitch_angle = 0;	//俯仰角
+        double ins_heading_angle = 0;	//航向角
+
+        int ins_status = 0;	//惯导状态 4
+        int rtk_status = 0;	//rtk状态 6 -5 -3
+        int gps_satelites_num = 0;
+
+        //-----加速度--------------
+        double accel_x = 0;
+        double accel_y = 0;
+        double accel_z = 0;
+
+        //-------角速度------------
+        double ang_rate_x = 0;
+        double ang_rate_y = 0;
+        double ang_rate_z = 0;
+
+        //-----------方向速度--------------
+        double vel_N = 0;
+        double vel_E = 0;
+        double vel_D = 0;
+
+        int speed_mode = 0;
+        int mode2 = 0;
+        double speed = 0;			//速度  若导航点则为导航预设速度  若为当前点则为当前车速
+
+        int roadMode;
+        int runMode;
+        int roadSum;
+        int roadOri;
+
+        double mfLaneWidth = 3.5; // Current Lane Width
+
+        double mfDisToLaneLeft = 1.8; //Distance to Lane Left
+        int mnLaneChangeMark = 0; //1 to Left 0 not change   -1 to right
+        double mfDisToRoadLeft = 1.8; //Distance to Road Left
+        double mfRoadWidth = 3.5; // Road Width
+
+        bool mbInLaneAvoid = false; //if true suport In Lane Avoid
+        double gps_lat_avoidleft;
+        double gps_lng_avoidleft;
+        double gps_lat_avoidright;
+        double gps_lng_avoidright;
+        double gps_x_avoidleft = 0;
+        double gps_y_avoidleft = 0;
+        double gps_x_avoidright = 0;
+        double gps_y_avoidright = 0;
+
+        bool mbnoavoid = false; //If true this point is no avoid.
+        double mfCurvature = 0.0;
+
+        char mcreserved[10];
+        int mnreserved[5];
+        double mfreserved[2];
+
+
+
+    };
+
+    typedef boost::shared_ptr<iv::GPS_INS> GPSData;
+    class Point2D
+   {
+     public:
+        double x = 0, y = 0, speed=0,obs_speed_x=0,obs_speed_y=0;
+        int v1 = 0, v2 = 0;
+        int roadMode = 0;
+        int obs_type=0;
+
+        double s=0;   //frenet coordinate : s,d;   add at 20220209
+        double d=0;
+
+
+        Point2D()
+       {
+           x = y = v1 = 0;
+       }
+
+        Point2D(double _x, double _y)
+       {
+           x = _x; y = _y;
+       }
+
+
+    };
+
+
+
+
+}
+#endif // !_IV_COMMON_GPS_TYPE_

src/driver/driver_map_xodrload_new/main.cpp

@@ -0,0 +1,1252 @@
+#include <QCoreApplication>
+#include <math.h>
+#include <string>
+#include <thread>
+
+#include <QFile>
+
+#include "OpenDrive/OpenDrive.h"
+#include "OpenDrive/OpenDriveXmlParser.h"
+
+#include "globalplan.h"
+
+#include "gpsimu.pb.h"
+
+#include "mapdata.pb.h"
+
+#include "v2x.pb.h"
+
+#include "modulecomm.h"
+
+#include "xmlparam.h"
+
+#include "gps_type.h"
+
+#include "xodrdijkstra.h"
+
+#include "gnss_coordinate_convert.h"
+
+#include "ivexit.h"
+
+#include "ivversion.h"
+
+#include "ivbacktrace.h"
+
+#include <signal.h>
+
+#include "service_roi_xodr.h"
+#include "ivservice.h"
+
+OpenDrive mxodr;
+xodrdijkstra * gpxd;
+bool gmapload = false;
+
+double glat0,glon0,ghead0;
+
+double gvehiclewidth = 2.0;
+
+bool gbExtendMap = true;
+
+static bool gbSideEnable = false;
+static bool gbSideLaneEnable = false;
+
+void * gpa;
+void * gpasrc;
+void * gpmap;
+void * gpagps;
+void * gpasimple;
+void * gpav2x;
+static void * gpanewtrace;
+
+iv::Ivfault *gfault = nullptr;
+iv::Ivlog *givlog = nullptr;
+
+static QCoreApplication * gApp;
+
+service_roi_xodr gsrx;
+
+
+namespace iv {
+struct simpletrace
+{
+    double gps_lat = 0;//纬度
+    double gps_lng = 0;//经度
+
+    double gps_x = 0;
+    double gps_y = 0;
+    double gps_z = 0;
+
+
+    double ins_heading_angle = 0;	//航向角
+};
+
+}
+/**
+ *
+ *
+ *
+ *
+ * */
+bool LoadXODR(std::string strpath)
+{
+    OpenDriveXmlParser xp(&mxodr);
+    xp.ReadFile(strpath);
+    std::cout<<"road cout is "<<mxodr.GetRoadCount()<<std::endl;
+    if(mxodr.GetRoadCount() < 1)
+    {
+        gmapload = true;
+        return false;
+    }
+
+
+    xodrdijkstra * pxd = new xodrdijkstra(&mxodr);
+    gpxd = pxd;
+//    std::vector<int> xpath = pxd->getpath(10001,2,30012,2);
+
+//    pxd->getgpspoint(10001,2,30012,2,xpath);
+
+    int i;
+    double  nlenth = 0;
+    int nroadsize = mxodr.GetRoadCount();
+    for(i=0;i<nroadsize;i++)
+    {
+        Road * px = mxodr.GetRoad(i);
+        nlenth = nlenth + px->GetRoadLength();
+        int bloksize = px->GetGeometryBlockCount();
+        if(px->GetGeometryBlock(0)->GetGeometryAt(0)->GetGeomType() == 4)
+        {
+            GeometryParamPoly3 * p = (GeometryParamPoly3 *) px->GetGeometryBlock(0)->GetGeometryAt(0);
+            double a = p->GetuA();
+            a = p->GetuB();
+            a = p->GetuC();
+            a = p->GetuD();
+            a = p->GetvA();
+        }
+
+
+    }
+
+//    void Header::GetAllParams(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)
+
+
+    unsigned short int revMajor,revMinor;
+    std::string name,date;
+    float version;
+    double north,south,east,west,lat0,lon0,hdg0;
+    if(mxodr.GetHeader() != 0)
+    {
+        mxodr.GetHeader()->GetAllParams(revMajor,revMinor,name,version,date,north,south,east,west,lat0,lon0,hdg0);
+        glat0 = lat0;
+        glon0 = lon0;
+    }
+
+
+
+    Road * proad1 = mxodr.GetRoad(0);
+    givlog->info("road name is %s",proad1->GetRoadName().data());
+    std::cout<<" road name is "<<proad1->GetRoadName()<<std::endl;
+
+    gsrx.SetXODR(&mxodr);
+}
+
+class roadx
+{
+  public:
+    roadx * para;
+    std::vector<roadx> child;
+    int nlen;
+};
+
+
+#define EARTH_RADIUS 6370856.0
+
+//从1到2的距离和方向
+int CalcDisAngle(double lat1,double lon1,double lat2,double lon2,double * pLatDis,double * pLonDis,double * pDis,double * pangle)
+{
+    double a,b;
+    double LonDis,LatDis;
+    double LonRadius;
+    double Dis;
+    double angle;
+    if((lat1 == lat2)&&(lon1 == lon2))return -1;
+
+    LonRadius = EARTH_RADIUS * cos(lat2/(180.0/M_PI));
+    a = (EARTH_RADIUS * M_PI*2.0/360.0)/(100000.0);
+    b = lat2 - lat1; b = b*100000.0;
+    LatDis = a*b;
+    a = (LonRadius * M_PI*2.0/360.0)/100000.0;
+    b = lon2 - lon1; b = b*100000.0;
+    LonDis = a*b;
+    Dis = sqrt(LatDis*LatDis + LonDis *LonDis);
+    angle = acos(fabs(LonDis)/Dis);
+    angle = angle * 180.0/M_PI;
+
+    if(LonDis > 0)
+    {
+        if(LatDis > 0)angle = 90.0 - angle;
+        else angle= 90.0+angle;
+    }
+    else
+    {
+        if(LatDis > 0)angle = 270.0+angle;
+        else angle = 270.0-angle;
+    }
+
+    if(pLatDis != 0)*pLatDis = LatDis;
+    if(pLonDis != 0)*pLonDis = LonDis;
+    if(pDis != 0)*pDis = Dis;
+    if(pangle != 0)*pangle = angle;
+}
+
+
+
+//==========================================================
+
+//高斯投影由经纬度(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;
+//}
+
+#include <math.h>
+static void OffLngLat(double fLat0,double fLng0,double & fLat,double & fLng,double fHeading,double x,double y)
+{
+    double fxdiff,fydiff;
+
+    double xoff = y*(-1);
+    double yoff = x;
+
+    fxdiff = 0+xoff * cos(fHeading*M_PI/180.0)+ yoff*sin(fHeading*M_PI/180.0);  //East
+    fydiff = 0-xoff * sin(fHeading*M_PI/180.0)+ yoff*cos(fHeading*M_PI/180.0);  //South
+
+    double fEarthRadius = 6378245.0;
+
+    double ns1d = fEarthRadius*2*M_PI/360.0;
+    double fewRadius = fEarthRadius * cos(fLat0*M_PI/180.0);
+    double ew1d = fewRadius * 2*M_PI/360.0;
+
+    fLat = fLat0 + fydiff/ns1d;
+    fLng = fLng0 + fxdiff/ew1d;
+
+
+}
+
+void CalcXY(const double lat0,const double lon0,const double hdg0,
+              const double lat,const double lon,
+              double & x,double & y)
+{
+
+    double x0,y0;
+    GaussProjCal(lon0,lat0,&x0,&y0);
+    GaussProjCal(lon,lat,&x,&y);
+    x = x - x0;
+    y = y-  y0;
+
+//    double ang,dis;
+//    CalcDisAngle(lat0,lon0,lat,lon,0,0,&dis,&ang);
+//    double xang = hdg0 - ang;
+//    while(xang<0)xang = xang + 360.0;
+//    x = dis * cos(xang * M_PI/180.0);
+//    y = dis * sin(xang * M_PI/180.0);
+}
+
+//void CalcLatLon(const double lat0,const double lon0,const double hdg0,
+//                const double x,const double y,const double xyhdg,
+//                double &lat,double & lon, double & hdg)
+//{
+//    OffLngLat(lat0,lon0,lat,lon,hdg0,x,y);
+//    hdg = hdg0 - xyhdg * 180.0/M_PI;
+//    while(hdg < 0)hdg = hdg + 360;
+//    while(hdg >= 360)hdg = hdg - 360;
+//}
+
+
+void CalcLatLon(const double lat0,const double lon0,
+                const double x,const double y,
+                double &lat,double & lon)
+{
+    double x0,y0;
+    GaussProjCal(lon0,lat0,&x0,&y0);
+    double x_gps,y_gps;
+    x_gps = x0+x;
+    y_gps = y0+y;
+    GaussProjInvCal(x_gps,y_gps,&lon,&lat);
+}
+
+
+void CalcLatLon(const double lat0,const double lon0,const double hdg0,
+                const double x,const double y,const double xyhdg,
+                double &lat,double & lon, double & hdg)
+{
+    double x0,y0;
+    GaussProjCal(lon0,lat0,&x0,&y0);
+    double x_gps,y_gps;
+    x_gps = x0+x;
+    y_gps = y0+y;
+    GaussProjInvCal(x_gps,y_gps,&lon,&lat);
+ //   hdg = hdg0 -xyhdg * 270/M_PI;
+    hdg = 90 -  xyhdg* 180.0/M_PI;
+
+//    OffLngLat(lat0,lon0,lat,lon,hdg0,x,y);
+//    hdg = hdg0 - xyhdg * 180.0/M_PI;
+    while(hdg < 0)hdg = hdg + 360;
+    while(hdg >= 360)hdg = hdg - 360;
+}
+
+class xodrobj
+{
+public:
+    double flatsrc;
+    double flonsrc;
+    double fhgdsrc;
+    double flat;
+    double flon;
+    int lane;
+};
+
+xodrobj gsrc;
+
+void ShareMap(std::vector<iv::GPSData> navigation_data)
+{
+    if(navigation_data.size()<1)return;
+    iv::GPS_INS x;
+    x = *(navigation_data.at(0));
+    char * data = new char[sizeof(iv::GPS_INS)*navigation_data.size()];
+    int gpssize = sizeof(iv::GPS_INS);
+    int i;
+    for(i=0;i<navigation_data.size();i++)
+    {
+        x = *(navigation_data.at(i));
+        memcpy(data+i*gpssize,&x,gpssize);
+    }
+
+    iv::modulecomm::ModuleSendMsg(gpmap,data,navigation_data.size()*gpssize);
+
+
+    int nsize = 100;
+    int nstep = 1;
+    if(navigation_data.size() < 100)
+    {
+        nsize = navigation_data.size();
+
+    }
+    else
+    {
+        nstep = navigation_data.size()/100;
+    }
+
+    iv::simpletrace psim[100];
+    for(i=0;i<nsize;i++)
+    {
+        x = *(navigation_data.at(i*nstep));
+        psim[i].gps_lat = x.gps_lat;
+        psim[i].gps_lng = x.gps_lng;
+        psim[i].gps_z = x.gps_z;
+        psim[i].gps_x = x.gps_x;
+        psim[i].gps_y = x.gps_y;
+        psim[i].ins_heading_angle = x.ins_heading_angle;
+    }
+    if(navigation_data.size()>100)
+    {
+        int nlast = 99;
+        x = *(navigation_data.at(navigation_data.size()-1));
+        psim[nlast].gps_lat = x.gps_lat;
+        psim[nlast].gps_lng = x.gps_lng;
+        psim[nlast].gps_z = x.gps_z;
+        psim[nlast].gps_x = x.gps_x;
+        psim[nlast].gps_y = x.gps_y;
+        psim[nlast].ins_heading_angle = x.ins_heading_angle;
+    }
+
+    iv::modulecomm::ModuleSendMsg(gpasimple,(char *)psim,nsize * sizeof(iv::simpletrace));
+
+    delete data;
+}
+
+static void ToGPSTrace(std::vector<PlanPoint> xPlan)
+{
+//    double x_src,y_src,x_dst,y_dst;
+
+
+
+//    x_src = -26;y_src = 10;
+//    x_dst = -50;y_dst = -220;
+
+    int i;
+    int nSize = xPlan.size();
+
+    std::vector<iv::GPSData> mapdata;
+
+
+    QFile xfile;
+    QString strpath;
+    strpath = getenv("HOME");
+    strpath = strpath + "/map/maptrace.txt";
+    xfile.setFileName(strpath);
+    bool bFileOpen = xfile.open(QIODevice::ReadWrite);
+
+    for(i=0;i<nSize;i++)
+    {
+        iv::GPSData data(new iv::GPS_INS);
+        CalcLatLon(glat0,glon0,ghead0,xPlan[i].x,xPlan[i].y,xPlan[i].hdg,data->gps_lat,
+                   data->gps_lng,data->ins_heading_angle);
+
+        data->index = i;
+        data->speed = xPlan[i].speed;
+        GaussProjCal(data->gps_lng,data->gps_lat,&data->gps_x,&data->gps_y);
+
+
+        givlog->debug(" %d %11.7f %11.7f %11.3f ",i,data->gps_lat,
+               data->gps_lng,data->ins_heading_angle);
+
+        data->roadSum = xPlan[i].mnLaneTotal;
+        data->roadMode = 0;
+        data->roadOri = xPlan[i].mnLaneori;
+
+        data->mfDisToLaneLeft = xPlan[i].mfDisToLaneLeft;
+        data->mfDisToRoadLeft = xPlan[i].mfDisToRoadLeft;
+        data->mfLaneWidth = xPlan[i].mWidth;
+        data->mfRoadWidth = xPlan[i].mfRoadWidth;
+        data->mnLaneChangeMark = xPlan[i].lanmp;
+
+        if(xPlan[i].lanmp == -1)data->roadMode = 15;
+        if(xPlan[i].lanmp == 1)data->roadMode = 14;
+
+        mapdata.push_back(data);
+
+        char strline[255];
+        snprintf(strline,255,"%d\t%11.7f\t%11.7f\t%d\t%d\t%11.3f\t%d\t%d\t%d\t%d\n",
+                 i,data->gps_lng,data->gps_lat,0,0,data->ins_heading_angle,0,0,0,0);
+        if(bFileOpen)  xfile.write(strline);
+    //                             fout << gps_index << "\t" << data->gps_lng << "\t" << data->gps_lat << "\t" << ServiceCarStatus.location->speed_mode << "\t" << ServiceCarStatus.location->mode2 << "\t" << data->ins_heading_angle << "\t" << obs_modes << "\t" << speed_modes << "\t" << lane_num << "\t" << lane_status <<"\t" <<road_width <<std::endl;
+
+    }
+    if(bFileOpen)xfile.close();
+
+    ShareMap(mapdata);
+}
+
+int avoidroadid[] = {10002,10019,10003,10098,10099,10063,10099,10100,10104,10110,10111};
+
+inline bool isboringroad(int nroadid)
+{
+    int i;
+    bool brtn = false;
+    for(i=0;i<11;i++)
+    {
+        if(avoidroadid[i] == nroadid)
+        {
+            brtn = true;
+            break;
+        }
+    }
+    return brtn;
+}
+
+
+void CalcLaneSide(std::vector<PlanPoint> & xPlan)
+{
+    const double fsidedis = 0.3;
+    const int nChangePoint = 150;
+    const int nStopPoint = 50;
+    if(xPlan.size()<nChangePoint)return;
+
+    int i;
+    int nsize = xPlan.size();
+
+
+    for(i=(nsize-1);i>=(nsize - nStopPoint);i--)
+    {
+        double fMove = xPlan[i].mfRoadWidth - xPlan[i].mfDisToRoadLeft - (gvehiclewidth/2.0 + fsidedis);
+   //     double fMove = xPlan[i].mWidth/2.0 - (gvehiclewidth/2.0 + fsidedis);
+        double xold = xPlan[i].x;
+        double yold = xPlan[i].y;
+        xPlan[i].x = xold + fMove*cos(xPlan[i].hdg - M_PI/2.0);
+        xPlan[i].y = yold + fMove*sin(xPlan[i].hdg - M_PI/2.0);
+        xPlan[i].mfDisToLaneLeft = xPlan[i].mfDisToLaneLeft + fMove;
+    }
+
+    for(i=(nsize-nStopPoint-1);i>=(nsize - nChangePoint);i--)
+    {
+        double fMove = xPlan[i].mfRoadWidth - xPlan[i].mfDisToRoadLeft - (gvehiclewidth/2.0 + fsidedis);
+//        double fMove = xPlan[i].mWidth/2.0 - (gvehiclewidth/2.0 + fsidedis);
+        double xold = xPlan[i].x;
+        double yold = xPlan[i].y;
+        double fRatio = 1.0 - ((nsize-nStopPoint) -i )*1.0/(nChangePoint-nStopPoint);
+        xPlan[i].x = xold + fRatio*fMove*cos(xPlan[i].hdg - M_PI/2.0);
+        xPlan[i].y = yold + fRatio*fMove*sin(xPlan[i].hdg - M_PI/2.0);
+        xPlan[i].mfDisToLaneLeft = xPlan[i].mfDisToLaneLeft + fRatio*fMove;
+    }
+    return;
+}
+
+void CalcSide(std::vector<PlanPoint> & xPlan)
+{
+    const double fsidedis = 0.3;
+    const int nChangePoint = 150;
+    const int nStopPoint = 50;
+    if(xPlan.size()<nChangePoint)return;
+    bool bChange = true;
+    int i;
+    int nsize = xPlan.size();
+    for(i=(nsize-1);i>=(nsize - nChangePoint);i--)
+    {
+        if(xPlan[i].mWidth<(2.0*(gvehiclewidth/2.0+fsidedis)))
+        {
+            std::cout<<" Because Lane is narrow, not change."<<std::endl;
+            bChange = false;
+            break;
+        }
+    }
+
+    if(bChange == false)return;
+
+    for(i=(nsize-1);i>=(nsize - nStopPoint);i--)
+    {
+        double fMove = xPlan[i].mWidth/2.0 - (gvehiclewidth/2.0 + fsidedis);
+        double xold = xPlan[i].x;
+        double yold = xPlan[i].y;
+        xPlan[i].x = xold + fMove*cos(xPlan[i].hdg - M_PI/2.0);
+        xPlan[i].y = yold + fMove*sin(xPlan[i].hdg - M_PI/2.0);
+        xPlan[i].mfDisToLaneLeft = xPlan[i].mfDisToLaneLeft + fMove;
+    }
+
+    for(i=(nsize-nStopPoint-1);i>=(nsize - nChangePoint);i--)
+    {
+        double fMove = xPlan[i].mWidth/2.0 - (gvehiclewidth/2.0 + fsidedis);
+        double xold = xPlan[i].x;
+        double yold = xPlan[i].y;
+        double fRatio = 1.0 - ((nsize-nStopPoint) -i )*1.0/(nChangePoint-nStopPoint);
+        xPlan[i].x = xold + fRatio*fMove*cos(xPlan[i].hdg - M_PI/2.0);
+        xPlan[i].y = yold + fRatio*fMove*sin(xPlan[i].hdg - M_PI/2.0);
+        xPlan[i].mfDisToLaneLeft = xPlan[i].mfDisToLaneLeft + fRatio*fMove;
+    }
+    return;
+}
+
+void SetPlan(xodrobj xo)
+{
+
+    double x_src,y_src,x_dst,y_dst;
+
+    CalcXY(glat0,glon0,ghead0,xo.flatsrc,xo.flonsrc,x_src,y_src);
+    CalcXY(glat0,glon0,ghead0,xo.flat,xo.flon,x_dst,y_dst);
+
+
+    std::vector<PlanPoint> xPlan;
+
+    double s;
+
+//    x_src = -5;y_src = 6;
+//    x_dst = -60;y_src = -150;
+    int nRtn = MakePlan(gpxd,&mxodr,x_src,y_src,(90 - xo.fhgdsrc)*M_PI/180.0,x_dst,y_dst,s,30.0,100.0,xo.lane,xPlan);
+
+    if(nRtn < 0)
+    {
+        qDebug("plan fail.");
+        return;
+    }
+
+    int i;
+    int nSize = xPlan.size();
+
+    if(gbSideLaneEnable)
+    {
+        CalcLaneSide(xPlan);
+    }
+    else
+    {
+        if(gbSideEnable)
+        {
+            CalcSide(xPlan);
+        }
+    }
+
+    if(nSize<1)
+    {
+        qDebug("plan fail.");
+        return;
+    }
+    PlanPoint xLastPoint = xPlan[nSize -1];
+    for(i=0;i<600;i++)
+    {
+        PlanPoint pp = xLastPoint;
+        double fdis = 0.1*(i+1);
+        pp.mS = pp.mS + i*0.1;
+        pp.x = pp.x + fdis * cos(pp.hdg);
+        pp.y = pp.y + fdis * sin(pp.hdg);
+        pp.nSignal = 23;
+        if(gbExtendMap)
+        {
+            xPlan.push_back(pp);
+        }
+
+    }
+    
+
+   iv::map::tracemap xtrace;
+
+    nSize = xPlan.size();
+
+    std::vector<iv::GPSData> mapdata;
+    //maptrace
+    QFile xfile;
+    QString strpath;
+    strpath = getenv("HOME");
+    strpath = strpath + "/map/maptrace.txt";
+    xfile.setFileName(strpath);
+    xfile.open(QIODevice::ReadWrite);
+
+    //maptrace_add
+    QFile xfile_1;
+    QString strpath_1;
+    strpath_1 = getenv("HOME");
+    strpath_1 = strpath_1 + "/map/mapadd.txt";
+    xfile_1.setFileName(strpath_1);
+    xfile_1.open(QIODevice::ReadWrite);
+
+    for(i=0;i<nSize;i++)
+    {
+        iv::map::mappoint * pmappoint =  xtrace.add_point();
+        double gps_lon,gps_lat,gps_lon_avoidleft,gps_lat_avoidleft,gps_lat_avoidright,gps_lon_avoidright,gps_heading;
+        CalcLatLon(glat0,glon0,ghead0,xPlan[i].x,xPlan[i].y,xPlan[i].hdg,gps_lat,
+                  gps_lon,gps_heading);
+        CalcLatLon(glat0,glon0,xPlan[i].mx_left,xPlan[i].my_left,
+                   gps_lat_avoidleft,gps_lon_avoidleft);
+        CalcLatLon(glat0,glon0,xPlan[i].mx_right,xPlan[i].my_right,
+                   gps_lat_avoidright,gps_lon_avoidright);
+
+        pmappoint->set_gps_lat(gps_lat);
+        pmappoint->set_gps_lat_avoidleft(gps_lat_avoidleft);
+        pmappoint->set_gps_lat_avoidright(gps_lat_avoidright);
+        pmappoint->set_gps_lng(gps_lon);
+        pmappoint->set_gps_lat_avoidleft(gps_lat_avoidleft);
+        pmappoint->set_gps_lng_avoidright(gps_lon_avoidright);
+        pmappoint->set_ins_heading_angle(gps_heading);
+
+        double gps_x,gps_y,gps_x_avoidleft,gps_y_avoidleft,gps_x_avoidright,gps_y_avoidright;
+
+        GaussProjCal(gps_lon,gps_lat,&gps_x,&gps_y);
+        GaussProjCal(gps_lon_avoidleft,gps_lat_avoidleft,&gps_x_avoidleft,&gps_y_avoidleft);
+        GaussProjCal(gps_lon_avoidright,gps_lat_avoidright,&gps_x_avoidright,&gps_y_avoidright);
+
+        pmappoint->set_gps_x(gps_x);
+        pmappoint->set_gps_x_avoidleft(gps_x_avoidleft);
+        pmappoint->set_gps_x_avoidright(gps_x_avoidright);
+        pmappoint->set_gps_y(gps_y);
+        pmappoint->set_gps_y_avoidleft(gps_y_avoidleft);
+        pmappoint->set_gps_y_avoidright(gps_y_avoidright);
+
+        pmappoint->set_speed(xPlan[i].speed);
+        pmappoint->set_index(i);
+
+        pmappoint->set_roadori(xPlan[i].mnLaneori);
+        pmappoint->set_roadsum(xPlan[i].mnLaneTotal);
+        pmappoint->set_mfdistolaneleft(xPlan[i].mfDisToLaneLeft);
+        pmappoint->set_mfdistoroadleft(xPlan[i].mfDisToRoadLeft);
+        pmappoint->set_mflanewidth(xPlan[i].mWidth);
+        pmappoint->set_mfroadwidth(xPlan[i].mfRoadWidth);
+        pmappoint->set_mbinlaneavoid(xPlan[i].bInlaneAvoid);
+
+
+        iv::GPSData data(new iv::GPS_INS);
+        data->gps_lat = gps_lat;
+        data->gps_lat_avoidleft = gps_lat_avoidleft;
+        data->gps_lat_avoidright = gps_lat_avoidright;
+        data->gps_lng = gps_lon;
+        data->gps_lng_avoidleft = gps_lon_avoidleft;
+        data->gps_lng_avoidright =  gps_lon_avoidright;
+        data->ins_heading_angle = gps_heading;
+        data->gps_x = gps_x;
+        data->gps_x_avoidleft = gps_x_avoidleft;
+        data->gps_x_avoidright = gps_x_avoidright;
+        data->gps_y = gps_y;
+        data->gps_y_avoidleft = gps_y_avoidleft;
+        data->gps_y_avoidright = gps_y_avoidright;
+        pmappoint->set_mbnoavoid(xPlan[i].mbNoavoid);
+        pmappoint->set_mfcurvature(xPlan[i].mfCurvature);
+
+
+//        CalcLatLon(glat0,glon0,ghead0,xPlan[i].x,xPlan[i].y,xPlan[i].hdg,data->gps_lat,
+//                   data->gps_lng,data->ins_heading_angle);
+//        CalcLatLon(glat0,glon0,xPlan[i].mx_left,xPlan[i].my_left,
+//                   data->gps_lat_avoidleft,data->gps_lng_avoidleft);
+//        CalcLatLon(glat0,glon0,xPlan[i].mx_right,xPlan[i].my_right,
+//                   data->gps_lat_avoidright,data->gps_lng_avoidright);
+        data->index = i;
+        data->speed = xPlan[i].speed;
+ //       ZBGaussProjCal(data->gps_lng,data->gps_lat,&data->gps_x,&data->gps_y);
+//        GaussProjCal(data->gps_lng,data->gps_lat,&data->gps_x,&data->gps_y);
+//        GaussProjCal(data->gps_lng_avoidleft,data->gps_lat_avoidleft,&data->gps_x_avoidleft,&data->gps_y_avoidleft);
+//        GaussProjCal(data->gps_lng_avoidright,data->gps_lat_avoidright,&data->gps_x_avoidright,&data->gps_y_avoidright);
+
+        givlog->verbose(" %d %11.7f %11.7f %11.3f ",i,data->gps_lat,
+               data->gps_lng,data->ins_heading_angle);
+
+        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->mbInLaneAvoid = xPlan[i].bInlaneAvoid;
+
+        if(xPlan[i].mbBoringRoad)
+        {
+            data->roadOri = 0;
+            data->roadSum = 2;
+            pmappoint->set_roadori(0);
+            pmappoint->set_roadsum(2);
+        }
+
+
+        data->mbnoavoid = xPlan[i].mbNoavoid;
+        if(data->mbnoavoid)
+        {
+            qDebug("no avoid i = %d",i);
+        }
+        data->mfCurvature = xPlan[i].mfCurvature;
+
+        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;
+                if((mapdata.at(k)->mode2 != -1)&&(mapdata.at(k)->mode2 !=3000))
+                {
+                 continue;
+                }
+                mapdata.at(k)->mode2 = 3000;
+            }
+         }
+        if(data->mode2 == 1000001)
+        {
+            int k;
+            for(k=(mapdata.size()-1);k>(mapdata.size()-10);k--)
+            {
+                if(k<0)break;
+                if((mapdata.at(k)->mode2 != -1)&&(mapdata.at(k)->mode2 !=1000001))
+                {
+                 continue;
+                }
+                mapdata.at(k)->mode2 = 1000001;
+            }
+         }
+
+       pmappoint->set_mode2(data->mode2);
+       pmappoint->set_rel_x(xPlan[i].x);
+       pmappoint->set_rel_y(xPlan[i].y);
+       pmappoint->set_rel_z(0);
+       pmappoint->set_rel_yaw(xPlan[i].hdg);
+       pmappoint->set_laneid(-1);
+       pmappoint->set_roadid(xPlan[i].nRoadID);
+
+#ifdef BOAVOID
+    if(isboringroad(xPlan[i].nRoadID))
+    {
+        const int nrangeavoid = 300;
+         if((i+(nrangeavoid + 10))<nSize)
+         {
+             double fhdg1 = xPlan[i].hdg;
+             bool bavoid = true;
+//             int k;
+//             for(k=0;k<=10;k++)
+//             {
+//                 double fhdg5 = xPlan[i+nrangeavoid*k/10].hdg;
+//                 double fhdgdiff1 = fhdg5 - fhdg1;
+//                 while(fhdgdiff1<0)fhdgdiff1 = fhdgdiff1 + 2.0*M_PI;
+//                 if((fhdgdiff1>(M_PI/3.0))&&(fhdgdiff1<(5.0*M_PI/3.0)))
+//                 {
+//                     bavoid = false;
+//                     break;
+//                 }
+
+//             }
+             if(bavoid)
+             {
+                 data->roadSum = 2;
+                 data->roadOri = 0;
+             }
+
+         }
+         else
+         {
+             int a = 1;
+             a++;
+         }
+    }
+#endif
+
+        mapdata.push_back(data);
+
+
+
+
+//        char strline[255];
+//  //      snprintf(strline,255,"%d\t%11.7f\t%11.7f\t%d\t%11.3f\t%d\t%d\t%d\t%d\t%d\t%d\t%11.7f\n",
+//  //               i,data->gps_lng,data->gps_lat,0,data->ins_heading_angle,0,0,0,data->roadOri,data->roadSum,data->mode2,data->mfCurvature);
+//              snprintf(strline,255,"%d\t%11.7f\t%11.7f\t%d\t%d\t%11.3f\t%d\t%d\t%d\t%d\t%d\n",
+//                      i,data->gps_lng,data->gps_lat,0,0,data->ins_heading_angle,0,0,data->roadSum,data->roadOri,data->mode2);
+//        xfile.write(strline);
+    }
+
+
+    for(int j=0;j<nSize;j++)
+    {
+        char strline[255];
+        snprintf(strline,255,"%d\t%11.7f\t%11.7f\t%d\t%d\t%11.3f\t%d\t%d\t%d\t%d\n",
+                      j,mapdata.at(j)->gps_lng,mapdata.at(j)->gps_lat,0,0,mapdata.at(j)->ins_heading_angle,0,0,mapdata.at(j)->roadSum,mapdata.at(j)->roadOri);
+        xfile.write(strline);
+     //mapttrace1
+        char strline_1[255];
+        snprintf(strline_1,255,"%d\t%11.7f\t%11.7f\t%d\t%d\t%11.3f\t%d\t%d\t%d\t%d\t%d\n",
+                      j,mapdata.at(j)->gps_lng,mapdata.at(j)->gps_lat,0,0,mapdata.at(j)->ins_heading_angle,0,0,mapdata.at(j)->roadSum,mapdata.at(j)->roadOri,mapdata.at(j)->mode2);
+        xfile_1.write(strline_1);
+    }
+
+    xfile.close();
+    xfile_1.close();
+    ShareMap(mapdata);
+
+    int nnewmapsize = xtrace.ByteSize();
+    std::shared_ptr<char> str_ptr = std::shared_ptr<char>(new char[nnewmapsize]);
+    if(xtrace.SerializeToArray(str_ptr.get(),nnewmapsize))
+    {
+        iv::modulecomm::ModuleSendMsg(gpanewtrace ,str_ptr.get(),nnewmapsize);
+    }
+    else
+    {
+        std::cout<<" new trace map serialize fail."<<std::endl;
+    }
+
+
+
+
+    s = 1;
+}
+
+void MultiStationPlan(iv::v2x::v2x * pxv2x,double fsrclat,double fsrclon,double fsrcheading,int nlane)
+{
+
+    std::vector<PlanPoint> xPlan;
+
+    int i;
+
+    double fLastHdg = 0;
+
+    int ndeflane =nlane;
+
+    for(i=0;i<pxv2x->stgps_size();i++)
+    {
+
+        double x_src,y_src,x_dst,y_dst;
+
+        if(i==0)
+        {
+            CalcXY(glat0,glon0,ghead0,fsrclat,fsrclon,x_src,y_src);
+        }
+        else
+        {
+            CalcXY(glat0,glon0,ghead0,pxv2x->stgps(i-1).lat(),pxv2x->stgps(i-1).lon(),x_src,y_src);
+        }
+        CalcXY(glat0,glon0,ghead0,pxv2x->stgps(i).lat(),pxv2x->stgps(i).lon(),x_dst,y_dst);
+
+
+        std::vector<PlanPoint> xPlanPart;
+
+        double s;
+
+        //    x_src = -5;y_src = 6;
+        //    x_dst = -60;y_src = -150;
+        int nRtn = -1;
+        if(i==0)
+        {
+            nRtn = MakePlan(gpxd,&mxodr,x_src,y_src,(90 - fsrcheading)*M_PI/180.0,x_dst,y_dst,s,30.0,100.0,ndeflane,xPlanPart);
+        }
+        else
+        {
+            nRtn = MakePlan(gpxd,&mxodr,x_src,y_src,fLastHdg,x_dst,y_dst,s,30.0,100.0,ndeflane,xPlanPart);
+        }
+
+
+
+        if(nRtn < 0)
+        {
+            qDebug("plan fail.");
+            return;
+        }
+
+        int j;
+        for(j=0;j<xPlanPart.size();j++)xPlan.push_back(xPlanPart.at(j));
+        fLastHdg = xPlanPart.at(xPlanPart.size()-1).hdg;
+
+    }
+
+
+
+    int nSize = xPlan.size();
+
+    std::vector<iv::GPSData> mapdata;
+
+    QFile xfile;
+    QString strpath;
+    strpath = getenv("HOME");
+    strpath = strpath + "/map/maptrace.txt";
+    xfile.setFileName(strpath);
+    xfile.open(QIODevice::ReadWrite);
+
+    for(i=0;i<nSize;i++)
+    {
+        iv::GPSData data(new iv::GPS_INS);
+        CalcLatLon(glat0,glon0,ghead0,xPlan[i].x,xPlan[i].y,xPlan[i].hdg,data->gps_lat,
+                   data->gps_lng,data->ins_heading_angle);
+        data->index = i;
+        data->speed = xPlan[i].speed;
+ //       ZBGaussProjCal(data->gps_lng,data->gps_lat,&data->gps_x,&data->gps_y);
+        GaussProjCal(data->gps_lng,data->gps_lat,&data->gps_x,&data->gps_y);
+
+        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;
+
+//        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);
+
+        char strline[255];
+        snprintf(strline,255,"%d\t%11.7f\t%11.7f\t%d\t%d\t%11.3f\t%d\t%d\t%d\t%d\n",
+                 i,data->gps_lng,data->gps_lat,0,data->ins_heading_angle,0,0,0,0,0);
+        xfile.write(strline);
+
+    }
+
+    xfile.close();
+
+    ShareMap(mapdata);
+
+}
+
+void ListenCmd(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
+{
+
+    if(nSize<sizeof(xodrobj))
+    {
+        std::cout<<"ListenCmd small."<<std::endl;
+        return;
+    }
+
+    xodrobj xo;
+    memcpy(&xo,strdata,sizeof(xodrobj));
+
+    givlog->debug("lat is %f", xo.flat);
+
+    xo.fhgdsrc = gsrc.fhgdsrc;
+    xo.flatsrc = gsrc.flatsrc;
+    xo.flonsrc = gsrc.flonsrc;
+
+    SetPlan(xo);
+
+
+}
+
+void ListenV2X(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
+{
+    iv::v2x::v2x xv2x;
+    if(!xv2x.ParseFromArray(strdata,nSize))
+    {
+        givlog->warn("ListernV2X Parse Error.");
+        std::cout<<"ListenV2X Parse Error."<<std::endl;
+        return;
+    }
+    if(xv2x.stgps_size()<1)
+    {
+        givlog->debug("ListenV2X no gps station.");
+        std::cout<<"ListenV2X no gps station."<<std::endl;
+        return;
+    }
+
+    MultiStationPlan(&xv2x,gsrc.flatsrc,gsrc.flonsrc,gsrc.fhgdsrc,1);
+}
+
+void ListenSrc(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
+{
+
+    if(nSize<sizeof(xodrobj))
+    {
+        givlog->warn("ListenSrc small");
+        std::cout<<"ListenSrc small."<<std::endl;
+        return;
+    }
+
+    memcpy(&gsrc,strdata,sizeof(xodrobj));
+
+    givlog->debug("src hdg is %f", gsrc.fhgdsrc);
+    std::cout<<"src hdg is "<<gsrc.fhgdsrc<<std::endl;
+
+}
+
+void UpdateGPSIMU(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
+{
+
+    iv::gps::gpsimu  xgpsimu;
+    if(!xgpsimu.ParseFromArray(strdata,nSize))
+    {
+        givlog->warn("ADCIntelligentVehicle::UpdateGPSIMU parse error. nSize is %d",nSize);
+        return;
+    }
+
+    xodrobj xo;
+    xo.fhgdsrc = xgpsimu.heading();
+    xo.flatsrc = xgpsimu.lat();
+    xo.flonsrc = xgpsimu.lon();
+
+    gsrc = xo;
+
+    givlog->debug("src hdg is %f", gsrc.fhgdsrc);
+    std::cout<<"src hdg is "<<gsrc.fhgdsrc<<std::endl;
+}
+
+void ExitFunc()
+{
+//    gApp->quit();
+    iv::modulecomm::Unregister(gpasimple);
+    iv::modulecomm::Unregister(gpav2x);
+    iv::modulecomm::Unregister(gpagps);
+    iv::modulecomm::Unregister(gpasrc);
+    iv::modulecomm::Unregister(gpmap);
+    iv::modulecomm::Unregister(gpa);
+    std::cout<<"driver_map_xodrload exit."<<std::endl;
+    gApp->quit();
+    std::this_thread::sleep_for(std::chrono::milliseconds(100));
+//    std::this_thread::sleep_for(std::chrono::milliseconds(900));
+}
+
+void signal_handler(int sig)
+{
+    if(sig == SIGINT)
+    {
+        ExitFunc();
+    }
+}
+
+void ProcROIReq(std::shared_ptr<char> pstr_req,const int nreqsize,std::shared_ptr<char> & pstr_res,int & nressize)
+{
+    (void)pstr_req;
+    (void)nreqsize;
+    std::shared_ptr<iv::roi::request> pstr_roireq = std::shared_ptr<iv::roi::request>(new iv::roi::request);
+    if(pstr_roireq->ParseFromArray(pstr_req.get(),nreqsize))
+    {
+        std::shared_ptr<iv::roi::resultarray> pstr_roires;
+        gsrx.GetROI(pstr_roireq,pstr_roires);
+        int nbytesize = pstr_roires->ByteSize();
+        pstr_res = std::shared_ptr<char>(new char[nbytesize]);
+        if(pstr_roires->SerializeToArray(pstr_res.get(),nbytesize))
+        {
+            nressize = nbytesize;
+            std::cout<<"return res."<<std::endl;
+            return;
+        }
+        else
+        {
+            std::cout<<" ProcROIReq fail serizlizetoarray"<<std::endl;
+        }
+    }
+    else
+    {
+        std::cout<<" ProcROIReq parse pstr_req fail."<<std::endl;
+        return;
+    }
+}
+
+int main(int argc, char *argv[])
+{
+    showversion("driver_map_xodrload");
+    QCoreApplication a(argc, argv);
+    gApp = &a;
+
+    RegisterIVBackTrace();
+
+    gfault = new iv::Ivfault("driver_map_xodrload");
+    givlog = new iv::Ivlog("driver_map_xodrload");
+
+    std::string strmapth,strparapath;
+    if(argc<3)
+    {
+//        strmapth = "./map.xodr";
+        strmapth = getenv("HOME");
+        strmapth = strmapth + "/map/map.xodr";
+//        strmapth = "/home/yuchuli/1226.xodr";
+        strparapath = "./driver_map_xodrload.xml";
+    }
+    else
+    {
+        strmapth = argv[1];
+        strparapath = argv[2];
+    }
+
+
+    iv::xmlparam::Xmlparam xp(strparapath);
+    xp.GetParam(std::string("he"),std::string("1"));
+    std::string strlat0 = xp.GetParam("lat0","39");
+    std::string strlon0 = xp.GetParam("lon0","117.0");
+    std::string strhdg0 = xp.GetParam("hdg0","360.0");
+
+    std::string strgpsmsg = xp.GetParam("gpsmsg","hcp2_gpsimu");
+
+    std::string strv2xmsg = xp.GetParam("v2xmsg","v2x");
+
+    std::string strvehiclewidth = xp.GetParam("vehiclewidth","2.0");
+
+    std::string strextendmap = xp.GetParam("extendmap","false");
+
+    std::string strsideenable = xp.GetParam("sideenable","false");
+
+    std::string strsidelaneenable = xp.GetParam("sidelaneenable","false");
+
+
+    glat0 = atof(strlat0.data());
+    glon0 = atof(strlon0.data());
+    ghead0 = atof(strhdg0.data());
+
+    gvehiclewidth = atof(strvehiclewidth.data());
+
+    if(strextendmap == "true")
+    {
+        gbExtendMap = true;
+    }
+    else
+    {
+        gbExtendMap = false;
+    }
+
+    if(strsideenable == "true")
+    {
+        gbSideEnable = true;
+    }
+
+    if(strsidelaneenable == "true")
+    {
+        gbSideLaneEnable = true;
+    }
+
+
+    LoadXODR(strmapth);
+
+    iv::service::Server serviceroi("ServiceROI",ProcROIReq);
+    (void)serviceroi;
+
+    gpmap = iv::modulecomm::RegisterSend("tracemap",20000000,1);
+    gpasimple = iv::modulecomm::RegisterSend("simpletrace",100000,1);
+    gpanewtrace = iv::modulecomm::RegisterSend("newtracemap",20000000,1);
+
+
+    gpa = iv::modulecomm::RegisterRecv("xodrreq",ListenCmd);
+    gpasrc =iv::modulecomm::RegisterRecv("xodrsrc",ListenSrc);
+    gpagps = iv::modulecomm::RegisterRecv(strgpsmsg.data(),UpdateGPSIMU);
+    gpav2x = iv::modulecomm::RegisterRecv(strv2xmsg.data(),ListenV2X);
+
+
+
+
+    double x_src,y_src,x_dst,y_dst;
+
+    x_src = -26;y_src = 10;
+    x_dst = -50;y_dst = -220;
+
+    x_src = 0;y_src = 0;
+    x_dst = -23;y_dst = -18;
+    x_dst = 21;y_dst =-21;
+    x_dst =5;y_dst = 0;
+
+    x_src = -20; y_src = -1000;
+    x_dst = 900; y_dst = -630;
+
+//    x_dst = 450; y_dst = -640;
+//    x_dst = -190; y_dst = -690;
+
+//    x_src = 900; y_src = -610;
+//    x_dst = -100; y_dst = -680;
+    std::vector<PlanPoint> xPlan;
+    double s;
+//    int nRtn = MakePlan(gpxd,&mxodr,x_src,y_src,3.14,x_dst,y_dst,s,30.0,100.0,1,xPlan);
+
+//    ToGPSTrace(xPlan);
+
+
+//    double lat = 39.1443880;
+//    double lon = 117.0812543;
+
+//    xodrobj xo;
+//    xo.fhgdsrc = 340;
+//    xo.flatsrc = lat; xo.flonsrc = lon;
+//    xo.flat = 39.1490196;
+//    xo.flon = 117.0806979;
+//    xo.lane = 1;
+//    SetPlan(xo);
+
+
+    void * pivexit = iv::ivexit::RegIVExitCall(ExitFunc);
+
+    signal(SIGINT,signal_handler);
+
+    int nrc = a.exec();
+
+    std::cout<<"driver_map_xodr app exit. code :"<<nrc<<std::endl;
+
+    return nrc;
+}

src/driver/driver_map_xodrload_new/mconf.h

@@ -0,0 +1,199 @@
+/*							mconf.h
+ *
+ *	Common include file for math routines
+ *
+ *
+ *
+ * SYNOPSIS:
+ *
+ * #include "mconf.h"
+ *
+ *
+ *
+ * DESCRIPTION:
+ *
+ * This file contains definitions for error codes that are
+ * passed to the common error handling routine mtherr()
+ * (which see).
+ *
+ * The file also includes a conditional assembly definition
+ * for the type of computer arithmetic (IEEE, DEC, Motorola
+ * IEEE, or UNKnown).
+ * 
+ * For Digital Equipment PDP-11 and VAX computers, certain
+ * IBM systems, and others that use numbers with a 56-bit
+ * significand, the symbol DEC should be defined.  In this
+ * mode, most floating point constants are given as arrays
+ * of octal integers to eliminate decimal to binary conversion
+ * errors that might be introduced by the compiler.
+ *
+ * For little-endian computers, such as IBM PC, that follow the
+ * IEEE Standard for Binary Floating Point Arithmetic (ANSI/IEEE
+ * Std 754-1985), the symbol IBMPC should be defined.  These
+ * numbers have 53-bit significands.  In this mode, constants
+ * are provided as arrays of hexadecimal 16 bit integers.
+ *
+ * Big-endian IEEE format is denoted MIEEE.  On some RISC
+ * systems such as Sun SPARC, double precision constants
+ * must be stored on 8-byte address boundaries.  Since integer
+ * arrays may be aligned differently, the MIEEE configuration
+ * may fail on such machines.
+ *
+ * To accommodate other types of computer arithmetic, all
+ * constants are also provided in a normal decimal radix
+ * which one can hope are correctly converted to a suitable
+ * format by the available C language compiler.  To invoke
+ * this mode, define the symbol UNK.
+ *
+ * An important difference among these modes is a predefined
+ * set of machine arithmetic constants for each.  The numbers
+ * MACHEP (the machine roundoff error), MAXNUM (largest number
+ * represented), and several other parameters are preset by
+ * the configuration symbol.  Check the file const.c to
+ * ensure that these values are correct for your computer.
+ *
+ * Configurations NANS, INFINITIES, MINUSZERO, and DENORMAL
+ * may fail on many systems.  Verify that they are supposed
+ * to work on your computer.
+ */
+/*
+Cephes Math Library Release 2.3:  June, 1995
+Copyright 1984, 1987, 1989, 1995 by Stephen L. Moshier
+*/
+
+
+/* Define if the `long double' type works.  */
+#define HAVE_LONG_DOUBLE 1
+
+/* Define as the return type of signal handlers (int or void).  */
+#define RETSIGTYPE void
+
+/* Define if you have the ANSI C header files.  */
+#define STDC_HEADERS 1
+
+/* Define if your processor stores words with the most significant
+   byte first (like Motorola and SPARC, unlike Intel and VAX).  */
+/* #undef WORDS_BIGENDIAN */
+
+/* Define if floating point words are bigendian.  */
+/* #undef FLOAT_WORDS_BIGENDIAN */
+
+/* The number of bytes in a int.  */
+#define SIZEOF_INT 4
+
+/* Define if you have the <string.h> header file.  */
+#define HAVE_STRING_H 1
+
+/* Name of package */
+#define PACKAGE "cephes"
+
+/* Version number of package */
+#define VERSION "2.7"
+
+/* Constant definitions for math error conditions
+ */
+
+//#define DOMAIN		1	/* argument domain error */
+//#define SING		2	/* argument singularity */
+//#define OVERFLOW	3	/* overflow range error */
+//#define UNDERFLOW	4	/* underflow range error */
+//#define TLOSS		5	/* total loss of precision */
+//#define PLOSS		6	/* partial loss of precision */
+
+#define EDOM		33
+#define ERANGE		34
+/* Complex numeral.  */
+typedef struct
+	{
+	double r;
+	double i;
+	} cmplx;
+
+#ifdef HAVE_LONG_DOUBLE
+/* Long double complex numeral.  */
+typedef struct
+	{
+	long double r;
+	long double i;
+	} cmplxl;
+#endif
+
+
+/* Type of computer arithmetic */
+
+/* PDP-11, Pro350, VAX:
+ */
+/* #define DEC 1 */
+
+/* Intel IEEE, low order words come first:
+ */
+/* #define IBMPC 1 */
+
+/* Motorola IEEE, high order words come first
+ * (Sun 680x0 workstation):
+ */
+/* #define MIEEE 1 */
+
+/* UNKnown arithmetic, invokes coefficients given in
+ * normal decimal format.  Beware of range boundary
+ * problems (MACHEP, MAXLOG, etc. in const.c) and
+ * roundoff problems in pow.c:
+ * (Sun SPARCstation)
+ */
+#define UNK 1
+
+/* If you define UNK, then be sure to set BIGENDIAN properly. */
+#ifdef FLOAT_WORDS_BIGENDIAN
+#define BIGENDIAN 1
+#else
+#define BIGENDIAN 0
+#endif
+/* Define this `volatile' if your compiler thinks
+ * that floating point arithmetic obeys the associative
+ * and distributive laws.  It will defeat some optimizations
+ * (but probably not enough of them).
+ *
+ * #define VOLATILE volatile
+ */
+#define VOLATILE
+
+/* For 12-byte long doubles on an i386, pad a 16-bit short 0
+ * to the end of real constants initialized by integer arrays.
+ *
+ * #define XPD 0,
+ *
+ * Otherwise, the type is 10 bytes long and XPD should be
+ * defined blank (e.g., Microsoft C).
+ *
+ * #define XPD
+ */
+#define XPD 0,
+
+/* Define to support tiny denormal numbers, else undefine. */
+#define DENORMAL 1
+
+/* Define to ask for infinity support, else undefine. */
+#define INFINITIES 1
+
+/* Define to ask for support of numbers that are Not-a-Number,
+   else undefine.  This may automatically define INFINITIES in some files. */
+#define NANS 1
+
+/* Define to distinguish between -0.0 and +0.0.  */
+#define MINUSZERO 1
+
+/* Define 1 for ANSI C atan2() function
+   See atan.c and clog.c. */
+#define ANSIC 1
+
+/* Get ANSI function prototypes, if you want them. */
+#if 1
+/* #ifdef __STDC__ */
+#define ANSIPROT 1
+int mtherr ( char *, int );
+#else
+int mtherr();
+#endif
+
+/* Variable for error reporting.  See mtherr.c.  */
+extern int merror;

src/driver/driver_map_xodrload_new/planpoint.h

@@ -0,0 +1,41 @@
+#ifndef PLANPOINT_H
+#define PLANPOINT_H
+
+class PlanPoint
+{
+public:
+    bool bInlaneAvoid = false;
+    double mx_left;
+    double my_left;
+    double mx_right;
+    double my_right;
+    double x;
+    double y;
+    double speed;
+    int lanmp; //left 1 right -1
+    double hdg;
+    double dis;
+    double mS;
+    double mWidth;  //Current Lane Width
+    double mLeftWidth[5];
+    double mRightWidth[5];
+
+    double mfRoadWidth;
+    double mfDisToRoadLeft;
+    double mfDisToLaneLeft;
+    int mnLaneori = 0;  //from Right 0
+    int mnLaneTotal = 1; //Lane Total
+
+    int nSignal = -1;   //if 0 no signal point
+
+    int nRoadID =-1;
+
+    double mfSecx;
+    double mfSecy;
+    int nlrchange; //1 left 2 right
+    bool mbBoringRoad = false;
+    bool mbNoavoid = false;
+    double mfCurvature = 0.0;
+};
+
+#endif // PLANPOINT_H

src/driver/driver_map_xodrload_new/polevl.c

@@ -0,0 +1,97 @@
+/*							polevl.c
+ *							p1evl.c
+ *
+ *	Evaluate polynomial
+ *
+ *
+ *
+ * SYNOPSIS:
+ *
+ * int N;
+ * double x, y, coef[N+1], polevl[];
+ *
+ * y = polevl( x, coef, N );
+ *
+ *
+ *
+ * DESCRIPTION:
+ *
+ * Evaluates polynomial of degree N:
+ *
+ *                     2          N
+ * y  =  C  + C x + C x  +...+ C x
+ *        0    1     2          N
+ *
+ * Coefficients are stored in reverse order:
+ *
+ * coef[0] = C  , ..., coef[N] = C  .
+ *            N                   0
+ *
+ *  The function p1evl() assumes that coef[N] = 1.0 and is
+ * omitted from the array.  Its calling arguments are
+ * otherwise the same as polevl().
+ *
+ *
+ * SPEED:
+ *
+ * In the interest of speed, there are no checks for out
+ * of bounds arithmetic.  This routine is used by most of
+ * the functions in the library.  Depending on available
+ * equipment features, the user may wish to rewrite the
+ * program in microcode or assembly language.
+ *
+ */
+
+
+/*
+Cephes Math Library Release 2.1:  December, 1988
+Copyright 1984, 1987, 1988 by Stephen L. Moshier
+Direct inquiries to 30 Frost Street, Cambridge, MA 02140
+*/
+
+
+double polevl( double x,  double *coef, int N )
+//double x;
+//double coef[];
+//int N;
+{
+double ans;
+int i;
+double *p;
+
+p = coef;
+ans = *p++;
+i = N;
+
+do
+	ans = ans * x  +  *p++;
+while( --i );
+
+return( ans );
+}
+
+/*							p1evl()	*/
+/*                                          N
+ * Evaluate polynomial when coefficient of x  is 1.0.
+ * Otherwise same as polevl.
+ */
+
+double p1evl( double x, double *coef, int N )
+//double x;
+//double coef[];
+//int N;
+{
+double ans;
+double *p;
+int i;
+
+p = coef;
+ans = x + *p++;
+i = N-1;
+
+do
+	ans = ans * x  + *p++;
+while( --i );
+
+return( ans );
+}

src/driver/driver_map_xodrload_new/readme.md

@@ -0,0 +1,3 @@
+主线上的工程直接复制到分支上来了
+
+余工修改了driver_map_xodrload, 增加了起点终点模糊路段智能选择的算法， 增加了终点靠边停车的算法（需要设置sidelaneenable为true）。

src/driver/driver_map_xodrload_new/service_roi_xodr.cpp

@@ -0,0 +1,342 @@
+#include "service_roi_xodr.h"
+
+#include <iostream>
+#include "gnss_coordinate_convert.h"
+
+service_roi_xodr::service_roi_xodr()
+{
+
+}
+
+void service_roi_xodr::SetXODR(OpenDrive *pxodr)
+{
+    mpxodr = pxodr;
+    mpxodr->GetHeader()->GetLat0Lon0(mlat0,mlon0);
+    updateroadarea();
+}
+
+
+#include <QTime>
+int service_roi_xodr::GetROI(std::shared_ptr<iv::roi::request> xreq_ptr, std::shared_ptr<iv::roi::resultarray> &xres_ptr)
+{
+
+    xres_ptr = std::shared_ptr<iv::roi::resultarray>(new iv::roi::resultarray);
+    xres_ptr->set_lat(xreq_ptr->lat());
+    xres_ptr->set_lon(xreq_ptr->lon());
+    xres_ptr->set_heading(xreq_ptr->heading());
+    xres_ptr->set_range(xreq_ptr->range());
+    xres_ptr->set_gridsize(xreq_ptr->gridsize());
+
+    double x0,y0;
+    double x,y;
+    GaussProjCal(xreq_ptr->lon(),xreq_ptr->lat(),&x,&y);
+    GaussProjCal(mlon0,mlat0,&x0,&y0);
+    x = x - x0;
+    y = y - y0;
+    unsigned int nroadsize = mvectorarea.size();
+    unsigned int i;
+    unsigned int j;
+    std::vector<iv::roi::area> xvectorarea;
+
+    QTime xTime;
+    xTime.start();
+
+    //Find Near ROI area
+    for(i=0;i<nroadsize;i++)
+    {
+        double fdis1;
+        iv::roi::roadarea * proadarea = &mvectorarea[i];
+        fdis1 = sqrt(pow(proadarea->startPoint.x - x,2)+pow(proadarea->startPoint.y - y,2));
+        if(fdis1 > (xreq_ptr->range() + proadarea->fRoadLen + 30.0))  //+30.0 road width.
+        {
+            continue;
+        }
+        unsigned int nareasize = proadarea->mvectorarea.size();
+
+        for(j=0;j<nareasize;j++)
+        {
+            iv::roi::area * parea = &proadarea->mvectorarea[j];
+            double fd1,fd2,fd3,fd4;
+            fd1 = sqrt(pow(x - parea->P1.x,2)+pow(y - parea->P1.y,2));
+            if(fd1<xreq_ptr->range())
+            {
+                xvectorarea.push_back(*parea);
+                continue;
+            }
+            fd2 = sqrt(pow(x - parea->P2.x,2)+pow(y - parea->P2.y,2));
+            if(fd2<xreq_ptr->range())
+            {
+                xvectorarea.push_back(*parea);
+                continue;
+            }
+            fd3 = sqrt(pow(x - parea->P3.x,2)+pow(y - parea->P3.y,2));
+            if(fd3<xreq_ptr->range())
+            {
+                xvectorarea.push_back(*parea);
+                continue;
+            }
+            fd4 = sqrt(pow(x - parea->P4.x,2)+pow(y - parea->P4.y,2));
+            if(fd4<xreq_ptr->range())
+            {
+                xvectorarea.push_back(*parea);
+            }
+        }
+    }
+
+    std::cout<<"xTime: "<<xTime.elapsed()<<std::endl;
+
+    if(xreq_ptr->gridsize() <= 0.0)
+    {
+        return -1;  //grid size is error
+    }
+
+    double fgridsize = xreq_ptr->gridsize()/2.0;
+
+    std::vector<iv::roi::Point> xvectorpoint;
+    unsigned int nareasize = xvectorarea.size();
+    double fgpsyaw = (90-xreq_ptr->heading())*M_PI/180.0 -3.0*M_PI/2.0;
+    for(i=0;i<nareasize;i++)
+    {
+        int ix,iy;
+        int ny = xvectorarea[i].fsteplen/fgridsize;
+        int nxl = 0;
+        int nxr = 0;
+        if(xvectorarea[i].fleftwidth>0)nxl = xvectorarea[i].fleftwidth/fgridsize;
+        if(xvectorarea[i].frightwidth>0)nxr = xvectorarea[i].frightwidth/fgridsize;
+        double fyaw = xvectorarea[i].fhdg - fgpsyaw;
+ //       double fyaw = xvectorarea[i].fhdg - fgpsyaw;
+        double flaneoff = xvectorarea[i].flaneoff;
+        if(fyaw < 0)fyaw = fyaw + 2.0*M_PI;
+        double frelxraw = xvectorarea[i].refPoint.x - x;
+        double frelyraw = xvectorarea[i].refPoint.y - y;
+        double fxrel,fyrel;
+//        fgpsyaw = 0;
+        fxrel = frelxraw * cos(-fgpsyaw) - frelyraw * sin(-fgpsyaw);
+        fyrel = frelxraw * sin(-fgpsyaw) + frelyraw * cos(-fgpsyaw);
+        double fxbase = fxrel + flaneoff * cos(fyaw + M_PI/2.0);
+        double fybase = fyrel + flaneoff * sin(fyaw + M_PI/2.0);
+//        iv::roi::Point xpoint;
+//        xpoint.x = fxbase;
+//        xpoint.y = fybase;
+//        xvectorpoint.push_back(xpoint);
+//        continue;
+
+        fyaw = fyaw + 3.0*M_PI/2.0;
+        if((nxl == 0)&&(nxr== 0)&&(ny <= 1))
+        {
+            iv::roi::Point xpoint;
+            xpoint.x = fxbase;
+            xpoint.y = fybase;
+            xvectorpoint.push_back(xpoint);
+        }
+        else
+        {
+            if((nxl == 0)&&(nxr== 0))
+            {
+                for(iy=(-ny);iy<=0;iy++)
+                {
+                    iv::roi::Point xpoint;
+                    xpoint.x = fxbase +(iy*fgridsize)*cos(fyaw);
+                    xpoint.y = fybase +(iy*fgridsize)*sin(fyaw);
+                    xvectorpoint.push_back(xpoint);
+                }
+            }
+            else
+            {
+                for(ix=(-nxl);ix<=nxr;ix++)
+                {
+                    for(iy=(-ny);iy<=0;iy++)
+                    {
+                        double fxlocal,fylocal;
+                        fxlocal = ix*fgridsize;
+                        fylocal = iy*fgridsize;
+                        iv::roi::Point xpoint;
+
+                        xpoint.x = fxbase +fxlocal*cos(fyaw) - fylocal * sin(fyaw);
+                        xpoint.y = fybase +fxlocal*sin(fyaw) + fylocal * cos(fyaw);
+                        xvectorpoint.push_back(xpoint);
+                    }
+                }
+            }
+        }
+    }
+
+
+    std::cout<<"xTime: "<<xTime.elapsed()<<" size : "<<xvectorpoint.size()<< std::endl;
+
+    for(i=0;i<xvectorpoint.size();i++)
+    {
+        iv::roi::resultunit * padd = xres_ptr->add_res();
+//        std::cout<<" x: "<<xvectorpoint[i].x<<" y: "<<xvectorpoint[i].y<<std::endl;
+        padd->set_x(xvectorpoint[i].x);
+        padd->set_y(xvectorpoint[i].y);
+    }
+
+    return 0;
+}
+
+void service_roi_xodr::updateroadarea()
+{
+    OpenDrive * pxodr = mpxodr;
+    mvectorarea.clear();
+    unsigned int nroadcount = pxodr->GetRoadCount();
+    unsigned int i;
+    double fstep = GEOSAMPLESTEP;
+    for(i=0;i<nroadcount;i++)
+    {
+        Road * pRoad = pxodr->GetRoad(i);
+        if(pRoad == NULL)
+        {
+            std::cout<<" Warning: OpenDrive Road "<<i<<" is NULL"<<std::endl;
+            continue;
+        }
+        iv::roi::roadarea xroadarea = GetRoadArea(pRoad,fstep);
+        mvectorarea.push_back(xroadarea);
+    }
+}
+
+iv::roi::roadarea service_roi_xodr::GetRoadArea(Road *pRoad, const double fstep)
+{
+    iv::roi::roadarea xroadarea;
+    xroadarea.fRoadLen = pRoad->GetRoadLength();
+    double x1,y1,hdg1;
+    pRoad->GetGeometryCoords(0.0,x1,y1,hdg1);
+    xroadarea.startPoint.x = x1;
+    xroadarea.startPoint.y = y1;
+    unsigned int nSectionCount = pRoad->GetLaneSectionCount();
+    unsigned int i;
+    if(nSectionCount == 0)
+    {
+        std::cout<<"Warning. Road"<<pRoad->GetRoadId()<<" not have lanesection."<<std::endl;
+    }
+    double snow= 0;
+    for(i=0;i<nSectionCount;i++)
+    {
+        LaneSection * pLS = pRoad->GetLaneSection(i);
+        if(pLS == NULL)
+        {
+            std::cout<<"Warning. LaneSection is NULL."<<std::endl;
+            continue;
+        }
+        snow = pLS->GetS();
+        double endS;
+        if(i == (nSectionCount -1))
+        {
+            endS = pRoad->GetRoadLength();
+        }
+        else
+        {
+            endS = pRoad->GetLaneSection(i+1)->GetS();
+        }
+
+        double fleftwidth = pRoad->GetRoadLeftWidth(snow);
+        double frigthwidth = pRoad->GetRoadRightWidth(snow);
+        double x,y,yaw;
+        pRoad->GetGeometryCoords(snow,x,y,yaw);
+        iv::roi::Point pointLeft,pointRight,pointLeft_old,pointRight_old;
+        double flaneoff = pRoad->GetLaneOffsetValue(snow);
+        pointLeft.x = x + (fleftwidth + flaneoff) * cos(yaw + M_PI/2.0);
+        pointLeft.y = y + (fleftwidth + flaneoff) * sin(yaw + M_PI/2.0);
+        pointRight.x = x + (frigthwidth - flaneoff ) * cos(yaw - M_PI/2.0);
+        pointRight.y = y + (frigthwidth - flaneoff) * sin(yaw -M_PI/2.0);
+        pointLeft_old = pointLeft;
+        pointRight_old = pointRight;
+        snow = snow + fstep;
+        while(snow < endS)
+        {
+            fleftwidth = pRoad->GetRoadLeftWidth(snow);
+            frigthwidth = pRoad->GetRoadRightWidth(snow);
+            pRoad->GetGeometryCoords(snow,x,y,yaw);
+            flaneoff = pRoad->GetLaneOffsetValue(snow);
+            pointLeft.x = x + (fleftwidth + flaneoff) * cos(yaw + M_PI/2.0);
+            pointLeft.y = y + (fleftwidth + flaneoff) * sin(yaw + M_PI/2.0);
+            pointRight.x = x + (frigthwidth - flaneoff ) * cos(yaw - M_PI/2.0);
+            pointRight.y = y + (frigthwidth - flaneoff) * sin(yaw -M_PI/2.0);
+            iv::roi::area xarea;
+            xarea.P1 = pointLeft_old;
+            xarea.P2 = pointLeft;
+            xarea.P3 = pointRight;
+            xarea.P4 = pointRight_old;
+            xarea.refPoint.x = x;
+            xarea.refPoint.y = y;
+            xarea.s = snow;
+            xarea.fmaxlen = fleftwidth + frigthwidth;
+            xarea.fhdg = yaw;
+            xarea.fleftwidth = fleftwidth;
+            xarea.frightwidth = frigthwidth;
+            xarea.flaneoff = flaneoff;
+            xarea.fsteplen = fstep;
+            xroadarea.mvectorarea.push_back(xarea);
+//            double fleftwidth = pRoad->GetRoadLeftWidth(snow);
+//            double frigthwidth = pRoad->GetRoadRightWidth(snow);
+//            std::cout<<" road : "<<pRoad->GetRoadId()<<" s: "<<snow<<" left: "<<fleftwidth<<" right:"<<frigthwidth<<std::endl;
+
+            pointLeft_old = pointLeft;
+            pointRight_old = pointRight;
+            snow = snow + fstep;
+
+        }
+        if((snow-fstep) < (endS - 0.000001))
+        {
+            double foldsnow = snow - fstep;
+            snow = endS - 0.000001;
+            fleftwidth = pRoad->GetRoadLeftWidth(snow);
+            frigthwidth = pRoad->GetRoadRightWidth(snow);
+            pRoad->GetGeometryCoords(snow,x,y,yaw);
+            flaneoff = pRoad->GetLaneOffsetValue(snow);
+            pointLeft.x = x + (fleftwidth + flaneoff) * cos(yaw + M_PI/2.0);
+            pointLeft.y = y + (fleftwidth + flaneoff) * sin(yaw + M_PI/2.0);
+            pointRight.x = x + (frigthwidth - flaneoff ) * cos(yaw - M_PI/2.0);
+            pointRight.y = y + (frigthwidth - flaneoff) * sin(yaw -M_PI/2.0);
+            iv::roi::area xarea;
+            xarea.P1 = pointLeft_old;
+            xarea.P2 = pointLeft;
+            xarea.P3 = pointRight;
+            xarea.P4 = pointRight_old;
+            xarea.refPoint.x = x;
+            xarea.refPoint.y = y;
+            xarea.s = snow;
+            xarea.fhdg = yaw;
+            xarea.fleftwidth = fleftwidth;
+            xarea.frightwidth = frigthwidth;
+            xarea.flaneoff = flaneoff;
+            xarea.fsteplen = endS - foldsnow;
+            xroadarea.mvectorarea.push_back(xarea);
+        }
+    }
+    return xroadarea;
+}
+
+bool service_roi_xodr::CheckPointInArea(double x, double y, iv::roi::area &xarea)
+{
+    double a = (xarea.P2.x - xarea.P1.x)*(y - xarea.P1.y) - (xarea.P2.y-xarea.P1.y)*(x-xarea.P1.x);
+    double b = (xarea.P3.x - xarea.P2.x)*(y - xarea.P2.y) - (xarea.P3.y-xarea.P2.y)*(x-xarea.P2.x);
+    double c = (xarea.P4.x - xarea.P3.x)*(y - xarea.P3.y) - (xarea.P4.y-xarea.P3.y)*(x-xarea.P3.x);
+    double d = (xarea.P1.x - xarea.P4.x)*(y - xarea.P4.y) - (xarea.P1.y-xarea.P4.y)*(x-xarea.P4.x);
+
+    if((a > 0 && b > 0 && c > 0 && d > 0) || (a < 0 && b < 0 && c < 0 && d < 0)) {
+        return true;
+    }
+    return false;
+}
+
+bool service_roi_xodr::CheckPointInROI(double x, double y, std::vector<iv::roi::area> &xvectorroi)
+{
+    unsigned int i;
+    unsigned nsize = xvectorroi.size();
+    for(i=0;i<nsize;i++)
+    {
+        iv::roi::area xarea = xvectorroi[i];
+        double fdis = sqrt(pow(xarea.refPoint.x-x,2)+pow(xarea.refPoint.y-y,2));
+        if(fdis>(xarea.fmaxlen+0.1))
+        {
+            continue;
+        }
+        if(CheckPointInArea(x,y,xarea))
+        {
+            return true;
+        }
+    }
+    return false;
+}
+

src/driver/driver_map_xodrload_new/service_roi_xodr.h

@@ -0,0 +1,86 @@
+#ifndef SERVICE_ROI_XODR_H
+#define SERVICE_ROI_XODR_H
+
+#include <vector>
+#include <OpenDrive/OpenDrive.h>
+#include "gpsimu.pb.h"
+#include "roireqest.pb.h"
+#include "roiresult.pb.h"
+
+namespace iv {
+
+namespace roi {
+
+struct Point
+{
+    double x;
+    double y;
+};
+
+struct area
+{
+
+    double s;
+    Point refPoint;
+    Point P1;
+    Point P2;
+    Point P3;
+    Point P4;
+    double fhdg;
+    double fleftwidth;
+    double frightwidth;
+    double fsteplen;
+    double flaneoff;
+    double fmaxlen;
+
+};
+
+struct roadarea
+{
+    int nroadid;
+    Point startPoint;
+    double fRoadLen;
+    std::vector<area> mvectorarea;
+};
+}
+}
+
+class service_roi_xodr
+{
+
+
+public:
+    service_roi_xodr();
+
+    /**
+     * @brief SetXODR Set OpenDrive Pointer to Service
+     * @param pxodr
+     */
+    void SetXODR(OpenDrive * pxodr);
+
+    /**
+     * @brief GetROI Get ROI From XODR Map
+     * @param xreq_ptr  now position and roi param
+     * @param xres_prt  result
+     * @return 0 Succeed -1 No Map  -2 Not in Map
+     */
+    int GetROI(std::shared_ptr<iv::roi::request> xreq_ptr,std::shared_ptr<iv::roi::resultarray> & xres_ptr );
+
+private:
+    void updateroadarea();
+    iv::roi::roadarea GetRoadArea(Road * pRoad,const double fstep);
+
+    bool CheckPointInROI(double x,double y, std::vector<iv::roi::area> & xvectorroi);
+
+    bool CheckPointInArea(double x, double y,iv::roi::area & xarea);
+
+private:
+    OpenDrive * mpxodr = NULL;
+    std::vector<iv::roi::roadarea> mvectorarea;
+    double mlon0;
+    double mlat0;
+    const double GEOSAMPLESTEP = 0.5;  // Get Road Unit at 0.5m
+
+};
+
+#endif // SERVICE_ROI_XODR_H

src/driver/driver_map_xodrload_new/xodrplan.cpp

@@ -0,0 +1,15 @@
+#include "xodrplan.h"
+
+xodrplan::xodrplan()
+{
+
+}
+
+int xodrplan::MakePlan(xodrdijkstra *pxd, OpenDrive *pxodr, const double x_now,
+                       const double y_now, const double head, const double x_obj,
+                       const double y_obj, const double &obj_dis, const double srcnearthresh,
+                       const double dstnearthresh, const int nlanesel, std::vector<PlanPoint> &xPlan,
+                       const double fvehiclewidth)
+{
+
+}

src/driver/driver_map_xodrload_new/xodrplan.h

@@ -0,0 +1,26 @@
+#ifndef XODRPLAN_H
+#define XODRPLAN_H
+
+#include "OpenDrive/OpenDrive.h"
+
+#include <vector>
+
+#include "xodrfunc.h"
+
+#include "xodrdijkstra.h"
+
+#include "planpoint.h"
+
+class xodrplan
+{
+public:
+    xodrplan();
+
+public:
+    static int MakePlan(xodrdijkstra * pxd,OpenDrive * pxodr,const double x_now,const double y_now,const double head,
+                 const double x_obj,const double y_obj,const double & obj_dis,
+                 const double srcnearthresh,const double dstnearthresh,
+                 const int nlanesel,std::vector<PlanPoint> & xPlan,const double fvehiclewidth = 2.0);
+};
+
+#endif // XODRPLAN_H