Merge branch 'master' of http://47.96.250.93:3000/adc_pilot/modularization

src/decition/common/common/car_status.h

@@ -52,6 +52,8 @@ namespace iv {
 
         int carState = 0;	// 0:停车	1:正常循迹	2:前往站点
         int emergencyStop=0;
+        int station_control_door=0;//0: door close    1:door open
+        bool station_control_door_option=false;
                 int istostation = 0;
                 //int ctostation = 0;
                 int currentstation = 0;

src/decition/common/common/gps_type.h

@@ -51,7 +51,12 @@ namespace iv {
         int roadSum;
         int roadOri;
 
-        double mfRoadWidth = 3.5; // Current Road Width
+    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
 
 
 

src/decition/common/common_sf/gps_type.h

@@ -51,7 +51,12 @@ namespace iv {
         int roadSum;
         int roadOri;
 
-	double mfRoadWidth = 3.5; // Current Road Width
+    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
 
 
 
@@ -94,6 +99,30 @@ namespace iv {
         }
 
      };
+     class StationCmd
+     {
+     public:
+         bool received;
+         uint32_t carID,carMode,emergencyStop,stationStop;
+         bool has_carID,has_carMode,has_emergencyStop,has_stationStop,mode_manual_drive;
+         uint32_t stationID[20];
+         GPS_INS  stationGps[20];
+         uint32_t stationTotalNum;
+         StationCmd()
+         {
+             received=false;
+             has_carID=false;
+             has_carMode=false;
+             has_emergencyStop=false;
+             has_stationStop=false;
+             mode_manual_drive=false;
+             carID=0;
+             carMode=0;
+             emergencyStop=0;
+             stationStop=0;
+             stationTotalNum=0;
+         }
+     };
 
 
 

src/decition/decition_brain/decition/adc_adapter/hapo_adapter.cpp

@@ -264,9 +264,23 @@ iv::decition::Decition iv::decition::HapoAdapter::getAdapterDeciton(GPS_INS now_
             (*decition)->nearLight=0;
              (*decition)->farLight=0;
 
+     if(ServiceCarStatus.stationCmd.mode_manual_drive==true)
+     {
+          (*decition)->mode=0;       //云平台控制切手动驾驶
+     }
+
+
+     if(ServiceCarStatus.station_control_door==0)
+     {
+         (*decition)->door=3;       //关门
+     }
+     else if((ServiceCarStatus.station_control_door==1)&&(ServiceCarStatus.station_control_door_option==false))
+     {
+         (*decition)->door=2;       //到达站点开门
+         ServiceCarStatus.station_control_door_option=true;
+     }
 
 
-       (*decition)->door=3;
 
 std::cout<<"==========================================="<<std::endl;
      std::cout<<"dSpeed:"<<dSpeed<<"   realSpeed:"<<realSpeed<<"   acc:"<<accAim<<"  torque_st:"<<ServiceCarStatus.torque_st

src/decition/decition_brain/decition/adc_math/coordinate_conversion.h

@@ -0,0 +1,122 @@
+/****************************************
+ * Functions:  Convert coordinates between GPS (longitude, latitude) system and UTM system.
+ * Purpose:    Vehicle localization
+ * Author:     Zhao Liang
+ * Last Updated:  2021/01/20
+ * Update note:  call third-party lib 'proj4' to do the conversionseee.
+*****************************************
+*
+* Convention: East is the X-axis, North is the Y-axis, Up (sky) is the Z-axis,
+* the pair (longitude, latitude) are in degrees: longitude ~ (-180, 180) and
+* latitude ~(-90, 90).
+*
+* Online tool: https://www.latlong.net/lat-long-utm.html
+*
+* Basically the convertion procedure can be splitted into three steps:
+* 1. Declare two `projPJ` objects for the source system and the target system.
+* 2. Put your projection params in two strings.
+* 3. Call `pj_transform` to do the conversion.
+*
+* String used by Apollo:
+*
+* "+proj=longlat +ellps=GRS80 +towgs84=0,0,0,0,0,0,0 +no_defs"
+*
+* 1. +proj=longlat means we are projecting from (longitude, latitude).
+* 2. +ellps=GRS80 means the ellipsoid model is GRS80.
+* 3. +towgs84=0,0,0,0,0,0,0 gives the 7 params for datum transformation.
+* 4. +no_defs means we don't want proj to read the default config file,
+* this option is obsolete since proj 6.x
+*/
+
+#ifndef COORDINATE_CONVERSION_H
+#define COORDINATE_CONVERSION_H
+
+#include <string>
+#include <proj_api.h>
+
+namespace iv {
+namespace math {
+
+/**
+ * @brief: Convert (lon, lat) coordinates to UTM coordinates.
+ * @param: longitude in degrees, range ~ (-180, 180).
+ * @param: latitude in degrees, range ~ (-90, 90).
+ * @param: x coordinate (East direction).
+ * @param: y coordinate (North direction).
+ * @return: Return true if the conversion between the two systems is valid.
+*/
+bool LatLonToUtmXY(double longitude, double latitude, double &x, double &y)
+{
+    projPJ pj_latlon;
+    projPJ pj_utm;
+    int zone = static_cast<int>((longitude + 180) / 6) + 1;
+    std::string latlon_src =
+        "+proj=longlat +ellps=GRS80 +towgs84=0,0,0,0,0,0,0 +no_defs";
+    std::string utm_dst =
+        "+proj=utm +zone=" + std::to_string(zone) + " +ellps=GRS80 +units=m +no_defs";
+
+    if (!(pj_latlon = pj_init_plus(latlon_src.c_str())))
+    {
+        return false;
+    }
+
+    if (!(pj_utm = pj_init_plus(utm_dst.c_str())))
+    {
+        return false;
+    }
+
+    // the pj_transform requires the (lon, lat) are in radians
+    double lon = longitude * DEG_TO_RAD;
+    double lat = latitude * DEG_TO_RAD;
+
+    // do the actual conversion
+    pj_transform(pj_latlon, pj_utm, 1, 1, &lon, &lat, nullptr);
+
+    x = lon;
+    y = lat;
+    pj_free(pj_latlon);
+    pj_free(pj_utm);
+    return true;
+}
+
+/**
+ * @brief: Convert UTM coordinates to (lon, lat) coordinates.
+ * @param: x coordinate (East direction).
+ * @param: y coordinate (North direction).
+ * @param: zone number.
+ * @param: longitude in degrees, range ~ (-180, 180).
+ * @param: latitude in degrees, range ~ (-90, 90).
+ * @return: Return true if the conversion between the two systems is valid.
+*/
+bool UtmXYToLatLon(double x, double y, int zone, double &longitude, double &latitude)
+{
+    projPJ pj_latlon;
+    projPJ pj_utm;
+    std::string latlon_src =
+        "+proj=longlat +ellps=GRS80 +towgs84=0,0,0,0,0,0,0 +no_defs";
+    std::string utm_dst =
+        "+proj=utm +zone=" + std::to_string(zone) + " +ellps=GRS80 +units=m +no_defs";
+
+    if (!(pj_latlon = pj_init_plus(latlon_src.c_str())))
+    {
+        return false;
+    }
+
+    if (!(pj_utm = pj_init_plus(utm_dst.c_str())))
+    {
+        return false;
+    }
+
+    // do the actual conversion
+    pj_transform(pj_utm, pj_latlon, 1, 1, &x, &y, nullptr);
+
+    // the result given by pj_transform are in radians
+    longitude = x * RAD_TO_DEG;
+    latitude = y * RAD_TO_DEG;
+    pj_free(pj_latlon);
+    pj_free(pj_utm);
+    return true;
+}
+}
+}
+#endif // COORDINATE_CONVERSION_H

src/decition/decition_brain/decition/adc_math/vec2.h

@@ -0,0 +1,289 @@
+/****************************************
+* Class:    2D vector class
+* Purpose:  A single header file handling 2D vector computations.
+* Author:   Zhao Liang
+* Last Updated:  2021/01/20
+*****************************************
+*
+* Note: Need to add error handling procedures in divisions.
+*
+*/
+
+#ifndef VEC2_H
+#define VEC2_H
+
+#pragma once
+
+#include <cmath>
+#include <iostream>
+
+namespace iv {
+namespace math {
+
+constexpr double _EPSILON = 1e-12;
+
+class Vec2
+{
+public:
+
+    // ----------------------------------------
+
+    /*
+     * Constructors.
+    */
+
+    Vec2() : x_(0), y_(0) {}
+
+    explicit Vec2(double a) : x_(a), y_(a) {}
+
+    Vec2(double a, double b) : x_(a), y_(b) {}
+
+    // ----------------------------------------
+
+    // Compare to another vector
+    bool operator == (const Vec2 &v) const
+    {
+        return (std::abs(x_ - v.x()) < _EPSILON &&
+                std::abs(y_ - v.y()) < _EPSILON);
+    }
+
+    // ----------------------------------------
+
+    /*
+     * Return the unit vector Vec2(cos(angle), sin(angle)),
+     * The param 'angle' should be in radians.
+    */
+    static Vec2 unitVec2(const double angle)
+    {
+        return Vec2(cos(angle), sin(angle));
+    }
+
+    // ----------------------------------------
+
+    /*
+     * Access x and y components
+    */
+    double x() const { return x_; }
+    double y() const { return y_; }
+
+    // ----------------------------------------
+
+    /*
+     * Set x and y components
+    */
+    void set_x(const double x) { x_ = x; }
+    void set_y(const double y) { y_ = y; }
+
+    // ----------------------------------------
+
+    /*
+     * Operations with other scalars,
+     * vec2 is the left operand, scalar is the right operand.
+    */
+
+    // ----------------------------------------
+
+    // add a scalar
+    Vec2 operator + (double c) const { return Vec2(x() + c, y() + c); }
+    // subtract a scalar
+    Vec2 operator - (double c) const { return Vec2(x() - c, y() - c); }
+    // multiply a scalar
+    Vec2 operator * (double c) const { return Vec2(x() * c, y() * c); }
+    // divide by a scalar
+    Vec2 operator / (double c) const { return Vec2(x() / c, y() / c); }
+    // in-place add a scalar
+    Vec2 operator += (double c) { x_ += c; y_ += c; return (*this); }
+    // in-place subtract a scalar
+    Vec2 operator -= (double c) { x_ -= c; y_ -= c; return (*this); }
+    // in-place multiply a scalar
+    Vec2 operator *= (double c) { x_ *= c; y_ *= c; return (*this); }
+    // in-place divide by a scalar
+    Vec2 operator /= (double c) { x_ /= c; y_ /= c; return (*this); }
+
+    // ----------------------------------------
+
+    /*
+     * Operations with other vec2
+    */
+
+    // ----------------------------------------
+
+    // add two vectors
+    Vec2 operator + (const Vec2 &v) const { return Vec2(x() + v.x(), y() + v.y()); }
+    // subtract another vector
+    Vec2 operator - (const Vec2 &v) const { return Vec2(x() - v.x(), y() - v.y()); }
+    // multiply two vectors as complex numbers
+    Vec2 operator * (const Vec2 &v) const
+    {
+        double x1 = x() * v.x() - y() * v.y();
+        double y1 = x() * v.y() + y() * v.x();
+        return Vec2(x1, y1);
+    }
+    // division as complex numbers
+    Vec2 operator / (const Vec2 &v) const
+    {
+        double snorm = v.squared_norm();
+        return Vec2(this->dot(v) / snorm, -this->cross(v) / snorm);
+    }
+    // in-place vector addition
+    Vec2 operator += (const Vec2 &v) { x_ += v.x(); y_ += v.y(); return (*this); }
+    // in-place vector subtraction
+    Vec2 operator -= (const Vec2 &v) { x_ -= v.x(); y_ -= v.y(); return (*this); }
+    // in-place vector multiplication as complex numbers
+    Vec2 operator *= (const Vec2 &v)
+    {
+        double x1 = x() * v.x() - y() * v.y();
+        double y1 = x() * v.y() + y() * v.x();
+        x_ = x1;
+        y_ = y1;
+        return (*this);
+    }
+    // in-place vector division as complex numbers
+    Vec2 operator /= (const Vec2 &v)
+    {
+        double snorm = v.squared_norm();
+        double x1 = this->dot(v) / snorm;
+        double y1 = -this->cross(v) / snorm;
+        x_ = x1;
+        y_ = y1;
+        return (*this);
+    }
+
+    // ----------------------------------------
+
+    /*
+     * Vector util functions
+    */
+
+    // ----------------------------------------
+
+    // Return squared norm
+    double squared_norm() const { return x() * x() + y() * y(); }
+
+    // Return the usual Euclidean norm
+    double norm() const { return sqrt(x() * x() + y() * y()); }
+
+    // Inner product of two vectors
+    double dot(const Vec2 &v) const { return x() * v.x() + y() * v.y(); }
+
+    // Angle with the positive x-axis in radians
+    double angle() const { return atan2(y(), x()); }
+
+    // Angle between two vectors
+    double angle(const Vec2 &v) const
+    {
+        return acos(this->dot(v) / (norm() * v.norm()));
+    }
+
+    // Distance between two vectors
+    double dist(const Vec2 &v) const { return (*this - v).norm(); }
+
+    // Squared distance between two vectors
+    double squared_dist(const Vec2 &v) const { return (*this - v).squared_norm(); }
+
+    // Cross product of two vectors
+    double cross(const Vec2 &v) const { return x() * v.y() - y() * v.x(); }
+
+    // Return a normalized version of this vector
+    Vec2 normalize() const
+    {
+        double s = norm();
+        return (*this) / s;
+    }
+
+    // Translate a vector
+    Vec2 translate(double tx, double ty) const { return Vec2(x() + tx, y() + ty); }
+
+    // Rotate a vector by angle
+    Vec2 rotate(const double theta) const
+    {
+        double ct = cos(theta), st = sin(theta);
+        double x1 = x() * ct - y() * st;
+        double y1 = x() * ct + y() * st;
+        return Vec2(x1, y1);
+    }
+
+    // return a perpendicular vector to this one
+    Vec2 perp() const { return Vec2(-y(), x()); }
+
+private:
+    double x_;
+    double y_;
+};
+
+// ----------------------------------------
+
+/*
+ * Vec2 operations as right operand
+*/
+
+// ----------------------------------------
+
+// print formatting
+std::ostream & operator << (std::ostream &out, const Vec2 &v)
+{
+    out << "Vec2(" << v.x() << ", " << v.y() << ")";
+    return out;
+}
+
+// add another scalar
+Vec2 operator + (double c, const Vec2 &v) { return Vec2(v.x() + c, v.y() + c); }
+// subtract another scalar
+Vec2 operator - (double c, const Vec2 &v) { return Vec2(c - v.x(), c - v.y()); }
+// multiply another scalar
+Vec2 operator * (double c, const Vec2 &v) { return Vec2(v.x() * c, v.y() * c); }
+// divide another scalar
+Vec2 operator / (double c, const Vec2 &v)
+{
+    double snorm = v.squared_norm();
+    return Vec2(v.x() * c / snorm, -v.y() * c / snorm);
+}
+
+// ----------------------------------------
+
+/*
+ * Vector util functions
+*/
+
+// ----------------------------------------
+
+// Inner product
+double vdot(const Vec2 &v1, const Vec2 &v2) { return v1.dot(v2); }
+
+// Cross
+double vcross(const Vec2 &v1, const Vec2 &v2) { return v1.cross(v2); }
+
+// Vector length
+double vlength(const Vec2 &v) { return v.norm(); }
+
+// Angle with positive x-axis
+double vangle(const Vec2 &v) { return v.angle(); }
+
+// Angle between two vectors
+double vangle(const Vec2 &v1, const Vec2 &v2) { return v1.angle(v2); }
+
+// distance between two vectors
+double vdist(const Vec2 &v1, const Vec2 &v2) { return v1.dist(v2); }
+
+// squared distance between two vectors
+double vsquared_dist(const Vec2 &v1, const Vec2 &v2) { return v1.squared_dist(v2); }
+
+// normalize a vector
+Vec2 vnormalize(const Vec2 &v) { return v.normalize(); }
+
+// return a perpendicular one
+Vec2 vperp(const Vec2 &v) { return v.perp(); }
+
+// rotate a vector
+Vec2 vrotate(const Vec2 &v, double theta) { return v.rotate(theta); }
+
+// return a unit vector of given direction
+Vec2 vdir(const double angle) { return Vec2::unitVec2(angle); }
+
+// return a linear combination of two vectors
+Vec2 vinterp(const Vec2 &v1, const Vec2 &v2, double t) { return v1 * (1 - t) + v2 * t; }
+
+}
+}
+
+#endif // VEC2_H

src/decition/decition_brain/decition/decide_gps_00.cpp

@@ -1772,6 +1772,8 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
             {
                 ServiceCarStatus.carState = 1;
                 busMode=false;
+                ServiceCarStatus.station_control_door=0;
+                ServiceCarStatus.station_control_door_option=false;
             }
         }
         if(ServiceCarStatus.stationCmd.has_carMode)
@@ -1794,7 +1796,6 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
     //carState == 0，紧急停车
     if (ServiceCarStatus.emergencyStop==1)
     {
-
         minDecelerate=-1.0;
     }
 
@@ -1845,6 +1846,11 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
             dSpeed = min(20.0, dSpeed);
         }
 
+        if((abs(realspeed)<0.1)&&(ServiceCarStatus.station_control_door==0))
+        {
+                ServiceCarStatus.station_control_door=1;   //open door
+        }
+
         //站点停车log
         givlog->debug("brain_v2x","aim_gps_ins.gps_lat: %.7f, aim_gps_ins.gps_lng: %.7f, dis: %.2f, dSpeed: %.1f",
                       aim_gps_ins.gps_lat, aim_gps_ins.gps_lng,
@@ -1909,17 +1915,8 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
     // givlog->debug("SPEED","dSpeed is %f",dSpeed);
     phaseSpeedDecition(gps_decition, secSpeed, obsDistance, obsSpeed,now_gps_ins);
 
-
-    if(ServiceCarStatus.stationCmd.mode_manual_drive==true)
-    {
-        gps_decition->mode=0;       //云平台控制切手动驾驶
-    }
-
-
     gps_decition->wheel_angle = 0.0 - controlAng;
 
-
-
     lastAngle=gps_decition->wheel_angle;
 
     //   qDebug("decide1 time is %d",xTime.elapsed());

src/decition/decition_external/gps_type.h

@@ -51,7 +51,12 @@ namespace iv {
         int roadSum;
         int roadOri;
 
-	double mfRoadWidth = 3.5; // Current Road Width
+    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
 
 
 

src/detection/detection_ndt_slam_hcp2/common/gps_type.h

@@ -51,7 +51,12 @@ struct GPS_INS
     int roadSum;
     int roadOri;
 
-	double mfRoadWidth = 3.5; // Current Road Width
+    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
 
 
 

src/driver/driver_map_trace/gps_type.h

@@ -51,7 +51,12 @@ namespace iv {
         int roadSum;
         int roadOri;
 
-        double mfRoadWidth = 3.5; // Current Road Width
+    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
 
 
 

src/driver/driver_map_xodrload/globalplan.cpp

@@ -1093,19 +1093,41 @@ double getwidth(Road * pRoad, int nlane,std::vector<iv::lanewidthabcd> xvectorlw
     return frtn;
 }
 
+
+
 std::vector<iv::lanewidthabcd> GetLaneWidthABCD(Road * pRoad)
 {
     std::vector<iv::lanewidthabcd> xvectorlwa;
     if(pRoad->GetLaneSectionCount()<1)return xvectorlwa;
     int i;
-    int nlanecount = pRoad->GetLaneSection(0)->GetLaneCount();
+
+
+    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 = pRoad->GetLaneSection(0)->GetLane(i)->GetLaneWidth(0);
-        xlwa.nlane = pRoad->GetLaneSection(0)->GetLane(i)->GetId();
+        LaneWidth* pLW = pLS->GetLane(i)->GetLaneWidth(0);
+        xlwa.nlane = pLS->GetLane(i)->GetId();
         if(pLW != 0)
         {
 
@@ -1123,8 +1145,8 @@ std::vector<iv::lanewidthabcd> GetLaneWidthABCD(Road * pRoad)
             xlwa.D = 0;
         }
 
-
-        xvectorlwa.push_back(xlwa);
+ //       if(strncmp(pLS->GetLane(i)->GetType().data(),"driving",256) == 0)
+        xvectorlwa.push_back(xlwa);   //Calculate Road Width, not driving need add in.
 
     }
     return xvectorlwa;
@@ -1166,6 +1188,112 @@ inline double getoff(int nlane,double s,std::vector<iv::lanewidthabcd> xvectorLW
 
 }
 
+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)
+{
+    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
+
+    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;
+                    }
+                }
+                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;
@@ -1283,6 +1411,14 @@ std::vector<PlanPoint> GetLanePoint(pathsection xps,std::vector<PlanPoint> xvPP)
             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.mWidth = getwidth(xps.mpRoad,nlane1,xvectorLWA,pp.mS);
+            pp.mfDisToLaneLeft = pp.mWidth/2.0;
+            pp.lanmp = 0;
+            pp.mfDisToRoadLeft = off1*(-1);
+            pp.mfRoadWidth = GetRoadWidth(xvectorLWA,nlane1,pp.mS);
+            GetLaneOriTotal(xps.mpRoad,nlane1,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed);
+
             xvectorPP.push_back(pp);
 
         }
@@ -1295,6 +1431,48 @@ std::vector<PlanPoint> GetLanePoint(pathsection xps,std::vector<PlanPoint> xvPP)
             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);
+
+            double flanewidth1 = getwidth(xps.mpRoad,nlane1,xvectorLWA,pp.mS);
+
+            pp.mfDisToRoadLeft = offx*(-1);
+            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);
 
         }
@@ -1304,6 +1482,14 @@ std::vector<PlanPoint> GetLanePoint(pathsection xps,std::vector<PlanPoint> xvPP)
             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.mWidth = getwidth(xps.mpRoad,nlane2,xvectorLWA,pp.mS);
+            pp.mfDisToLaneLeft = pp.mWidth/2.0;
+            pp.lanmp = 0;
+            pp.mfDisToRoadLeft = off2*(-1);
+            pp.mfRoadWidth = GetRoadWidth(xvectorLWA,nlane2,pp.mS);
+            GetLaneOriTotal(xps.mpRoad,nlane2,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed);
+
             xvectorPP.push_back(pp);
 
         }
@@ -1316,6 +1502,48 @@ std::vector<PlanPoint> GetLanePoint(pathsection xps,std::vector<PlanPoint> xvPP)
             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);
+
+            double flanewidth1 = getwidth(xps.mpRoad,nlane2,xvectorLWA,pp.mS);
+
+            pp.mfDisToRoadLeft = offx*(-1);
+            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);
 
         }
@@ -1325,6 +1553,15 @@ std::vector<PlanPoint> GetLanePoint(pathsection xps,std::vector<PlanPoint> xvPP)
             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.mWidth = getwidth(xps.mpRoad,nlane3,xvectorLWA,pp.mS);
+            pp.mfDisToLaneLeft = pp.mWidth/2.0;
+            pp.lanmp = 0;
+            pp.mfDisToRoadLeft = off3*(-1);
+            pp.mfRoadWidth = GetRoadWidth(xvectorLWA,nlane3,pp.mS);
+            GetLaneOriTotal(xps.mpRoad,nlane3,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed);
+
+
             xvectorPP.push_back(pp);
 
         }
@@ -1339,6 +1576,15 @@ std::vector<PlanPoint> GetLanePoint(pathsection xps,std::vector<PlanPoint> xvPP)
             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);
 
         }
@@ -1352,6 +1598,48 @@ std::vector<PlanPoint> GetLanePoint(pathsection xps,std::vector<PlanPoint> xvPP)
             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);
 
         }
@@ -1362,6 +1650,15 @@ std::vector<PlanPoint> GetLanePoint(pathsection xps,std::vector<PlanPoint> xvPP)
             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);
 
         }
@@ -1375,6 +1672,48 @@ std::vector<PlanPoint> GetLanePoint(pathsection xps,std::vector<PlanPoint> xvPP)
             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);
 
         }
@@ -1385,6 +1724,15 @@ std::vector<PlanPoint> GetLanePoint(pathsection xps,std::vector<PlanPoint> xvPP)
             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);
 
         }
@@ -1651,6 +1999,9 @@ void checktrace(std::vector<PlanPoint> & xPlan)
     }
 }
 
+
+#include <QFile>
+
 /**
  * @brief MakePlan         全局路径规划
  * @param pxd              有向图
@@ -1754,6 +2105,13 @@ int MakePlan(xodrdijkstra * pxd,OpenDrive * pxodr,const double x_now,const doubl
                     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);
+
                     xvectorPP.push_back(pp);
                     i++;
                 }
@@ -1764,6 +2122,15 @@ int MakePlan(xodrdijkstra * pxd,OpenDrive * pxodr,const double x_now,const doubl
                     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);
+
+
                     xvectorPP.push_back(pp);
                     i--;
                 }
@@ -1785,6 +2152,22 @@ int MakePlan(xodrdijkstra * pxd,OpenDrive * pxodr,const double x_now,const doubl
 
     }
 
+    int i;
+
+//    QFile xFile;
+//    xFile.setFileName("/home/yuchuli/tempgps.txt");
+//    xFile.open(QIODevice::ReadWrite);
+
+//    for(i<0;i<xPlan.size();i++)
+//    {
+//        char strout[1000];
+//        snprintf(strout,1000,"%d %f %f %d %d  %d %f \n",i,xPlan[i].mfDisToLaneLeft,
+//                 xPlan[i].mfDisToRoadLeft,xPlan[i].mnLaneori,xPlan[i].mnLaneTotal,
+//                 xPlan[i].lanmp,xPlan[i].mWidth);
+//        xFile.write(strout,strnlen(strout,1000));
+//    }
+//    xFile.close();
+
     checktrace(xPlan);
 //    std::cout<<"pp size is "<<xvectorPP.size()<<std::endl;
     return 0;
@@ -1813,7 +2196,7 @@ int MakePlan(xodrdijkstra * pxd,OpenDrive * pxodr,const double x_now,const doubl
 
     bianxiandis = pRoad->GetLaneSection(0)->GetLane(1)->GetWidthValue(1)/2.0;
 
-    int i = 1;
+//    int i = 1;
     for(i=1;i<nlane;i++)
     {
         bianxiandis = bianxiandis + pRoad->GetLaneSection(0)->GetLane(i)->GetWidthValue(1)/2.0

src/driver/driver_map_xodrload/globalplan.h

@@ -18,6 +18,12 @@ public:
     double mWidth;
     double mLeftWidth[5];
     double mRightWidth[5];
+
+    double mfRoadWidth;
+    double mfDisToRoadLeft;
+    double mfDisToLaneLeft;
+    int mnLaneori = 0;  //from Right 0
+    int mnLaneTotal = 1; //Lane Total
 };
 
 int GetNearPoint(const double x,const double y,OpenDrive * pxodr,Road ** pObjRoad,GeometryBlock ** pgeo, double & s,double & nearx,

src/driver/driver_map_xodrload/gps_type.h

@@ -51,7 +51,12 @@ namespace iv {
         int roadSum;
         int roadOri;
 
-	double mfRoadWidth = 3.5; // Current Road Width
+    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
 
 
 

src/driver/driver_map_xodrload/main.cpp

@@ -391,9 +391,15 @@ static void ToGPSTrace(std::vector<PlanPoint> xPlan)
         givlog->debug(" %d %11.7f %11.7f %11.3f ",i,data->gps_lat,
                data->gps_lng,data->ins_heading_angle);
 
-        data->roadSum = 1;
+        data->roadSum = xPlan[i].mnLaneTotal;
         data->roadMode = 0;
-        data->roadOri = 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;

src/driver/driver_radio_collision/gps_type.h

@@ -51,7 +51,12 @@ struct GPS_INS
     int roadSum;
     int roadOri;
 	
-	double mfRoadWidth = 3.5; // Current Road Width
+    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
 };
 
 typedef boost::shared_ptr<iv::GPS_INS> GPSData;

src/driver/driver_radio_p900/gps_type.h

@@ -51,7 +51,12 @@ struct GPS_INS
     int roadSum;
     int roadOri;
 
-	double mfRoadWidth = 3.5; // Current Road Width
+    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
 };
 
 typedef boost::shared_ptr<iv::GPS_INS> GPSData;

src/tool/data_serials/gps_type.h

@@ -51,7 +51,12 @@ struct GPS_INS
     int roadSum;
     int roadOri;
 
-	double mfRoadWidth = 3.5; // Current Road Width
+    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
 
 
 

src/tool/ivmapmake/common/gps_type.h

@@ -51,7 +51,12 @@ namespace iv {
         int roadSum;
         int roadOri;
 
-	double mfRoadWidth = 3.5; // Current Road Width
+    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
 
 
 

src/tool/ivmapmake_sharemem/common/gps_type.h

@@ -51,7 +51,12 @@ namespace iv {
         int roadSum;
         int roadOri;
 
-	double mfRoadWidth = 3.5; // Current Road Width
+    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
 
 
 

src/tool/map_lanetoxodr/gps_type.h

@@ -51,7 +51,12 @@ namespace iv {
         int roadSum;
         int roadOri;
 
-	double mfRoadWidth = 3.5; // Current Road Width
+    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
 
 
 

src/tool/map_lanetoxodr_graphscene/gps_type.h

@@ -51,7 +51,12 @@ namespace iv {
         int roadSum;
         int roadOri;
 
-	double mfRoadWidth = 3.5; // Current Road Width
+    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
 
 
 

src/tool/tool_mapcreate/gps_type.h

@@ -51,7 +51,12 @@ namespace iv {
         int roadSum;
         int roadOri;
 
-	double mfRoadWidth = 3.5; // Current Road Width
+    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
 
 
 

src/tool/tool_xodrobj/gps_type.h

@@ -51,7 +51,12 @@ namespace iv {
         int roadSum;
         int roadOri;
 
-	double mfRoadWidth = 3.5; // Current Road Width
+    double mfLaneWidth = 3.5; // Current Road 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
 
 
 

src/tool/tracetoxodr/gps_type.h

@@ -51,7 +51,12 @@ namespace iv {
         int roadSum;
         int roadOri;
 
-	double mfRoadWidth = 3.5; // Current Road Width
+    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
 
 
 

src/ui/ui_ads_hmi/common/gps_type.h

@@ -51,7 +51,12 @@ struct GPS_INS
     int roadSum;
     int roadOri;
 
-	double mfRoadWidth = 3.5; // Current Road Width
+    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
 
 
 

src/v2x/platform/common/gps_type.h

@@ -51,7 +51,12 @@ namespace iv {
         int roadSum;
         int roadOri;
 
-	double mfRoadWidth = 3.5; // Current Road Width
+    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