mpc test ok.

src/decition/common/common/car_status.h

@@ -295,6 +295,11 @@ namespace iv {
 
         double mfWheelAngle_feedback;  //从底盘获得的当前方向盘角度
 
+        bool mbUseMPC = false;   //是否使用MPC
+        double mfWheelBase;   //轴距
+        double mfWheelRatio  = 13.6; //方向盘角度和轮胎角度的比例
+
+
     };
     typedef boost::serialization::singleton<iv::CarStatus> CarStatusSingleton;//非线程安全，注意多线程加锁，单例模式
 }

src/decition/decition_brain_sf_changan_shenlan/decition/adc_controller/pid_controller.cpp

@@ -1,6 +1,6 @@
 #include <decition/adc_controller/pid_controller.h>
 #include <common/constants.h>
-#include <common/car_status.h>
+
 #include <math.h>
 #include <iostream>
 #include <fstream>
@@ -20,6 +20,7 @@ iv::decition::PIDController::PIDController(){
 
 #ifdef USEAUTOWAREMPC
     mmpcif = std::shared_ptr<MPCIf>(new MPCIf());
+
 #endif
 }
 iv::decition::PIDController::~PIDController(){
@@ -119,13 +120,20 @@ MPCTrajectory iv::decition::PIDController::ConvertPointToMPCTrajectory(std::vect
 //    y = 0;
  //   traj.push_back(x,y,0,0,vel,0,0,0);
 
-traj.push_back(-1.0,0.0,0.0,0.0,1.0,0,0,0);
+//traj.push_back(-1.0,0.0,0.0,0.0,1.0,0,0,0);
+
+//    traj.push_back(0.5,0.0,0.0,0.0,1.0,0,0,0);
+
+    if(trace.size()== 0)
+        traj.push_back(0.0,0.0,0.0,0.0,1.0,0,0,0);
+    else
+        traj.push_back(0.0,trace[0].x *(-1.0),0.0,0.0,1.0,0,0,0);
+
 
-    traj.push_back(0.5,0.0,0.0,0.0,1.0,0,0,0);
     for(i=0;i<nsize;i++)
     {
 
-        vel = 1.0;
+        vel = 10.0/3.6;
         x = trace[i].y;
         y = trace[i].x * (-1.0);
         if((traj.size() == 0))
@@ -134,7 +142,7 @@ traj.push_back(-1.0,0.0,0.0,0.0,1.0,0,0,0);
         }
         else
         {
-            if(((x - traj.x[traj.size() -1])>=1.0) &&(traj.size()<30))
+            if(((x - traj.x[traj.size() -1])>=1.0) &&(traj.size()<1000))
             {
                 traj.push_back(x,y,0,0,vel,0,0,0);
             }
@@ -143,6 +151,14 @@ traj.push_back(-1.0,0.0,0.0,0.0,1.0,0,0,0);
     }
 
 
+    if(traj.size() < 2)
+    {
+        traj.push_back(0.5,0.0,0.0,0.0,1.0,0,0,0);
+    }
+
+
+
+
 //    traj.clear();
 //    traj.push_back(0,0.0,0.0,0.0,1.0,0,0,0);
 //    traj.push_back(1,0.1,0.0,0.0,1.0,0,0,0);
@@ -155,12 +171,16 @@ traj.push_back(-1.0,0.0,0.0,0.0,1.0,0,0,0);
 
 #endif
 
+
+//realSpeed km/h
 float iv::decition::PIDController::getPidAngle(float realSpeed, std::vector<Point2D> trace, int roadMode){
 
     double ang = 0;
 
+//    std::cout<<"realSpeed: "<<realSpeed<<std::endl;
+
 #ifdef USEAUTOWAREMPC
-    bool bUseMPC = true;
+    bool bUseMPC = ServiceCarStatus.mbUseMPC;
 
     if(bUseMPC)
     {
@@ -173,18 +193,19 @@ float iv::decition::PIDController::getPidAngle(float realSpeed, std::vector<Poin
         ADCAckermannLateralCommand ctrl_cmd;
         MPCTrajectory pred_traj;
         iv::ADCSteeringReport steerrep;
-        steerrep.steering_tire_angle = (ServiceCarStatus.mfWheelAngle_feedback/13.6) * M_PI/180.0;
-        bool bcalc = mmpcif->calculateMPC(steerrep, ctrl_cmd, pred_traj);
+        steerrep.steering_tire_angle = (ServiceCarStatus.mfWheelAngle_feedback/ServiceCarStatus.mfWheelRatio) * M_PI/180.0;
+        bool bcalc = mmpcif->calculateMPC(steerrep, realSpeed/3.6, ctrl_cmd, pred_traj);
 
         std::cout<<" mpc calc : "<<bcalc<<" wheel: "<<ctrl_cmd.steering_tire_angle<<std::endl;
 
-        ang =  ctrl_cmd.steering_tire_angle * 13.6 * 180.0/M_PI;
+        ang =  (-1.0) * ctrl_cmd.steering_tire_angle * ServiceCarStatus.mfWheelRatio * 180.0/M_PI;
+
 
         if(bcalc)
         {
-            int j;
-            for(j=0;j<1000;j++)std::cout<<"mpc ok.  "<<std::endl;
+            std::cout<<"mpc ok.  use mpc"<<std::endl;
             return ang;
+
         }
 
     }

src/decition/decition_brain_sf_changan_shenlan/decition/adc_controller/pid_controller.h

@@ -9,6 +9,8 @@
 #include <decition/adc_controller/base_controller.h>
 #include <decition/adc_tools/transfer.h>
 
+#include <common/car_status.h>
+
 #ifdef USEAUTOWAREMPC
 #include "mpc_lateral_controller/mpc_trajectory.hpp"
 #include "mpc_lateral_controller/mpc.hpp"
@@ -34,7 +36,7 @@ public:
     //   rclcpp::Logger logger = rclcpp::get_logger("mpc_test_logger");
 
     // Vehicle model parameters
-    double wheelbase = 2.7;
+    double wheelbase = 2.9;
     double steer_limit = 1.0;
     double steer_tau = 0.1;
     double mass_fl = 600.0;
@@ -46,13 +48,13 @@ public:
 
     // Filters parameter
     double steering_lpf_cutoff_hz = 3.0;
-    double error_deriv_lpf_cutoff_hz = 5.0;
+    double error_deriv_lpf_cutoff_hz =  15.0;
 
     // Test Parameters
-    double admissible_position_error = 5.0;
+    double admissible_position_error = 15.0;//5.0;
     double admissible_yaw_error_rad = M_PI_2;
     double steer_lim = 0.610865;      // 35 degrees
-    double steer_rate_lim = 2.61799;  // 150 degrees
+    double steer_rate_lim = 5.3; //2.61799;  // 150 degrees
     double ctrl_period = 0.03;
 
     bool use_steer_prediction = true;
@@ -61,6 +63,7 @@ public:
 
     MPC mmpc;
 
+
     void SetUp()
     {
         param.prediction_horizon = 50;
@@ -98,6 +101,12 @@ public:
         trajectory_param.enable_path_smoothing = true;
         trajectory_param.extend_trajectory_for_end_yaw_control = true;
 
+        dummy_straight_trajectory.push_back(0,0.0,0.0,0.0,1.0,0,0,0);
+        dummy_straight_trajectory.push_back(1,0.1,0.0,0.0,1.0,0,0,0);
+        dummy_straight_trajectory.push_back(2,0.2,0.0,0.0,1.0,0,0,0);
+        dummy_straight_trajectory.push_back(3,0.3,0.0,0.0,1.0,0,0,0);
+        dummy_straight_trajectory.push_back(4,0.4,0.0,0.0,1.0,0,0,0);
+
 
 
         neutral_steer.steering_tire_angle = 0.0;
@@ -126,6 +135,7 @@ public:
     {
         SetUp();
 
+        wheelbase = ServiceCarStatus.mfWheelBase;
         std::shared_ptr<VehicleModelInterface> vehicle_model_ptr =
             std::make_shared<KinematicsBicycleModel>(wheelbase, steer_limit, steer_tau);
         mmpc.setVehicleModel(vehicle_model_ptr);
@@ -137,6 +147,23 @@ public:
 
         std::cout<<" haveVehicleModel : "<<mmpc.hasVehicleModel()<<std::endl;
         std::cout<<" haveQPSolver : "<<mmpc.hasQPSolver()<<std::endl;
+
+
+        const auto current_kinematics =
+            makeOdometry(pose_zero, 0.0);
+        mmpc.setReferenceTrajectory(dummy_straight_trajectory, trajectory_param, current_kinematics);
+
+        // Calculate MPC
+        ADCAckermannLateralCommand ctrl_cmd;
+        MPCTrajectory pred_traj;
+        const auto odom = makeOdometry(pose_zero, default_velocity);
+        bool bcalc = mmpc.calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj);
+
+
+        if(bcalc)
+        {
+            std::cout<<"test ok."<<std::endl;
+        }
     }
 
     iv::ADCOdometry makeOdometry(const iv::ADCPose & pose, const double velocity)
@@ -152,14 +179,14 @@ public:
     {
         // Init trajectory
         const auto current_kinematics =
-            makeOdometry(pose_zero, 0.0);
+            makeOdometry(pose_zero, 1.0);
         mmpc.setReferenceTrajectory(traj, trajectory_param, current_kinematics);
     }
 
-    bool calculateMPC(const iv::ADCSteeringReport & current_steer,
+    bool calculateMPC(const iv::ADCSteeringReport & current_steer,double fvel,
                       ADCAckermannLateralCommand & ctrl_cmd, MPCTrajectory & predicted_trajectory)
     {
-        const auto odom = makeOdometry(pose_zero, default_velocity);
+        const auto odom = makeOdometry(pose_zero, fvel);
         bool bcalc = mmpc.calculateMPC(current_steer, odom, ctrl_cmd, predicted_trajectory);
         return bcalc;
     }

src/decition/decition_brain_sf_changan_shenlan/decition/brain.cpp

@@ -436,6 +436,10 @@ void iv::decition::BrainDecition::run() {
     if(ServiceCarStatus.mfChassisMaxBrake < 0.1)ServiceCarStatus.mfChassisMaxBrake = 0.1;
     ServiceCarStatus.mfMaxSpeed = sqrt(2.0 * ServiceCarStatus.mfChassisMaxBrake * (ServiceCarStatus.mfPerceptionMax - 5.0)) * 3.6;
 
+    ServiceCarStatus.mbUseMPC = xp.GetParam("UseMPC",false);
+    ServiceCarStatus.mfWheelBase = xp.GetParam("WheelBase",2.9);
+    ServiceCarStatus.mfWheelRatio = xp.GetParam("WheelRatio",13.6);
+
     if((ServiceCarStatus.msysparam.mvehtype=="shenlan") &&(ServiceCarStatus.mbLimitSpeed))
     {
         ServiceCarStatus.mfMaxSpeed = 20.0;

src/decition/mpccontroller/mpc_utils.cpp

@@ -79,7 +79,7 @@ iv::ADCQuaternion ADCcreateQuaternionFromYaw(double yaw) // yaw (Z), pitch (Y),
 }
 
 
-inline static double ADCnormalizeDegree(const double deg, const double min_deg = -180)
+static double ADCnormalizeDegree(const double deg, const double min_deg = -180)
 {
   const auto max_deg = min_deg + 360.0;
 
@@ -91,7 +91,7 @@ inline static double ADCnormalizeDegree(const double deg, const double min_deg =
   return value - std::copysign(360.0, value);
 }
 
-inline  double ADCnormalizeRadian(const double rad, const double min_rad)
+double ADCnormalizeRadian(const double rad, const double min_rad)
 {
   const auto max_rad = min_rad + 2 * M_PI;
 
@@ -103,7 +103,7 @@ inline  double ADCnormalizeRadian(const double rad, const double min_rad)
   return value - std::copysign(2 * M_PI, value);
 }
 
-inline  double ADCcalcDistance2d(iv::ADCPoint p1, iv::ADCPoint p2)
+double ADCcalcDistance2d(iv::ADCPoint p1, iv::ADCPoint p2)
 {
   return sqrt(pow(p2.x - p1.x,2)+pow(p2.y - p1.y,2));
 }

src/decition/mpccontroller/mpctest.cpp

@@ -1,5 +1,8 @@
 #include "mpctest.h"
 
+
+#include <thread>
+
 MPCTest::MPCTest()
 {
     SetUp();
@@ -22,12 +25,31 @@ void MPCTest::InitializeAndCalculate()
     // Init parameters and reference trajectory
     initializeMPC(mpc);
 
+
+    std::this_thread::sleep_for(std::chrono::milliseconds(100));
+    const auto current_kinematics =
+        makeOdometry(pose_zero, 0.0);
+    mpc.setReferenceTrajectory(dummy_straight_trajectory, trajectory_param, current_kinematics);
+
     // Calculate MPC
     ADCAckermannLateralCommand ctrl_cmd;
     MPCTrajectory pred_traj;
     const auto odom = makeOdometry(pose_zero, default_velocity);
     bool bcalc = mpc.calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj);
 
+    std::this_thread::sleep_for(std::chrono::milliseconds(100));
+    mpc.setReferenceTrajectory(dummy_straight_trajectory, trajectory_param, current_kinematics);
+    bcalc = mpc.calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj);
+
+    std::this_thread::sleep_for(std::chrono::milliseconds(100));
+    mpc.setReferenceTrajectory(dummy_straight_trajectory, trajectory_param, current_kinematics);
+    bcalc = mpc.calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj);
+
+
+    std::this_thread::sleep_for(std::chrono::milliseconds(100));
+    mpc.setReferenceTrajectory(dummy_straight_trajectory, trajectory_param, current_kinematics);
+    bcalc = mpc.calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj);
+
     if(bcalc)
     {
         std::cout<<" tire: "<<ctrl_cmd.steering_tire_angle<<std::endl;

src/decition/mpccontroller/mpctest.h

@@ -98,11 +98,17 @@ protected:
         trajectory_param.enable_path_smoothing = true;
         trajectory_param.extend_trajectory_for_end_yaw_control = true;
 
+//        dummy_straight_trajectory.push_back(0,0.0,0.0,0.0,1.0,0,0,0);
+//        dummy_straight_trajectory.push_back(1,0.0,0.0,0.0,1.0,0,0,0);
+//        dummy_straight_trajectory.push_back(2,0.0,0.0,0.0,1.0,0,0,0);
+//        dummy_straight_trajectory.push_back(3,0.0,0.0,0.0,1.0,0,0,0);
+//        dummy_straight_trajectory.push_back(36,30.9,0.0,0.0,1.0,0,0,0);
+
         dummy_straight_trajectory.push_back(0,0.0,0.0,0.0,1.0,0,0,0);
         dummy_straight_trajectory.push_back(1,0.1,0.0,0.0,1.0,0,0,0);
-        dummy_straight_trajectory.push_back(2,0.3,0.0,0.0,1.0,0,0,0);
-        dummy_straight_trajectory.push_back(3,0.6,0.0,0.0,1.0,0,0,0);
-        dummy_straight_trajectory.push_back(4,0.9,0.0,0.0,1.0,0,0,0);
+        dummy_straight_trajectory.push_back(2,0.2,0.0,0.0,1.0,0,0,0);
+        dummy_straight_trajectory.push_back(3,0.3,0.0,0.0,1.0,0,0,0);
+        dummy_straight_trajectory.push_back(4,0.4,0.0,0.0,1.0,0,0,0);
 
 
         dummy_right_turn_trajectory.push_back(-1.0,-1.0,0.0,0.0,1.0,0,0,0);
@@ -141,10 +147,11 @@ protected:
         mpc.initializeSteeringPredictor();
 
 
+//        const auto current_kinematics =
+//            makeOdometry(pose_zero, 0.0);
+//        mpc.setReferenceTrajectory(dummy_straight_trajectory, trajectory_param, current_kinematics);
         // Init trajectory
-        const auto current_kinematics =
-            makeOdometry(pose_zero, 0.0);
-        mpc.setReferenceTrajectory(dummy_straight_trajectory, trajectory_param, current_kinematics);
+
     }
 
     iv::ADCOdometry makeOdometry(const iv::ADCPose & pose, const double velocity)