change decition_brain_changanshenlan for add mpc.not test ok.

src/decition/common/common/car_status.h

@@ -293,6 +293,8 @@ namespace iv {
 
         double tlStopDis = 5.0;
 
+        double mfWheelAngle_feedback;  //从底盘获得的当前方向盘角度
+
     };
     typedef boost::serialization::singleton<iv::CarStatus> CarStatusSingleton;//非线程安全，注意多线程加锁，单例模式
 }

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

@@ -6,12 +6,21 @@
 #include <fstream>
 
 
+
+
+
 using namespace std;
 
 
+
+
 iv::decition::PIDController::PIDController(){
     controller_id = 0;
     controller_name="pid";
+
+#ifdef USEAUTOWAREMPC
+    mmpcif = std::shared_ptr<MPCIf>(new MPCIf());
+#endif
 }
 iv::decition::PIDController::~PIDController(){
 
@@ -96,8 +105,91 @@ double iv::decition::PIDController::ExtendPreviewDistance(std::vector<Point2D> &
 
 }
 
+#ifdef USEAUTOWAREMPC
+MPCTrajectory iv::decition::PIDController::ConvertPointToMPCTrajectory(std::vector<Point2D> & trace)
+{
+    MPCTrajectory traj;
+
+    unsigned int nsize = trace.size();
+    unsigned int i;
+
+    double x,y,vel;
+//    vel = 3.6;
+//    x = 0;
+//    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(0.5,0.0,0.0,0.0,1.0,0,0,0);
+    for(i=0;i<nsize;i++)
+    {
+
+        vel = 1.0;
+        x = trace[i].y;
+        y = trace[i].x * (-1.0);
+        if((traj.size() == 0))
+        {
+            if(x>1)traj.push_back(x,y,0,0,vel,0,0,0);
+        }
+        else
+        {
+            if(((x - traj.x[traj.size() -1])>=1.0) &&(traj.size()<30))
+            {
+                traj.push_back(x,y,0,0,vel,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);
+//    traj.push_back(2,0.2,0.0,0.0,1.0,0,0,0);
+//    traj.push_back(3,0.3,0.0,0.0,1.0,0,0,0);
+//    traj.push_back(4,0.4,0.0,0.0,1.0,0,0,0);
+
+    return traj;
+}
+
+#endif
+
 float iv::decition::PIDController::getPidAngle(float realSpeed, std::vector<Point2D> trace, int roadMode){
+
     double ang = 0;
+
+#ifdef USEAUTOWAREMPC
+    bool bUseMPC = true;
+
+    if(bUseMPC)
+    {
+        MPCTrajectory traj = ConvertPointToMPCTrajectory(trace);
+        mmpcif->setReferenceTrajectory(traj);
+
+
+
+        // Calculate MPC
+        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);
+
+        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;
+
+        if(bcalc)
+        {
+            int j;
+            for(j=0;j<1000;j++)std::cout<<"mpc ok.  "<<std::endl;
+            return ang;
+        }
+
+    }
+#endif
+
     double EPos = 0, EAng = 0;
 
     double KEang = 14, KEPos = 10, DEang = 0, DEPos = 0,IEpos=0,IEang=0;

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

@@ -9,6 +9,167 @@
 #include <decition/adc_controller/base_controller.h>
 #include <decition/adc_tools/transfer.h>
 
+#ifdef USEAUTOWAREMPC
+#include "mpc_lateral_controller/mpc_trajectory.hpp"
+#include "mpc_lateral_controller/mpc.hpp"
+#include "mpc_lateral_controller/qp_solver/qp_solver_osqp.hpp"
+#include "mpc_lateral_controller/qp_solver/qp_solver_unconstraint_fast.hpp"
+#include "mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_dynamics.hpp"
+#include "mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp"
+#include "mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp"
+
+
+using namespace autoware::motion::control::mpc_lateral_controller;
+
+class MPCIf
+{
+public:
+    MPCParam param;
+    // Test inputs
+    MPCTrajectory dummy_straight_trajectory;
+    MPCTrajectory dummy_right_turn_trajectory;
+    iv::ADCSteeringReport neutral_steer;
+    iv::ADCPose pose_zero;
+    double default_velocity = 1.0;
+    //   rclcpp::Logger logger = rclcpp::get_logger("mpc_test_logger");
+
+    // Vehicle model parameters
+    double wheelbase = 2.7;
+    double steer_limit = 1.0;
+    double steer_tau = 0.1;
+    double mass_fl = 600.0;
+    double mass_fr = 600.0;
+    double mass_rl = 600.0;
+    double mass_rr = 600.0;
+    double cf = 155494.663;
+    double cr = 155494.663;
+
+    // Filters parameter
+    double steering_lpf_cutoff_hz = 3.0;
+    double error_deriv_lpf_cutoff_hz = 5.0;
+
+    // Test Parameters
+    double admissible_position_error = 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 ctrl_period = 0.03;
+
+    bool use_steer_prediction = true;
+
+    TrajectoryFilteringParam trajectory_param;
+
+    MPC mmpc;
+
+    void SetUp()
+    {
+        param.prediction_horizon = 50;
+        param.prediction_dt = 0.1;
+        param.zero_ff_steer_deg = 0.5;
+        param.input_delay = 0.0;
+        param.acceleration_limit = 2.0;
+        param.velocity_time_constant = 0.3;
+        param.min_prediction_length = 5.0;
+        param.steer_tau = 0.1;
+        param.nominal_weight.lat_error = 1.0;
+        param.nominal_weight.heading_error = 1.0;
+        param.nominal_weight.heading_error_squared_vel = 1.0;
+        param.nominal_weight.terminal_lat_error = 1.0;
+        param.nominal_weight.terminal_heading_error = 0.1;
+        param.low_curvature_weight.lat_error = 0.1;
+        param.low_curvature_weight.heading_error = 0.0;
+        param.low_curvature_weight.heading_error_squared_vel = 0.3;
+        param.nominal_weight.steering_input = 1.0;
+        param.nominal_weight.steering_input_squared_vel = 0.25;
+        param.nominal_weight.lat_jerk = 0.0;
+        param.nominal_weight.steer_rate = 0.0;
+        param.nominal_weight.steer_acc = 0.000001;
+        param.low_curvature_weight.steering_input = 1.0;
+        param.low_curvature_weight.steering_input_squared_vel = 0.25;
+        param.low_curvature_weight.lat_jerk = 0.0;
+        param.low_curvature_weight.steer_rate = 0.0;
+        param.low_curvature_weight.steer_acc = 0.000001;
+        param.low_curvature_thresh_curvature = 0.0;
+
+        trajectory_param.traj_resample_dist = 0.1;
+        trajectory_param.path_filter_moving_ave_num = 35;
+        trajectory_param.curvature_smoothing_num_traj = 1;
+        trajectory_param.curvature_smoothing_num_ref_steer = 35;
+        trajectory_param.enable_path_smoothing = true;
+        trajectory_param.extend_trajectory_for_end_yaw_control = true;
+
+
+
+        neutral_steer.steering_tire_angle = 0.0;
+        pose_zero.position.x = 0.0;
+        pose_zero.position.y = 0.0;
+    }
+
+    void initializeMPC(MPC & mpc)
+    {
+        mpc.m_param = param;
+        mpc.m_admissible_position_error = admissible_position_error;
+        mpc.m_admissible_yaw_error_rad = admissible_yaw_error_rad;
+        mpc.m_steer_lim = steer_lim;
+        mpc.m_steer_rate_lim_map_by_curvature.emplace_back(0.0, steer_rate_lim);
+        mpc.m_steer_rate_lim_map_by_velocity.emplace_back(0.0, steer_rate_lim);
+        mpc.m_ctrl_period = ctrl_period;
+        mpc.m_use_steer_prediction = use_steer_prediction;
+
+        mpc.initializeLowPassFilters(steering_lpf_cutoff_hz, error_deriv_lpf_cutoff_hz);
+        mpc.initializeSteeringPredictor();
+
+    }
+
+
+    MPCIf()
+    {
+        SetUp();
+
+        std::shared_ptr<VehicleModelInterface> vehicle_model_ptr =
+            std::make_shared<KinematicsBicycleModel>(wheelbase, steer_limit, steer_tau);
+        mmpc.setVehicleModel(vehicle_model_ptr);
+
+        std::shared_ptr<QPSolverInterface> qpsolver_ptr = std::make_shared<QPSolverEigenLeastSquareLLT>();
+        mmpc.setQPSolver(qpsolver_ptr);
+
+        initializeMPC(mmpc);
+
+        std::cout<<" haveVehicleModel : "<<mmpc.hasVehicleModel()<<std::endl;
+        std::cout<<" haveQPSolver : "<<mmpc.hasQPSolver()<<std::endl;
+    }
+
+    iv::ADCOdometry makeOdometry(const iv::ADCPose & pose, const double velocity)
+    {
+        iv::ADCOdometry odometry;
+        odometry.pose = pose;
+        odometry.twist.linear_x = velocity;
+        return odometry;
+    }
+
+
+    void setReferenceTrajectory(MPCTrajectory & traj)
+    {
+        // Init trajectory
+        const auto current_kinematics =
+            makeOdometry(pose_zero, 0.0);
+        mmpc.setReferenceTrajectory(traj, trajectory_param, current_kinematics);
+    }
+
+    bool calculateMPC(const iv::ADCSteeringReport & current_steer,
+                      ADCAckermannLateralCommand & ctrl_cmd, MPCTrajectory & predicted_trajectory)
+    {
+        const auto odom = makeOdometry(pose_zero, default_velocity);
+        bool bcalc = mmpc.calculateMPC(current_steer, odom, ctrl_cmd, predicted_trajectory);
+        return bcalc;
+    }
+
+
+
+};
+
+#endif
+
 namespace iv {
         namespace decition {
         //根据传感器传输来的信息作出决策
@@ -57,6 +218,12 @@ namespace iv {
                         double CalcKappa(std::vector<Point2D> trace,int preindex);
 
 
+#ifdef USEAUTOWAREMPC
+                        std::shared_ptr<MPCIf> mmpcif;
+                        MPCTrajectory ConvertPointToMPCTrajectory(std::vector<Point2D> & trace);
+#endif
+
+
                     private:
 
                         double ExtendPreviewDistance(std::vector<Point2D> & trace,double & PreviewDistance);

src/decition/decition_brain_sf_changan_shenlan/decition/brain.cpp

@@ -1210,6 +1210,12 @@ void iv::decition::BrainDecition::UpdateChassis(const char *strdata, const unsig
             ServiceCarStatus.mbRunPause = true;
         }
     }
+
+    if(xchassis.has_angle_feedback())
+    {
+        ServiceCarStatus.mfWheelAngle_feedback = xchassis.angle_feedback();
+    }
+
     if(xchassis.has_torque())
     {
         ServiceCarStatus.torque_st = xchassis.torque();

src/decition/decition_brain_sf_changan_shenlan/decition_brain_sf_changan_shenlan.pro

@@ -106,4 +106,18 @@ HEADERS += \
     ../../include/msgtype/groupmsg.pb.h \
     ../../include/msgtype/brainrunstate.pb.h
 
+#DEFINES += USEAUTOWAREMPC
+
+
+if(contains(DEFINES,USEAUTOWAREMPC)){
+
+INCLUDEPATH += $$PWD/../../common/include
+INCLUDEPATH += $$PWD/../mpccontroller
+
+LIBS += -linterpolation -losqp -lmpccontroller
+
+}
+
+
+
 

src/include/proto/chassis.proto

@@ -13,7 +13,7 @@ message chassis
   optional int32 CDDAvailable = 7;
   optional int32 angle_feedback = 8;
   optional float torque = 9;
-  optional float vel = 10;
+  optional float vel = 10;   //km/h
   optional float accstep = 11;
   optional float soc = 12;
   optional float brake_feedback = 13;

src/tool/simple_planning_simulator/simmodel.cpp

@@ -15,6 +15,7 @@ simmodel::simmodel()
 
     mpa = iv::modulecomm::RegisterSend("hcp2_gpsimu",100000,1);
     mpatrack = iv::modulecomm::RegisterSend("lidar_track",1000000,1);
+    mpachassis = iv::modulecomm::RegisterSend("chassis",10000,1);
 
     mpthread = new std::thread(&simmodel::threadupdate,this);
     mpthreadstate = new std::thread(&simmodel::threadstate,this);
@@ -283,6 +284,26 @@ void simmodel::threadstate()
         //   char * strout = lidarobjtostr(lidarobjvec,ntlen);
         iv::modulecomm::ModuleSendMsg(mpatrack,out.data(),out.length());
 
+        iv::chassis xchassis;
+        xchassis.set_angle_feedback(mwheelsteer * mfwheelratio * 180.0/M_PI);
+        xchassis.set_vel(mvel * 3.6);
+
+#if PROTOBUF_VERSION < 3012000
+        nbytesize = xchassis.ByteSize();
+#else
+        nbytesize = xchassis.ByteSizeLong();
+#endif
+
+        std::shared_ptr<char> pstr_ptr = std::shared_ptr<char>(new char[nbytesize]);
+        if(xchassis.SerializeToArray(pstr_ptr.get(),nbytesize))
+        {
+            iv::modulecomm::ModuleSendMsg(mpachassis,pstr_ptr.get(),nbytesize);
+        }
+        else
+        {
+            std::cout<<" chassis serializetoarray fail."<<std::endl;
+        }
+
         std::this_thread::sleep_for(std::chrono::milliseconds(20));
     }
 }

src/tool/simple_planning_simulator/simmodel.h

@@ -6,6 +6,7 @@
 
 #include "gpsimu.pb.h"
 #include "objectarray.pb.h"
+#include "chassis.pb.h"
 
 #include "modulecomm.h"
 
@@ -58,6 +59,10 @@ protected:
 
     double mfSensorRange = 50.0;
 
+    double mfWHEELSPEED = 300;
+    double mfwheelbase = 2.9;
+    double mfwheelratio = 13.6;
+
 public:
     double GetX();
     double GetY();
@@ -103,6 +108,7 @@ private:
 
     void * mpa;
     void * mpatrack;
+    void * mpachassis;
 
     std::vector<iv::simobj> mvectorsimobj;
     std::mutex mmutexsimobj;

src/tool/simple_planning_simulator/simmodel_shenlan.h

@@ -12,9 +12,7 @@ public:
     void CalcModel();
 
 private:
-    double mfWHEELSPEED = 300;
-    double mfwheelbase = 2.9;
-    double mfwheelratio = 13.6;
+
 
 };
 

src/tool/simple_planning_simulator/simple_planning_simulator.pro

@@ -13,6 +13,7 @@ QMAKE_LFLAGS += -no-pie
 SOURCES += \
     ../../common/common/math/gnss_coordinate_convert.cpp \
     ../../common/common/xodr/xodrfunc/roadsample.cpp \
+    ../../include/msgtype/chassis.pb.cc \
     ../../include/msgtype/decition.pb.cc \
     ../../include/msgtype/gpsimu.pb.cc \
     ../../include/msgtype/object.pb.cc \
@@ -40,6 +41,7 @@ SOURCES += \
 HEADERS += \
     ../../common/common/math/gnss_coordinate_convert.h \
     ../../common/common/xodr/xodrfunc/roadsample.h \
+    ../../include/msgtype/chassis.pb.h \
     ../../include/msgtype/decition.pb.h \
     ../../include/msgtype/gpsimu.pb.h \
     ../../include/msgtype/object.pb.h \