change apollocontroller_shenlanfd. for get /apollo/planning.

src/apollo/code/apollocontroller_shenlanfd/.vscode/c_cpp_properties.json

@@ -0,0 +1,17 @@
+{
+    "configurations": [
+        {
+            "name": "Linux",
+            "includePath": [
+                "${workspaceFolder}/**",
+                "/apollo_workspace/**"
+            ],
+            "defines": [],
+            "compilerPath": "/usr/bin/gcc",
+            "cStandard": "c17",
+            "cppStandard": "gnu++14",
+            "intelliSenseMode": "linux-gcc-arm64"
+        }
+    ],
+    "version": 4
+}

src/apollo/code/apollocontroller_shenlanfd/.vscode/launch.json

@@ -0,0 +1,27 @@
+{
+    "version": "0.2.0",
+    "configurations": [
+      {
+        "name": "Debug Qt (GDB)",
+        "type": "cppdbg",
+        "request": "launch",
+        "program": "${workspaceFolder}/apollocontroller_shenlanfd", //
+        "args": [],
+        "stopAtEntry": false,
+        "cwd": "${workspaceFolder}",
+        "environment": [
+          {"name": "LD_LIBRARY_PATH", "value": "/usr/local/qt5/lib"}  //
+        ],
+        "externalConsole": false,
+        "MIMode": "gdb",
+        "setupCommands": [
+          {
+            "description": "Print",
+            "text": "-enable-pretty-printing",
+            "ignoreFailures": true
+          }
+        ]
+      }
+    ]
+  }
+  

src/apollo/code/apollocontroller_shenlanfd/.vscode/tasks.json

@@ -0,0 +1,26 @@
+{
+	"version": "2.0.0",
+	"tasks": [
+	  {
+		"label": "qmake (Debug)",
+		"type": "shell",
+		"command": "qmake",
+		"args": [
+		  "${workspaceFolder}/apollocontroller_shenlanfd.pro", 
+		  "CONFIG+=debug"  //
+		],
+		"problemMatcher": ["$gcc"],
+		"group": "build"
+	  },
+	  {
+		"label": "make (Debug)",
+		"type": "shell",
+		"command": "make",
+		"args": ["-j4"],  //
+		"dependsOn": ["qmake (Debug)"],
+		"problemMatcher": ["$gcc"],
+		"group": "build"
+	  }
+	]
+}
+  

src/apollo/code/apollocontroller_shenlanfd/main.cpp

@@ -27,6 +27,8 @@ static bool gbthreadrun = true;
 static std::thread * gpthreadsend;
 
 static Shenlanfd * gpShenlanfd;
+static double gfTotalPathLen = 100;
+static double gfNowVel = 1.0; //m/s
 
 
 static std::shared_ptr<apollo::cyber::Writer<iv::can::canmsg>>
@@ -106,10 +108,12 @@ static void ExecSend()
 void threadsend()
 {
     int i;
+    i=0;
 
 
     while(gbthreadrun)
     {
+        i++;
         std::this_thread::sleep_for(std::chrono::milliseconds(10));
         ExecSend();
     }
@@ -119,17 +123,20 @@ void threadsend()
 
 void RecvCANMsg(const std::shared_ptr<iv::can::canmsg> &xmsg)
 {
-    
+
 }
 
 void RecvPlanning(const std::shared_ptr<apollo::planning::ADCTrajectory> & xplanning)
 {
-
+    gfTotalPathLen = xplanning->total_path_length();
+//    std::cout<<"total path length: "<<xplanning->total_path_length()<<std::endl;
+//    std::cout<<"total path time: "<<xplanning->total_path_time()<<std::endl;
 }
 
 void RecvChassis(const std::shared_ptr<iv::chassis> &xchassis)
 {
     
+    gfNowVel = xchassis->vel()/3.6;
     gpShenlanfd->SetSpeed(xchassis->vel());
 
     std::shared_ptr<apollo::canbus::Chassis> apollochassis_ptr;
@@ -147,8 +154,22 @@ void RecvController(const std::shared_ptr<apollo::control::ControlCommand> &xcmd
     double fbrake = xcmd->brake();
     double fwheelangle = 0;
 
+    const double fEndBrakeCtrlLen = 1.0;
+
     facc = xcmd->acceleration();
 
+    if((gfTotalPathLen<fEndBrakeCtrlLen) && (gfTotalPathLen>0.001))
+    {
+        double fcalcacc = 0.0;
+        fcalcacc = (-1.0) * gfNowVel * gfNowVel /(2.0*gfTotalPathLen);
+        if(fcalcacc < (-5.0))fcalcacc = -5.0;
+        if(facc<fcalcacc)
+        {
+            facc = fcalcacc;
+        }
+
+    }
+
         double fVehWeight = 1800;
         double fg = 9.8;
         double fRollForce = 50;
@@ -192,7 +213,7 @@ void RecvController(const std::shared_ptr<apollo::control::ControlCommand> &xcmd
         }
     }
 
-    gpShenlanfd->SetDecision(xdcs);   
+//    gpShenlanfd->SetDecision(xdcs);   
 }
 
 void testthread()