change read vehparam method

src/decition/common/common/sysparam_type.h

@@ -28,6 +28,9 @@ namespace iv {
         float gpsOffset_x=0;
         float gpsOffset_y=0;
 
+        float vehWidth= 2.1;
+        float vehLenth= 4.6;
+
 
     };
 }

src/decition/decition_brain/decition/adc_planner/lane_change_planner.cpp

@@ -35,7 +35,7 @@ std::vector<iv::Point2D> iv::decition::LaneChangePlanner::getPath(GPS_INS now_gp
 
 bool iv::decition::LaneChangePlanner::checkAvoidEnable(const std::vector<double>& obsDisVector, int roadAim){
     int roadNow = ServiceParameterStatus.now_road_num;
-    if ((obsDisVector[roadAim]>0 && obsDisVector[roadAim]<obsDisVector[roadNow]+Veh_Lenth)||
+    if ((obsDisVector[roadAim]>0 && obsDisVector[roadAim]<obsDisVector[roadNow]+ServiceCarStatus.msysparam.vehLenth)||
             (obsDisVector[roadAim]>0 && obsDisVector[roadAim]<15))
     {
         return false;

src/decition/decition_brain/decition/adc_tools/compute_00.cpp

@@ -462,11 +462,11 @@ iv::Point2D iv::decition::Compute00::getLidarObsPoint(std::vector<Point2D> gpsTr
         double carFrontx = gpsTrace[j].x;// -Form1.CarRear * Math.Cos(anglevalue);
         double carFronty = gpsTrace[j].y;// -Form1.CarRear * Math.Sin(anglevalue);
 
-        Point2D ptLeft(carFrontx + Veh_Width / 2 * cos(anglevalue + PI / 2),
-                       carFronty + Veh_Width / 2 * sin(anglevalue + PI / 2));
+        Point2D ptLeft(carFrontx + ServiceCarStatus.msysparam.vehWidth / 2 * cos(anglevalue + PI / 2),
+                       carFronty + ServiceCarStatus.msysparam.vehWidth / 2 * sin(anglevalue + PI / 2));
 
-        Point2D ptRight(carFrontx + Veh_Width / 2 * cos(anglevalue - PI / 2),
-                        carFronty + Veh_Width / 2 * sin(anglevalue - PI / 2));
+        Point2D ptRight(carFrontx + ServiceCarStatus.msysparam.vehWidth / 2 * cos(anglevalue - PI / 2),
+                        carFronty + ServiceCarStatus.msysparam.vehWidth / 2 * sin(anglevalue - PI / 2));
 
         gpsTraceLeft.push_back(ptLeft);
         gpsTraceRight.push_back(ptRight);
@@ -483,10 +483,10 @@ iv::Point2D iv::decition::Compute00::getLidarObsPoint(std::vector<Point2D> gpsTr
         {
             int count = 0;
 
-            for (double length = 0; length <= Veh_Width; length += 0.4)
+            for (double length = 0; length <= ServiceCarStatus.msysparam.vehWidth; length += 0.4)
             {
-                double ptx = gpsTraceLeft[j].x + (gpsTraceRight[j].x - gpsTraceLeft[j].x) / Veh_Width * length;
-                double pty = gpsTraceLeft[j].y + (gpsTraceRight[j].y - gpsTraceLeft[j].y) / Veh_Width * length;
+                double ptx = gpsTraceLeft[j].x + (gpsTraceRight[j].x - gpsTraceLeft[j].x) / ServiceCarStatus.msysparam.vehWidth * length;
+                double pty = gpsTraceLeft[j].y + (gpsTraceRight[j].y - gpsTraceLeft[j].y) / ServiceCarStatus.msysparam.vehWidth * length;
 
                 //				int dx = (int)(ptx / gridwide * 2 + (ptx / abs(ptx))) / 2 + centerx;  //+(ptx / abs(ptx))) / 2左右多出一半的车宽（1米）
                 //				int dy = (int)(pty / gridwide * 2 + (pty / abs(pty))) / 2 + centery;
@@ -505,10 +505,10 @@ iv::Point2D iv::decition::Compute00::getLidarObsPoint(std::vector<Point2D> gpsTr
 
             j++;
 
-            for (double length = 0; length <= Veh_Width; length += 0.4)
+            for (double length = 0; length <= ServiceCarStatus.msysparam.vehWidth; length += 0.4)
             {
-                double ptx = gpsTraceLeft[j].x + (gpsTraceRight[j].x - gpsTraceLeft[j].x) / Veh_Width * length;
-                double pty = gpsTraceLeft[j].y + (gpsTraceRight[j].y - gpsTraceLeft[j].y) / Veh_Width * length;
+                double ptx = gpsTraceLeft[j].x + (gpsTraceRight[j].x - gpsTraceLeft[j].x) / ServiceCarStatus.msysparam.vehWidth * length;
+                double pty = gpsTraceLeft[j].y + (gpsTraceRight[j].y - gpsTraceLeft[j].y) / ServiceCarStatus.msysparam.vehWidth * length;
 
                 //				int dx = (int)(ptx / gridwide * 2 + (ptx / abs(ptx))) / 2 + centerx;
                 //				int dy = (int)(pty / gridwide * 2 + (pty / abs(pty))) / 2 + centery;
@@ -574,11 +574,11 @@ iv::Point2D iv::decition::Compute00::getLidarRearObsPoint(std::vector<Point2D> g
         double carFrontx = gpsTrace[j].x;// -Form1.CarRear * Math.Cos(anglevalue);
         double carFronty = gpsTrace[j].y;// -Form1.CarRear * Math.Sin(anglevalue);
 
-        Point2D ptLeft(carFrontx + (Veh_Width-0.3) / 2 * cos(anglevalue + PI / 2),
-                       carFronty + (Veh_Width-0.3) / 2 * sin(anglevalue + PI / 2));
+        Point2D ptLeft(carFrontx + (ServiceCarStatus.msysparam.vehWidth-0.3) / 2 * cos(anglevalue + PI / 2),
+                       carFronty + (ServiceCarStatus.msysparam.vehWidth-0.3) / 2 * sin(anglevalue + PI / 2));
 
-        Point2D ptRight(carFrontx + (Veh_Width-0.3) / 2 * cos(anglevalue - PI / 2),
-                        carFronty + (Veh_Width-0.3) / 2 * sin(anglevalue - PI / 2));
+        Point2D ptRight(carFrontx + (ServiceCarStatus.msysparam.vehWidth-0.3) / 2 * cos(anglevalue - PI / 2),
+                        carFronty + (ServiceCarStatus.msysparam.vehWidth-0.3) / 2 * sin(anglevalue - PI / 2));
 
         gpsTraceLeft.push_back(ptLeft);
         gpsTraceRight.push_back(ptRight);
@@ -595,10 +595,10 @@ iv::Point2D iv::decition::Compute00::getLidarRearObsPoint(std::vector<Point2D> g
         {
             int count = 0;
 
-            for (double length = 0; length <= Veh_Width; length += 0.4)
+            for (double length = 0; length <= ServiceCarStatus.msysparam.vehWidth; length += 0.4)
             {
-                double ptx = gpsTraceLeft[j].x + (gpsTraceRight[j].x - gpsTraceLeft[j].x) / Veh_Width * length;
-                double pty = gpsTraceLeft[j].y + (gpsTraceRight[j].y - gpsTraceLeft[j].y) / Veh_Width * length;
+                double ptx = gpsTraceLeft[j].x + (gpsTraceRight[j].x - gpsTraceLeft[j].x) / ServiceCarStatus.msysparam.vehWidth * length;
+                double pty = gpsTraceLeft[j].y + (gpsTraceRight[j].y - gpsTraceLeft[j].y) / ServiceCarStatus.msysparam.vehWidth * length;
 
                 //				int dx = (int)(ptx / gridwide * 2 + (ptx / abs(ptx))) / 2 + centerx;  //+(ptx / abs(ptx))) / 2左右多出一半的车宽（1米）
                 //				int dy = (int)(pty / gridwide * 2 + (pty / abs(pty))) / 2 + centery;
@@ -620,10 +620,10 @@ iv::Point2D iv::decition::Compute00::getLidarRearObsPoint(std::vector<Point2D> g
 
             j++;
 
-            for (double length = 0; length <= Veh_Width; length += 0.4)
+            for (double length = 0; length <= ServiceCarStatus.msysparam.vehWidth; length += 0.4)
             {
-                double ptx = gpsTraceLeft[j].x + (gpsTraceRight[j].x - gpsTraceLeft[j].x) / Veh_Width * length;
-                double pty = gpsTraceLeft[j].y + (gpsTraceRight[j].y - gpsTraceLeft[j].y) / Veh_Width * length;
+                double ptx = gpsTraceLeft[j].x + (gpsTraceRight[j].x - gpsTraceLeft[j].x) / ServiceCarStatus.msysparam.vehWidth * length;
+                double pty = gpsTraceLeft[j].y + (gpsTraceRight[j].y - gpsTraceLeft[j].y) / ServiceCarStatus.msysparam.vehWidth * length;
 
                 //				int dx = (int)(ptx / gridwide * 2 + (ptx / abs(ptx))) / 2 + centerx;
                 //				int dy = (int)(pty / gridwide * 2 + (pty / abs(pty))) / 2 + centery;
@@ -689,11 +689,11 @@ iv::Point2D iv::decition::Compute00::getLidarObsPointAvoid(std::vector<Point2D>
 
         //1127 fanwei xiuzheng
         float buchang=0;
-        Point2D ptLeft(carFrontx + (Veh_Width+(buchang)*2) / 2 * cos(anglevalue + PI / 2),
-                       carFronty +  (Veh_Width+(buchang)*2) / 2 * sin(anglevalue + PI / 2));
+        Point2D ptLeft(carFrontx + (ServiceCarStatus.msysparam.vehWidth+(buchang)*2) / 2 * cos(anglevalue + PI / 2),
+                       carFronty +  (ServiceCarStatus.msysparam.vehWidth+(buchang)*2) / 2 * sin(anglevalue + PI / 2));
 
-        Point2D ptRight(carFrontx +  (Veh_Width+(buchang)*2) / 2 * cos(anglevalue - PI / 2),
-                        carFronty +  (Veh_Width+(buchang)*2) / 2 * sin(anglevalue - PI / 2));
+        Point2D ptRight(carFrontx +  (ServiceCarStatus.msysparam.vehWidth+(buchang)*2) / 2 * cos(anglevalue - PI / 2),
+                        carFronty +  (ServiceCarStatus.msysparam.vehWidth+(buchang)*2) / 2 * sin(anglevalue - PI / 2));
 
         gpsTraceLeft.push_back(ptLeft);
         gpsTraceRight.push_back(ptRight);
@@ -710,10 +710,10 @@ iv::Point2D iv::decition::Compute00::getLidarObsPointAvoid(std::vector<Point2D>
         {
             int count = 0;
 
-            for (double length = 0; length <= Veh_Width; length += 0.4)
+            for (double length = 0; length <= ServiceCarStatus.msysparam.vehWidth; length += 0.4)
             {
-                double ptx = gpsTraceLeft[j].x + (gpsTraceRight[j].x - gpsTraceLeft[j].x) / Veh_Width * length;
-                double pty = gpsTraceLeft[j].y + (gpsTraceRight[j].y - gpsTraceLeft[j].y) / Veh_Width * length;
+                double ptx = gpsTraceLeft[j].x + (gpsTraceRight[j].x - gpsTraceLeft[j].x) / ServiceCarStatus.msysparam.vehWidth * length;
+                double pty = gpsTraceLeft[j].y + (gpsTraceRight[j].y - gpsTraceLeft[j].y) / ServiceCarStatus.msysparam.vehWidth * length;
 
                 int dx = (int)(ptx / gridwide * 2 + (ptx / abs(ptx))) / 2 + centerx;  //*2左右多出一半的车宽（1米）
                 int dy = (int)(pty / gridwide * 2 + (pty / abs(pty))) / 2 + centery;
@@ -729,10 +729,10 @@ iv::Point2D iv::decition::Compute00::getLidarObsPointAvoid(std::vector<Point2D>
 
             j++;
 
-            for (double length = 0; length <= Veh_Width; length += 0.4)
+            for (double length = 0; length <= ServiceCarStatus.msysparam.vehWidth; length += 0.4)
             {
-                double ptx = gpsTraceLeft[j].x + (gpsTraceRight[j].x - gpsTraceLeft[j].x) / Veh_Width * length;
-                double pty = gpsTraceLeft[j].y + (gpsTraceRight[j].y - gpsTraceLeft[j].y) / Veh_Width * length;
+                double ptx = gpsTraceLeft[j].x + (gpsTraceRight[j].x - gpsTraceLeft[j].x) / ServiceCarStatus.msysparam.vehWidth * length;
+                double pty = gpsTraceLeft[j].y + (gpsTraceRight[j].y - gpsTraceLeft[j].y) / ServiceCarStatus.msysparam.vehWidth * length;
 
                 int dx = (int)(ptx / gridwide * 2 + (ptx / abs(ptx))) / 2 + centerx;
                 int dy = (int)(pty / gridwide * 2 + (pty / abs(pty))) / 2 + centery;
@@ -777,7 +777,7 @@ iv::Point2D iv::decition::Compute00::getLidarObsPointAvoid(std::vector<Point2D>
 //				double xxx = esrRadars[i].nomal_x + Esr_Offset;
 //				double yyy = esrRadars[i].nomal_y;
 //
-//				if (abs(xxx - gpsTrace[j].x) <= 3.0*Veh_Width / 4.0 && abs(yyy - (gpsTrace[j].y)) <= 1)
+//				if (abs(xxx - gpsTrace[j].x) <= 3.0*ServiceCarStatus.msysparam.vehWidth / 4.0 && abs(yyy - (gpsTrace[j].y)) <= 1)
 //				{
 //
 //					if (lastEsrID == (esrRadars[i]).esr_ID)
@@ -827,13 +827,13 @@ int  iv::decition::Compute00::getEsrIndex(std::vector<Point2D> gpsTrace,int road
                 ODS("\nESR毫米波检测物体Y距离:%f\n", yyy);*/
 
 //优化
-//                if(sqrt((xxx - gpsTrace[j].x)*(xxx - gpsTrace[j].x) + (yyy - gpsTrace[j].y)*(yyy - gpsTrace[j].y)) < (1.0*Veh_Width / 2.0+DecideGps00().xiuzhengCs)){
+//                if(sqrt((xxx - gpsTrace[j].x)*(xxx - gpsTrace[j].x) + (yyy - gpsTrace[j].y)*(yyy - gpsTrace[j].y)) < (1.0*ServiceCarStatus.msysparam.vehWidth / 2.0+DecideGps00().xiuzhengCs)){
 //                    *esrPathpoint = j;
 //                    return i;
 //                }
 
 
-                if (abs(xxx - gpsTrace[j].x) <= (3.0*Veh_Width / 4.0+fxiuzhengCs) && abs(yyy - (gpsTrace[j].y)) <= 1)
+                if (abs(xxx - gpsTrace[j].x) <= (3.0*ServiceCarStatus.msysparam.vehWidth / 4.0+fxiuzhengCs) && abs(yyy - (gpsTrace[j].y)) <= 1)
                 {
                     return i;
 
@@ -889,7 +889,7 @@ int  iv::decition::Compute00::getRearEsrIndex(std::vector<Point2D> gpsTrace,int
                 ODS("\nESR毫米波检测物体Y距离:%f\n", yyy);*/
 
 
-                if (abs(xxx - gpsTrace[j].x) <= (3.0*Veh_Width / 4.0+DecideGps00().xiuzhengCs) && abs(yyy - (gpsTrace[j].y)) <= 1)
+                if (abs(xxx - gpsTrace[j].x) <= (3.0*ServiceCarStatus.msysparam.vehWidth / 4.0+DecideGps00().xiuzhengCs) && abs(yyy - (gpsTrace[j].y)) <= 1)
                 {
 
                     if (lastEsrID == i)
@@ -939,7 +939,7 @@ int  iv::decition::Compute00::getRearEsrIndex(std::vector<Point2D> gpsTrace,int
 //                ODS("\nESR毫米波检测物体Y距离:%f\n", yyy);*/
 
 
-//                if (abs(xxx - gpsTrace[j].x) <= 3.0*Veh_Width / 4.0 && abs(yyy - (gpsTrace[j].y)) <= 1)
+//                if (abs(xxx - gpsTrace[j].x) <= 3.0*ServiceCarStatus.msysparam.vehWidth / 4.0 && abs(yyy - (gpsTrace[j].y)) <= 1)
 //                {
 
 //                    if (lastEsrID == i)
@@ -983,7 +983,7 @@ int  iv::decition::Compute00::getEsrIndexAvoid(std::vector<Point2D> gpsTrace) {
                 double xxx = ServiceCarStatus.obs_radar[i].nomal_x + Esr_Offset;
                 double yyy = ServiceCarStatus.obs_radar[i].nomal_y;
 
-                if (abs(xxx - gpsTrace[j].x) <= 3.0*Veh_Width / 4.0 && abs(yyy - (gpsTrace[j].y)) <= 1)
+                if (abs(xxx - gpsTrace[j].x) <= 3.0*ServiceCarStatus.msysparam.vehWidth / 4.0 && abs(yyy - (gpsTrace[j].y)) <= 1)
                 {
 
                     if (lastEsrIDAvoid == i)
@@ -2048,10 +2048,10 @@ int iv::decition::Compute00::getEsrIndexByFrenet(std::vector<Point2D> gpsTrace,
 //            esrObsPoint = iv::decition::FrenetPlanner::XY2Frenet(xxx,yyy,gpsTrace);
             esrObsPoint = iv::decition::FrenetPlanner::getFrenetfromXY(xxx,yyy,gpsTrace,gpsMap,pathpoint,nowGps);
 
-            //如果障碍物与道路的横向距离d<=3.0*Veh_Width / 4.0，则认为道路上有障碍物。
+            //如果障碍物与道路的横向距离d<=3.0*ServiceCarStatus.msysparam.vehWidth / 4.0，则认为道路上有障碍物。
             //s则可理解为障碍物距离。为障碍物与车辆沿着道路方向的距离，而不是空间上的x或y坐标或者直线距离。
             //minDistance、minDis_index用来统计最近的障碍物信息。
-            if(abs(esrObsPoint.d)<=(3.0*Veh_Width / 4.0+DecideGps00().xiuzhengCs)){
+            if(abs(esrObsPoint.d)<=(3.0*ServiceCarStatus.msysparam.vehWidth / 4.0+DecideGps00().xiuzhengCs)){
                 if(esrObsPoint.s<minDistance){
                     minDistance = esrObsPoint.s;
                     minDis_index = i;

src/decition/decition_brain/decition/brain.cpp

@@ -272,6 +272,8 @@ void iv::decition::BrainDecition::run() {
     ServiceCarStatus.msysparam.mstriccid = xp.GetParam("iccid","11111111111111111111");
     ServiceCarStatus.msysparam.mvehtype= xp.GetParam("vehType","ge3");
 
+    ServiceCarStatus.msysparam.vehWidth = atof(xp.GetParam("vehWidth","2.1").data());
+    ServiceCarStatus.msysparam.vehLenth = atof(xp.GetParam("vehLenth","4.6").data());
 
 
     /*=============     20200113 apollo_fu  add ===========*/

src/decition/decition_brain/decition/decide_gps_00.cpp

@@ -1586,7 +1586,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
                 }
                 else if(useFrenet){
                     if(roadPre != roadNow){
-                        avoidMinDistanceVector[roadNow] = obsDisVector[roadNow] + Veh_Lenth;
+                        avoidMinDistanceVector[roadNow] = obsDisVector[roadNow] + ServiceCarStatus.msysparam.vehLenth;
                         startAvoidGpsInsVector[roadNow] = now_gps_ins;
                     }
                     roadNow = roadPre;
@@ -2400,7 +2400,7 @@ double iv::decition::DecideGps00::limitSpeed(double angle, double speed) {
 
 bool iv::decition::DecideGps00::checkAvoidEnable(double avoidX, iv::LidarGridPtr lidarGridPtr,int roadNum) {
 
-    if ((obsDisVector[roadNum]>0 && obsDisVector[roadNum]<obsDisVector[roadNow]+Veh_Lenth)||
+    if ((obsDisVector[roadNum]>0 && obsDisVector[roadNum]<obsDisVector[roadNow]+ServiceCarStatus.msysparam.vehLenth)||
             (obsDisVector[roadNum]>0 && obsDisVector[roadNum]<15))
     {
         return false;
@@ -3111,10 +3111,10 @@ int iv::decition::DecideGps00::chooseAvoidRoad(iv::LidarGridPtr lidarGridPtr, GP
             {
                 /*if (roadNow==roadOri)
                 {
-                    avoidRunDistance = obsDisVector[roadNow] + Veh_Lenth;
+                    avoidRunDistance = obsDisVector[roadNow] + ServiceCarStatus.msysparam.vehLenth;
                     startAvoid_gps_ins = now_gps_ins;
                 }	*/
-                avoidMinDistanceVector[roadNow]= obsDisVector[roadNow] + Veh_Lenth;
+                avoidMinDistanceVector[roadNow]= obsDisVector[roadNow] + ServiceCarStatus.msysparam.vehLenth;
                 startAvoidGpsInsVector[roadNow] = now_gps_ins;
                 roadPre = roadNow + i;
                 return roadPre;
@@ -3128,10 +3128,10 @@ int iv::decition::DecideGps00::chooseAvoidRoad(iv::LidarGridPtr lidarGridPtr, GP
             {
                 /*if (roadNow == roadOri)
                 {
-                    avoidRunDistance = obsDisVector[roadNow] + Veh_Lenth;
+                    avoidRunDistance = obsDisVector[roadNow] + ServiceCarStatus.msysparam.vehLenth;
                     startAvoid_gps_ins = now_gps_ins;
                 }*/
-                avoidMinDistanceVector[roadNow] = obsDisVector[roadNow] + Veh_Lenth;
+                avoidMinDistanceVector[roadNow] = obsDisVector[roadNow] + ServiceCarStatus.msysparam.vehLenth;
                 startAvoidGpsInsVector[roadNow] = now_gps_ins;
                 roadPre = roadNow - i;
                 return roadPre;

src/decition/decition_brain/decition/obs_predict.cpp

@@ -59,7 +59,7 @@ double iv::decition::getCrashDis(double realSpeed,std::vector<Point2D>  gpsTrace
         double obsY= obs.location.y+obs.velocity.y*time;
         Point2D obsP(obsX,obsY);
         double obsDis= GetDistance(obsP,gpsTrace[i*10]);
-        if(obsDis<0.7*(Veh_Width+obs.size.x) ){
+        if(obsDis<0.7*(ServiceCarStatus.msysparam.vehWidth+obs.size.x) ){
             return dis;
         }
 

src/decition/decition_brain_sf/decition/adc_planner/lane_change_planner.cpp

@@ -35,7 +35,7 @@ std::vector<iv::Point2D> iv::decition::LaneChangePlanner::getPath(GPS_INS now_gp
 
 bool iv::decition::LaneChangePlanner::checkAvoidEnable(const std::vector<double>& obsDisVector, int roadAim){
     int roadNow = ServiceParameterStatus.now_road_num;
-    if ((obsDisVector[roadAim]>0 && obsDisVector[roadAim]<obsDisVector[roadNow]+Veh_Lenth)||
+    if ((obsDisVector[roadAim]>0 && obsDisVector[roadAim]<obsDisVector[roadNow]+ServiceCarStatus.msysparam.vehLenth)||
             (obsDisVector[roadAim]>0 && obsDisVector[roadAim]<15))
     {
         return false;

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

@@ -458,11 +458,11 @@ iv::Point2D iv::decition::Compute00::getLidarObsPoint(std::vector<Point2D> gpsTr
         double carFrontx = gpsTrace[j].x;// -Form1.CarRear * Math.Cos(anglevalue);
         double carFronty = gpsTrace[j].y;// -Form1.CarRear * Math.Sin(anglevalue);
 
-        Point2D ptLeft(carFrontx + Veh_Width / 2 * cos(anglevalue + PI / 2),
-                       carFronty + Veh_Width / 2 * sin(anglevalue + PI / 2));
+        Point2D ptLeft(carFrontx + ServiceCarStatus.msysparam.vehWidth / 2 * cos(anglevalue + PI / 2),
+                       carFronty + ServiceCarStatus.msysparam.vehWidth / 2 * sin(anglevalue + PI / 2));
 
-        Point2D ptRight(carFrontx + Veh_Width / 2 * cos(anglevalue - PI / 2),
-                        carFronty + Veh_Width / 2 * sin(anglevalue - PI / 2));
+        Point2D ptRight(carFrontx + ServiceCarStatus.msysparam.vehWidth / 2 * cos(anglevalue - PI / 2),
+                        carFronty + ServiceCarStatus.msysparam.vehWidth / 2 * sin(anglevalue - PI / 2));
 
         gpsTraceLeft.push_back(ptLeft);
         gpsTraceRight.push_back(ptRight);
@@ -479,10 +479,10 @@ iv::Point2D iv::decition::Compute00::getLidarObsPoint(std::vector<Point2D> gpsTr
         {
             int count = 0;
 
-            for (double length = 0; length <= Veh_Width; length += 0.4)
+            for (double length = 0; length <= ServiceCarStatus.msysparam.vehWidth; length += 0.4)
             {
-                double ptx = gpsTraceLeft[j].x + (gpsTraceRight[j].x - gpsTraceLeft[j].x) / Veh_Width * length;
-                double pty = gpsTraceLeft[j].y + (gpsTraceRight[j].y - gpsTraceLeft[j].y) / Veh_Width * length;
+                double ptx = gpsTraceLeft[j].x + (gpsTraceRight[j].x - gpsTraceLeft[j].x) / ServiceCarStatus.msysparam.vehWidth * length;
+                double pty = gpsTraceLeft[j].y + (gpsTraceRight[j].y - gpsTraceLeft[j].y) / ServiceCarStatus.msysparam.vehWidth * length;
 
                 //				int dx = (int)(ptx / gridwide * 2 + (ptx / abs(ptx))) / 2 + centerx;  //+(ptx / abs(ptx))) / 2左右多出一半的车宽（1米）
                 //				int dy = (int)(pty / gridwide * 2 + (pty / abs(pty))) / 2 + centery;
@@ -501,10 +501,10 @@ iv::Point2D iv::decition::Compute00::getLidarObsPoint(std::vector<Point2D> gpsTr
 
             j++;
 
-            for (double length = 0; length <= Veh_Width; length += 0.4)
+            for (double length = 0; length <= ServiceCarStatus.msysparam.vehWidth; length += 0.4)
             {
-                double ptx = gpsTraceLeft[j].x + (gpsTraceRight[j].x - gpsTraceLeft[j].x) / Veh_Width * length;
-                double pty = gpsTraceLeft[j].y + (gpsTraceRight[j].y - gpsTraceLeft[j].y) / Veh_Width * length;
+                double ptx = gpsTraceLeft[j].x + (gpsTraceRight[j].x - gpsTraceLeft[j].x) / ServiceCarStatus.msysparam.vehWidth * length;
+                double pty = gpsTraceLeft[j].y + (gpsTraceRight[j].y - gpsTraceLeft[j].y) / ServiceCarStatus.msysparam.vehWidth * length;
 
                 //				int dx = (int)(ptx / gridwide * 2 + (ptx / abs(ptx))) / 2 + centerx;
                 //				int dy = (int)(pty / gridwide * 2 + (pty / abs(pty))) / 2 + centery;
@@ -577,11 +577,11 @@ iv::Point2D iv::decition::Compute00::getLidarRearObsPoint(std::vector<Point2D> g
         double carFrontx = gpsTrace[j].x;// -Form1.CarRear * Math.Cos(anglevalue);
         double carFronty = gpsTrace[j].y;// -Form1.CarRear * Math.Sin(anglevalue);
 
-        Point2D ptLeft(carFrontx + (Veh_Width-0.3) / 2 * cos(anglevalue + PI / 2),
-                       carFronty + (Veh_Width-0.3) / 2 * sin(anglevalue + PI / 2));
+        Point2D ptLeft(carFrontx + (ServiceCarStatus.msysparam.vehWidth-0.3) / 2 * cos(anglevalue + PI / 2),
+                       carFronty + (ServiceCarStatus.msysparam.vehWidth-0.3) / 2 * sin(anglevalue + PI / 2));
 
-        Point2D ptRight(carFrontx + (Veh_Width-0.3) / 2 * cos(anglevalue - PI / 2),
-                        carFronty + (Veh_Width-0.3) / 2 * sin(anglevalue - PI / 2));
+        Point2D ptRight(carFrontx + (ServiceCarStatus.msysparam.vehWidth-0.3) / 2 * cos(anglevalue - PI / 2),
+                        carFronty + (ServiceCarStatus.msysparam.vehWidth-0.3) / 2 * sin(anglevalue - PI / 2));
 
         gpsTraceLeft.push_back(ptLeft);
         gpsTraceRight.push_back(ptRight);
@@ -598,10 +598,10 @@ iv::Point2D iv::decition::Compute00::getLidarRearObsPoint(std::vector<Point2D> g
         {
             int count = 0;
 
-            for (double length = 0; length <= Veh_Width; length += 0.4)
+            for (double length = 0; length <= ServiceCarStatus.msysparam.vehWidth; length += 0.4)
             {
-                double ptx = gpsTraceLeft[j].x + (gpsTraceRight[j].x - gpsTraceLeft[j].x) / Veh_Width * length;
-                double pty = gpsTraceLeft[j].y + (gpsTraceRight[j].y - gpsTraceLeft[j].y) / Veh_Width * length;
+                double ptx = gpsTraceLeft[j].x + (gpsTraceRight[j].x - gpsTraceLeft[j].x) / ServiceCarStatus.msysparam.vehWidth * length;
+                double pty = gpsTraceLeft[j].y + (gpsTraceRight[j].y - gpsTraceLeft[j].y) / ServiceCarStatus.msysparam.vehWidth * length;
 
                 //				int dx = (int)(ptx / gridwide * 2 + (ptx / abs(ptx))) / 2 + centerx;  //+(ptx / abs(ptx))) / 2左右多出一半的车宽（1米）
                 //				int dy = (int)(pty / gridwide * 2 + (pty / abs(pty))) / 2 + centery;
@@ -623,10 +623,10 @@ iv::Point2D iv::decition::Compute00::getLidarRearObsPoint(std::vector<Point2D> g
 
             j++;
 
-            for (double length = 0; length <= Veh_Width; length += 0.4)
+            for (double length = 0; length <= ServiceCarStatus.msysparam.vehWidth; length += 0.4)
             {
-                double ptx = gpsTraceLeft[j].x + (gpsTraceRight[j].x - gpsTraceLeft[j].x) / Veh_Width * length;
-                double pty = gpsTraceLeft[j].y + (gpsTraceRight[j].y - gpsTraceLeft[j].y) / Veh_Width * length;
+                double ptx = gpsTraceLeft[j].x + (gpsTraceRight[j].x - gpsTraceLeft[j].x) / ServiceCarStatus.msysparam.vehWidth * length;
+                double pty = gpsTraceLeft[j].y + (gpsTraceRight[j].y - gpsTraceLeft[j].y) / ServiceCarStatus.msysparam.vehWidth * length;
 
                 //				int dx = (int)(ptx / gridwide * 2 + (ptx / abs(ptx))) / 2 + centerx;
                 //				int dy = (int)(pty / gridwide * 2 + (pty / abs(pty))) / 2 + centery;
@@ -692,11 +692,11 @@ iv::Point2D iv::decition::Compute00::getLidarObsPointAvoid(std::vector<Point2D>
 
         //1127 fanwei xiuzheng
         float buchang=0;
-        Point2D ptLeft(carFrontx + (Veh_Width+(buchang)*2) / 2 * cos(anglevalue + PI / 2),
-                       carFronty +  (Veh_Width+(buchang)*2) / 2 * sin(anglevalue + PI / 2));
+        Point2D ptLeft(carFrontx + (ServiceCarStatus.msysparam.vehWidth+(buchang)*2) / 2 * cos(anglevalue + PI / 2),
+                       carFronty +  (ServiceCarStatus.msysparam.vehWidth+(buchang)*2) / 2 * sin(anglevalue + PI / 2));
 
-        Point2D ptRight(carFrontx +  (Veh_Width+(buchang)*2) / 2 * cos(anglevalue - PI / 2),
-                        carFronty +  (Veh_Width+(buchang)*2) / 2 * sin(anglevalue - PI / 2));
+        Point2D ptRight(carFrontx +  (ServiceCarStatus.msysparam.vehWidth+(buchang)*2) / 2 * cos(anglevalue - PI / 2),
+                        carFronty +  (ServiceCarStatus.msysparam.vehWidth+(buchang)*2) / 2 * sin(anglevalue - PI / 2));
 
         gpsTraceLeft.push_back(ptLeft);
         gpsTraceRight.push_back(ptRight);
@@ -713,10 +713,10 @@ iv::Point2D iv::decition::Compute00::getLidarObsPointAvoid(std::vector<Point2D>
         {
             int count = 0;
 
-            for (double length = 0; length <= Veh_Width; length += 0.4)
+            for (double length = 0; length <= ServiceCarStatus.msysparam.vehWidth; length += 0.4)
             {
-                double ptx = gpsTraceLeft[j].x + (gpsTraceRight[j].x - gpsTraceLeft[j].x) / Veh_Width * length;
-                double pty = gpsTraceLeft[j].y + (gpsTraceRight[j].y - gpsTraceLeft[j].y) / Veh_Width * length;
+                double ptx = gpsTraceLeft[j].x + (gpsTraceRight[j].x - gpsTraceLeft[j].x) / ServiceCarStatus.msysparam.vehWidth * length;
+                double pty = gpsTraceLeft[j].y + (gpsTraceRight[j].y - gpsTraceLeft[j].y) / ServiceCarStatus.msysparam.vehWidth * length;
 
                 int dx = (int)(ptx / gridwide * 2 + (ptx / abs(ptx))) / 2 + centerx;  //*2左右多出一半的车宽（1米）
                 int dy = (int)(pty / gridwide * 2 + (pty / abs(pty))) / 2 + centery;
@@ -732,10 +732,10 @@ iv::Point2D iv::decition::Compute00::getLidarObsPointAvoid(std::vector<Point2D>
 
             j++;
 
-            for (double length = 0; length <= Veh_Width; length += 0.4)
+            for (double length = 0; length <= ServiceCarStatus.msysparam.vehWidth; length += 0.4)
             {
-                double ptx = gpsTraceLeft[j].x + (gpsTraceRight[j].x - gpsTraceLeft[j].x) / Veh_Width * length;
-                double pty = gpsTraceLeft[j].y + (gpsTraceRight[j].y - gpsTraceLeft[j].y) / Veh_Width * length;
+                double ptx = gpsTraceLeft[j].x + (gpsTraceRight[j].x - gpsTraceLeft[j].x) / ServiceCarStatus.msysparam.vehWidth * length;
+                double pty = gpsTraceLeft[j].y + (gpsTraceRight[j].y - gpsTraceLeft[j].y) / ServiceCarStatus.msysparam.vehWidth * length;
 
                 int dx = (int)(ptx / gridwide * 2 + (ptx / abs(ptx))) / 2 + centerx;
                 int dy = (int)(pty / gridwide * 2 + (pty / abs(pty))) / 2 + centery;
@@ -780,7 +780,7 @@ iv::Point2D iv::decition::Compute00::getLidarObsPointAvoid(std::vector<Point2D>
 //				double xxx = esrRadars[i].nomal_x + Esr_Offset;
 //				double yyy = esrRadars[i].nomal_y;
 //
-//				if (abs(xxx - gpsTrace[j].x) <= 3.0*Veh_Width / 4.0 && abs(yyy - (gpsTrace[j].y)) <= 1)
+//				if (abs(xxx - gpsTrace[j].x) <= 3.0*ServiceCarStatus.msysparam.vehWidth / 4.0 && abs(yyy - (gpsTrace[j].y)) <= 1)
 //				{
 //
 //					if (lastEsrID == (esrRadars[i]).esr_ID)
@@ -830,13 +830,13 @@ int  iv::decition::Compute00::getEsrIndex(std::vector<Point2D> gpsTrace,int road
                 ODS("\nESR毫米波检测物体Y距离:%f\n", yyy);*/
 
 //优化
-//                if(sqrt((xxx - gpsTrace[j].x)*(xxx - gpsTrace[j].x) + (yyy - gpsTrace[j].y)*(yyy - gpsTrace[j].y)) < (1.0*Veh_Width / 2.0+DecideGps00().xiuzhengCs)){
+//                if(sqrt((xxx - gpsTrace[j].x)*(xxx - gpsTrace[j].x) + (yyy - gpsTrace[j].y)*(yyy - gpsTrace[j].y)) < (1.0*ServiceCarStatus.msysparam.vehWidth / 2.0+DecideGps00().xiuzhengCs)){
 //                    *esrPathpoint = j;
 //                    return i;
 //                }
 
 
-                if (abs(xxx - gpsTrace[j].x) <= (3.0*Veh_Width / 4.0+fxiuzhengCs) && abs(yyy - (gpsTrace[j].y)) <= 1)
+                if (abs(xxx - gpsTrace[j].x) <= (3.0*ServiceCarStatus.msysparam.vehWidth / 4.0+fxiuzhengCs) && abs(yyy - (gpsTrace[j].y)) <= 1)
                 {
 
                     if (lastEsrID == i)
@@ -890,7 +890,7 @@ int  iv::decition::Compute00::getRearEsrIndex(std::vector<Point2D> gpsTrace,int
                 ODS("\nESR毫米波检测物体Y距离:%f\n", yyy);*/
 
 
-                if (abs(xxx - gpsTrace[j].x) <= (3.0*Veh_Width / 4.0+DecideGps00().xiuzhengCs) && abs(yyy - (gpsTrace[j].y)) <= 1)
+                if (abs(xxx - gpsTrace[j].x) <= (3.0*ServiceCarStatus.msysparam.vehWidth / 4.0+DecideGps00().xiuzhengCs) && abs(yyy - (gpsTrace[j].y)) <= 1)
                 {
 
                     if (lastEsrID == i)
@@ -940,7 +940,7 @@ int  iv::decition::Compute00::getRearEsrIndex(std::vector<Point2D> gpsTrace,int
 //                ODS("\nESR毫米波检测物体Y距离:%f\n", yyy);*/
 
 
-//                if (abs(xxx - gpsTrace[j].x) <= 3.0*Veh_Width / 4.0 && abs(yyy - (gpsTrace[j].y)) <= 1)
+//                if (abs(xxx - gpsTrace[j].x) <= 3.0*ServiceCarStatus.msysparam.vehWidth / 4.0 && abs(yyy - (gpsTrace[j].y)) <= 1)
 //                {
 
 //                    if (lastEsrID == i)
@@ -984,7 +984,7 @@ int  iv::decition::Compute00::getEsrIndexAvoid(std::vector<Point2D> gpsTrace) {
                 double xxx = ServiceCarStatus.obs_radar[i].nomal_x + Esr_Offset;
                 double yyy = ServiceCarStatus.obs_radar[i].nomal_y;
 
-                if (abs(xxx - gpsTrace[j].x) <= 3.0*Veh_Width / 4.0 && abs(yyy - (gpsTrace[j].y)) <= 1)
+                if (abs(xxx - gpsTrace[j].x) <= 3.0*ServiceCarStatus.msysparam.vehWidth / 4.0 && abs(yyy - (gpsTrace[j].y)) <= 1)
                 {
 
                     if (lastEsrIDAvoid == i)
@@ -2048,10 +2048,10 @@ int iv::decition::Compute00::getEsrIndexByFrenet(std::vector<Point2D> gpsTrace,
 //            esrObsPoint = iv::decition::FrenetPlanner::XY2Frenet(xxx,yyy,gpsTrace);
             esrObsPoint = iv::decition::FrenetPlanner::getFrenetfromXY(xxx,yyy,gpsTrace,gpsMap,pathpoint,nowGps);
 
-            //如果障碍物与道路的横向距离d<=3.0*Veh_Width / 4.0，则认为道路上有障碍物。
+            //如果障碍物与道路的横向距离d<=3.0*ServiceCarStatus.msysparam.vehWidth / 4.0，则认为道路上有障碍物。
             //s则可理解为障碍物距离。为障碍物与车辆沿着道路方向的距离，而不是空间上的x或y坐标或者直线距离。
             //minDistance、minDis_index用来统计最近的障碍物信息。
-            if(abs(esrObsPoint.d)<=(3.0*Veh_Width / 4.0+DecideGps00().xiuzhengCs)){
+            if(abs(esrObsPoint.d)<=(3.0*ServiceCarStatus.msysparam.vehWidth / 4.0+DecideGps00().xiuzhengCs)){
                 if(esrObsPoint.s<minDistance){
                     minDistance = esrObsPoint.s;
                     minDis_index = i;

src/decition/decition_brain_sf/decition/brain.cpp

@@ -232,6 +232,10 @@ void iv::decition::BrainDecition::run() {
     ServiceCarStatus.msysparam.mstriccid = xp.GetParam("iccid","11111111111111111111");
     ServiceCarStatus.msysparam.mvehtype= xp.GetParam("vehType","ge3");
 
+    ServiceCarStatus.msysparam.vehWidth = atof(xp.GetParam("vehWidth","2.1").data());
+    ServiceCarStatus.msysparam.vehLenth = atof(xp.GetParam("vehLenth","4.6").data());
+
+
 
 
     /*=============     20200113 apollo_fu  add ===========*/

src/decition/decition_brain_sf/decition/decide_gps_00.cpp

@@ -1596,7 +1596,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
                 }
                 else if(useFrenet){
                     if(roadPre != roadNow){
-                        avoidMinDistanceVector[roadNow] = obsDisVector[roadNow] + Veh_Lenth;
+                        avoidMinDistanceVector[roadNow] = obsDisVector[roadNow] + ServiceCarStatus.msysparam.vehLenth;
                         startAvoidGpsInsVector[roadNow] = now_gps_ins;
                     }
                     roadNow = roadPre;
@@ -2302,7 +2302,7 @@ double iv::decition::DecideGps00::limitSpeed(double angle, double speed) {
 
 bool iv::decition::DecideGps00::checkAvoidEnable(double avoidX, iv::LidarGridPtr lidarGridPtr,int roadNum) {
 
-    if ((obsDisVector[roadNum]>0 && obsDisVector[roadNum]<obsDisVector[roadNow]+Veh_Lenth)||
+    if ((obsDisVector[roadNum]>0 && obsDisVector[roadNum]<obsDisVector[roadNow]+ServiceCarStatus.msysparam.vehLenth)||
             (obsDisVector[roadNum]>0 && obsDisVector[roadNum]<15))
     {
         return false;
@@ -2780,10 +2780,10 @@ int iv::decition::DecideGps00::chooseAvoidRoad(iv::LidarGridPtr lidarGridPtr, GP
             {
                 /*if (roadNow==roadOri)
                 {
-                    avoidRunDistance = obsDisVector[roadNow] + Veh_Lenth;
+                    avoidRunDistance = obsDisVector[roadNow] + ServiceCarStatus.msysparam.vehLenth;
                     startAvoid_gps_ins = now_gps_ins;
                 }	*/
-                avoidMinDistanceVector[roadNow]= obsDisVector[roadNow] + Veh_Lenth;
+                avoidMinDistanceVector[roadNow]= obsDisVector[roadNow] + ServiceCarStatus.msysparam.vehLenth;
                 startAvoidGpsInsVector[roadNow] = now_gps_ins;
                 roadPre = roadNow + i;
                 return roadPre;
@@ -2797,10 +2797,10 @@ int iv::decition::DecideGps00::chooseAvoidRoad(iv::LidarGridPtr lidarGridPtr, GP
             {
                 /*if (roadNow == roadOri)
                 {
-                    avoidRunDistance = obsDisVector[roadNow] + Veh_Lenth;
+                    avoidRunDistance = obsDisVector[roadNow] + ServiceCarStatus.msysparam.vehLenth;
                     startAvoid_gps_ins = now_gps_ins;
                 }*/
-                avoidMinDistanceVector[roadNow] = obsDisVector[roadNow] + Veh_Lenth;
+                avoidMinDistanceVector[roadNow] = obsDisVector[roadNow] + ServiceCarStatus.msysparam.vehLenth;
                 startAvoidGpsInsVector[roadNow] = now_gps_ins;
                 roadPre = roadNow - i;
                 return roadPre;

src/decition/decition_brain_sf/decition/obs_predict.cpp

@@ -59,7 +59,7 @@ double iv::decition::getCrashDis(double realSpeed,std::vector<Point2D>  gpsTrace
         double obsY= obs.location.y+obs.velocity.y*time;
         Point2D obsP(obsX,obsY);
         double obsDis= GetDistance(obsP,gpsTrace[i*10]);
-        if(obsDis<0.7*(Veh_Width+obs.size.x) ){
+        if(obsDis<0.7*(ServiceCarStatus.msysparam.vehWidth+obs.size.x) ){
             return dis;
         }