HAPO brain control door

sh/HAPO/agx1/driver_rpc_client.yaml

@@ -1,9 +1,31 @@
 server : 192.168.1.103:10001
 message:
   - hcp2_gpsimu 
+  - ultra
+  - corner_radar_right
+  - corner_radar_left
+  - canrecv1 
 hcp2_gpsimu:
   msgname: hcp2_gpsimu 
   buffersize: 10000
   buffercount: 3
+ultra:
+  msgname: ultra
+  buffersize: 10000
+  buffercount: 3
+
+
+corner_radar_right:
+  msgname: corner_radar_right
+  buffersize: 10000
+  buffercount: 3
+corner_radar_left:
+  msgname: corner_radar_left
+  buffersize: 10000
+  buffercount: 3
+canrecv1:
+  msgname: canrecv1
+  buffersize: 10000
+  buffercount: 3
 
 

src/controller/controller_hapo/main.cpp

@@ -94,11 +94,14 @@ void executeDecition(const iv::brain::decition decition)
     gcontroller->control_home_light(decition.home_light());
     gcontroller->control_air_worktime(decition.air_worktime());
     gcontroller->control_air_offtime(decition.air_offtime());
-
     if(decition.door())
-        gcontroller->control_door(2);
-    else
-        gcontroller->control_door(3);
+    {
+    gcontroller->control_door(2);
+    }else{
+      gcontroller->control_door(3);
+    }
+
+
 
 
 

src/decition/decition_brain/decition/adc_adapter/bus_adapter.cpp

@@ -264,7 +264,7 @@ iv::decition::Decition iv::decition::BusAdapter::getAdapterDeciton(GPS_INS now_g
 
 
 
-       (*decition)->door=3;
+       //(*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_adapter/hapo_adapter.cpp

@@ -17,7 +17,8 @@ iv::decition::HapoAdapter::~HapoAdapter(){
 
 }
 
-
+#include "ivlog.h"
+extern iv::Ivlog * givlog;
 
 iv::decition::Decition iv::decition::HapoAdapter::getAdapterDeciton(GPS_INS now_gps_ins, std::vector<Point2D>  trace , float dSpeed, float obsDistance ,
                                                                         float  obsSpeed,float accAim,float accNow  ,bool changingDangWei,Decition *decition){
@@ -266,21 +267,24 @@ iv::decition::Decition iv::decition::HapoAdapter::getAdapterDeciton(GPS_INS now_
 
      if(ServiceCarStatus.stationCmd.mode_manual_drive==true)
      {
-          (*decition)->mode=0;       //云平台控制切手动驾驶
+          (*decition)->mode=0;
      }
 
 
      if(ServiceCarStatus.station_control_door==0)
      {
-         (*decition)->door=false;       //关门
+         (*decition)->door=false;      //CLOSE
+       givlog->debug("DOOR","STATUS is: %d",5);
      }
      else if((ServiceCarStatus.station_control_door==1)&&(ServiceCarStatus.station_control_door_option==false))
      {
-         (*decition)->door=true;       //到达站点开门
+         (*decition)->door=true;       //OPEN
          ServiceCarStatus.station_control_door_option=true;
+         givlog->debug("DOOR","STATUS is: %d",8);
      }
 
-
+givlog->debug("DOOR","door is: %d",(*decition)->door);
+givlog->debug("DOOR","station_control_door is: %d",ServiceCarStatus.station_control_door);
 
 std::cout<<"==========================================="<<std::endl;
      std::cout<<"dSpeed:"<<dSpeed<<"   realSpeed:"<<realSpeed<<"   acc:"<<accAim<<"  torque_st:"<<ServiceCarStatus.torque_st

src/decition/decition_brain/decition/adc_adapter/qingyuan_adapter.cpp

@@ -249,7 +249,7 @@ iv::decition::Decition iv::decition::QingYuanAdapter::getAdapterDeciton(GPS_INS
 
 
 
-       (*decition)->door=3;
+       //(*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_tools/compute_00.cpp

@@ -60,12 +60,13 @@ int iv::decition::Compute00::getDesiredSpeed(std::vector<GPSData> gpsMap)
     for (int j = startIndex; j < endIndex; j++)
     {
         int i = (j + gpsMap.size()) % gpsMap.size();
-        (*gpsMap[i]).roadMode=5;
+        if((*gpsMap[i]).roadMode!=6)
+                (*gpsMap[i]).roadMode=5;
     }
-    for (int j = startIndex; j < (endIndex-10); j+=10)
+    for (int j = startIndex; j < (endIndex-40); j+=40)
     {
         int i = (j + gpsMap.size()) % gpsMap.size();
-        for(int front_k=i;front_k<i+5;front_k++)
+        for(int front_k=i;front_k<i+20;front_k++)
         {
             if(fabs(((*gpsMap[front_k]).ins_heading_angle)-((*gpsMap[i]).ins_heading_angle))<10)
             {
@@ -74,11 +75,11 @@ int iv::decition::Compute00::getDesiredSpeed(std::vector<GPSData> gpsMap)
             }
 
         }
-        i+=5;
+        i+=20;
         FrontAveFive=FrontTotalFive/FrontCount;
         FrontTotalFive=0;
         FrontCount=0;
-        for(int back_k=i;back_k<i+5;back_k++)
+        for(int back_k=i;back_k<i+20;back_k++)
         {
             if(fabs((*gpsMap[back_k]).ins_heading_angle-(*gpsMap[i]).ins_heading_angle)<10)
             {
@@ -87,14 +88,14 @@ int iv::decition::Compute00::getDesiredSpeed(std::vector<GPSData> gpsMap)
             }
 
         }
-        i+=5;
+        i+=20;
         CurrentIndex=i-1;
         BackAveFive=BackTotalFive/BackCount;
         BackTotalFive=0;
         BackCount=0;
         if(fabs(FrontAveFive-BackAveFive)<50)
         {
-                   if(fabs(FrontAveFive-BackAveFive)>3)
+                   if(fabs(FrontAveFive-BackAveFive)>4)
                    {
                         CurveContinue+=1;
                         if(CurveContinue>=2)
@@ -102,14 +103,15 @@ int iv::decition::Compute00::getDesiredSpeed(std::vector<GPSData> gpsMap)
                             MarkingCount=0;
                             for(MarkingIndex=CurrentIndex;MarkingIndex>=MarkedIndex;MarkingIndex--)
                             {
-
+                                if((*gpsMap[MarkingIndex]).roadMode!=6)
+                                {
                                 if(MarkingCount<100)
                                 {
-                                     if((BackAveFive-FrontAveFive)<3)
+                                     if((BackAveFive-FrontAveFive)<4)
                                      {
                                            (*gpsMap[MarkingIndex]).roadMode=14;   //弯道，2米,14
                                      }
-                                     else if((BackAveFive-FrontAveFive)>3)
+                                     else if((BackAveFive-FrontAveFive)>4)
                                      {
                                            (*gpsMap[MarkingIndex]).roadMode=15;
                                      }
@@ -131,13 +133,14 @@ int iv::decition::Compute00::getDesiredSpeed(std::vector<GPSData> gpsMap)
                                 {
                                      (*gpsMap[MarkingIndex]).roadMode=11;   //高速/疯狂加速，大于60米
                                 }
+                                }
 
                                 MarkingCount++;
                             }
                             MarkedIndex=CurrentIndex;
                         }
                    }
-                   else if(fabs(FrontAveFive-BackAveFive)<=3)
+                   else if(fabs(FrontAveFive-BackAveFive)<=4)
                    {
                         CurveContinue=0;
                    }
@@ -151,7 +154,8 @@ int iv::decition::Compute00::getDesiredSpeed(std::vector<GPSData> gpsMap)
         MarkingCount=0;
         for(MarkingIndex=endIndex;MarkingIndex>=MarkedIndex;MarkingIndex--)
         {
-
+            if((*gpsMap[MarkingIndex]).roadMode!=6)
+            {
             if(MarkingCount<100)
             {
                 if((BackAveFive-FrontAveFive)<3)
@@ -179,7 +183,7 @@ int iv::decition::Compute00::getDesiredSpeed(std::vector<GPSData> gpsMap)
             {
                  (*gpsMap[MarkingIndex]).roadMode=11;   //高速/疯狂加速，大于60米
             }
-
+             }
             MarkingCount++;
         }
     }

src/decition/decition_brain/decition/decide_gps_00.cpp

@@ -1846,7 +1846,7 @@ 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))
+        if((pt.y<5)&&(abs(realspeed)<0.1)&&(ServiceCarStatus.station_control_door==0))
         {
                 ServiceCarStatus.station_control_door=1;   //open door
         }

src/v2x/v2xTcpClient/mainwindow.cpp

@@ -131,7 +131,7 @@ void MainWindow::upDataStream()
 {
 //    upVehicleStatus();
 //    upHardwareStatus();
-//   upObstacleDataStatus();
+//    upObstacleDataStatus();
     upSoftwareStatus();
 }
 void MainWindow::socket_Read_Data()