organize code and create a solver class (MpcDiffModel) for testing.

mpc_control/CMakeLists.txt

@@ -146,7 +146,7 @@ catkin_package(
 # target_link_libraries(laser_scan_matcher ${catkin_LIBRARIES} ${csm_LIBRARIES} )
 
 #Create node
-add_executable(mpc_controller_node src/mpc_controller_node.cpp)
+add_executable(mpc_controller_node src/mpc_controller_node.cpp src/MpcDiffModel.cpp)
 target_link_libraries(mpc_controller_node ${catkin_LIBRARIES} ${PCL_LIBRARIES} ipopt)
 
 add_executable(mpc_tools src/mpc_tools.cpp)

mpc_control/src/MpcDiffModel.cpp

@@ -0,0 +1,320 @@
+#include  "MpcDiffModel.h"
+using CppAD::AD;
+
+static double ref_v=2.5;
+
+class FG_eval {
+ public:
+  // Fitted polynomial coefficients
+  Eigen::VectorXd coeffs;
+  FG_eval(Eigen::VectorXd coeffs) { this->coeffs = coeffs; }
+
+  typedef CPPAD_TESTVECTOR(AD<double>) ADvector;
+  void operator()(ADvector& fg, const ADvector& vars) {
+    // Implementing MPC below
+    // `fg` a vector of the cost constraints, `vars` is a vector of variable values (state & actuators)
+    // The cost is stored is the first element of `fg`.
+    // Any additions to the cost should be added to `fg[0]`.
+    fg[0] = 0;
+    
+    // Reference State Cost
+    // Below defines the cost related the reference state and
+    // any anything you think may be beneficial.
+    
+    // Weights for how "important" each cost is - can be tuned
+    const int cte_cost_weight = 8000;
+    const int epsi_cost_weight = 1000;
+    const int v_cost_weight = 2;
+    // const int delta_cost_weight = 10;
+    const int a_cost_weight = 7;
+    // const int delta_change_cost_weight = 100;
+    const int a_change_cost_weight = 7;
+
+    // std::cout<<"fg0: "<<fg[0]<<std::endl;
+    // Cost for CTE, psi error and velocity
+    for (int t = 0; t < MpcDiffModel::N; t++) {
+      fg[0] += cte_cost_weight * CppAD::pow(vars[MpcDiffModel::cte_start + t], 2);
+      fg[0] += epsi_cost_weight * CppAD::pow(vars[MpcDiffModel::epsi_start + t], 2);
+      fg[0] += v_cost_weight * CppAD::pow(vars[MpcDiffModel::v_left_start + t] - ref_v, 2);
+      fg[0] += v_cost_weight * CppAD::pow(vars[MpcDiffModel::v_right_start + t] - ref_v, 2);
+      // std::cout<<"v l r : "<<vars[v_left_start + t]<<", "<<vars[v_right_start + t]<<std::endl;
+    }
+    
+    // Costs for steering (delta) and acceleration (a)
+    for (int t = 0; t < MpcDiffModel::N-1; t++) {
+      // fg[0] += delta_cost_weight * CppAD::pow(vars[delta_start + t], 2);
+      fg[0] += a_cost_weight * CppAD::pow(vars[MpcDiffModel::a_left_start + t], 2);
+      fg[0] += a_cost_weight * CppAD::pow(vars[MpcDiffModel::a_right_start + t], 2);
+      // std::cout<<"a l r : "<<vars[a_left_start + t]<<", "<<vars[a_right_start + t]<<std::endl;
+    }
+    // std::cout<<"fg0: "<<fg[0]<<std::endl;
+    
+    // Costs related to the change in steering and acceleration (makes the ride smoother)
+    for (int t = 0; t < MpcDiffModel::N-2; t++) {
+      // fg[0] += delta_change_cost_weight * pow(vars[delta_start + t + 1] - vars[delta_start + t], 2);
+      fg[0] += a_change_cost_weight * pow(vars[MpcDiffModel::a_left_start + t + 1] - vars[MpcDiffModel::a_left_start + t], 2);
+      fg[0] += a_change_cost_weight * pow(vars[MpcDiffModel::a_right_start + t + 1] - vars[MpcDiffModel::a_right_start + t], 2);
+    }
+    // std::cout<<"fg0: "<<fg[0]<<std::endl;
+    
+    // Setup Model Constraints
+
+    // Initial constraints
+    // We add 1 to each of the starting indices due to cost being located at index 0 of `fg`.
+    // This bumps up the position of all the other values.
+    fg[1 + MpcDiffModel::x_start] = vars[MpcDiffModel::x_start];
+    fg[1 + MpcDiffModel::y_start] = vars[MpcDiffModel::y_start];
+    fg[1 + MpcDiffModel::psi_start] = vars[MpcDiffModel::psi_start];
+    fg[1 + MpcDiffModel::v_left_start] = vars[MpcDiffModel::v_left_start];
+    fg[1 + MpcDiffModel::v_right_start] = vars[MpcDiffModel::v_right_start];
+    fg[1 + MpcDiffModel::cte_start] = vars[MpcDiffModel::cte_start];
+    fg[1 + MpcDiffModel::epsi_start] = vars[MpcDiffModel::epsi_start];
+    // std::cout << "112,,,fg,vars: " <<1 + a_left_start<<", "<<1 + a_right_start<< std::endl;
+    // fg[1 + a_left_start] = vars[a_left_start];
+    // std::cout << "113,,,fg,vars: " <<1 + a_left_start<<", "<<1 + a_right_start<< std::endl;
+    // fg[1 + a_right_start] = vars[a_right_start];
+    // std::cout << "114,,,fg,vars: " <<1 + a_left_start<<", "<<1 + a_right_start<< std::endl;
+    
+    // The rest of the constraints
+    for (int t = 1; t < MpcDiffModel::N; t++) {
+      // State at time t + 1
+      AD<double> x1 = vars[MpcDiffModel::x_start + t];
+      AD<double> y1 = vars[MpcDiffModel::y_start + t];
+      AD<double> psi1 = vars[MpcDiffModel::psi_start + t];
+      AD<double> v_left1 = vars[MpcDiffModel::v_left_start + t];
+      AD<double> v_right1 = vars[MpcDiffModel::v_right_start + t];
+      AD<double> cte1 = vars[MpcDiffModel::cte_start + t];
+      AD<double> epsi1 = vars[MpcDiffModel::epsi_start + t];
+      
+      // State at time t
+      AD<double> x0 = vars[MpcDiffModel::x_start + t - 1];
+      AD<double> y0 = vars[MpcDiffModel::y_start + t - 1];
+      AD<double> psi0 = vars[MpcDiffModel::psi_start + t - 1];
+      AD<double> v_left0 = vars[MpcDiffModel::v_left_start + t - 1];
+      AD<double> v_right0 = vars[MpcDiffModel::v_right_start + t - 1];
+      AD<double> cte0 = vars[MpcDiffModel::cte_start + t - 1];
+      AD<double> epsi0 = vars[MpcDiffModel::epsi_start + t - 1];
+      
+      // Actuator constraints at time t only
+      AD<double> a_left0 = vars[MpcDiffModel::a_left_start + t - 1];
+      AD<double> a_right0 = vars[MpcDiffModel::a_right_start + t - 1];
+      
+      AD<double> f0 = coeffs[0] + coeffs[1] * x0 + coeffs[2] * pow(x0, 2) + coeffs[3] * pow(x0, 3);
+      AD<double> psi_des0 = CppAD::atan(coeffs[1] + 2*coeffs[2]*x0 + 3*coeffs[3]*pow(x0,2));
+      
+      // Setting up the rest of the model constraints
+      AD<double> v0 = (v_left0+v_right0)/2.0;
+      AD<double> v_diff0 = v_right0 - v_left0;
+      fg[1 + MpcDiffModel::x_start + t] = x1 - (x0 + v0 * CppAD::cos(psi0) * MpcDiffModel::dt);
+      fg[1 + MpcDiffModel::y_start + t] = y1 - (y0 + v0 * CppAD::sin(psi0) * MpcDiffModel::dt);
+      fg[1 + MpcDiffModel::psi_start + t] = psi1 - (psi0 - v_diff0 / MpcDiffModel::Lf * MpcDiffModel::dt);
+      // fg[1 + v_start + t] = v1 - (v0 + a0 * dt);
+      fg[1 + MpcDiffModel::v_left_start + t] = v_left1 - (v_left0 + a_left0 * MpcDiffModel::dt);
+      fg[1 + MpcDiffModel::v_right_start + t] = v_right1 - (v_right0 + a_right0 * MpcDiffModel::dt);
+      fg[1 + MpcDiffModel::cte_start + t] = cte1 - ((f0-y0) + (v0 * CppAD::sin(epsi0) * MpcDiffModel::dt));// 预测路径误差 - (y方向与路径差 + 当前角度误差带来的y方向移动)
+      fg[1 + MpcDiffModel::epsi_start + t] = epsi1 - ((psi0 - psi_des0) - v_diff0 / MpcDiffModel::Lf * MpcDiffModel::dt);// 预测角度误差 - (当前与目标角度差 - 当前角度偏转或补偿)
+    }    
+  }
+};
+
+MpcDiffModel::MpcDiffModel()
+{
+  // const double dt = 0.1;
+  // const double Lf = 1;
+  // ref_v = 2.5;
+  v_left = 0;
+  v_right = 0;
+  a_left = 0;
+  a_right = 0;
+  last_theta = 0;
+
+  // for (int i = -30; i < 100; i++)
+  // {
+  //   double x = i * (15.0 / 100);
+  //   double y = -0.005 * x * x * x + 0.0525 * x * x + 0.001 * x+1;
+  //   ptsx.push_back(x);
+  //   ptsy.push_back(y);
+  //   // std::cout<<"x,y: "<<x<<", "<<y<<std::endl;
+  // }
+}
+
+MpcDiffModel::~MpcDiffModel()
+{
+}
+
+// 求y
+double MpcDiffModel::polyeval(Eigen::VectorXd coeffs, double x) {
+  double result = 0.0;
+  for (int i = 0; i < coeffs.size(); i++) {
+    result += coeffs[i] * pow(x, i);
+  }
+  return result;
+}
+
+// 求参数
+Eigen::VectorXd MpcDiffModel::polyfit(Eigen::VectorXd xvals, Eigen::VectorXd yvals, int order) {
+  assert(xvals.size() == yvals.size());
+  assert(order >= 1 && order <= xvals.size() - 1);
+  Eigen::MatrixXd A(xvals.size(), order + 1);
+
+  for (int i = 0; i < xvals.size(); i++) {
+    A(i, 0) = 1.0;
+  }
+
+  for (int j = 0; j < xvals.size(); j++) {
+    for (int i = 0; i < order; i++) {
+      A(j, i + 1) = A(j, i) * xvals(j);
+    }
+  }
+
+  auto Q = A.householderQr();
+  auto result = Q.solve(yvals);
+  return result;
+}
+
+// 输入局部路径，当前位姿，速度与角速度
+// 输出用于下发的速度与角速度
+bool MpcDiffModel::Solve(std::vector<tf::Pose> ref_poses,tf::Pose curr_pose, double& v, double& vth)
+{
+  // std::cout << "model 000 v: " << v << ", " << vth << std::endl;
+  double theta_diff = curr_pose.getRotation().getAngle() - last_theta;
+  v_left = v - vth*Lf/2.0;//(theta_diff * Lf) / (2 * dt);
+  v_right = v + vth*Lf/2.0;//(theta_diff * Lf) / (2 * dt);
+  Eigen::VectorXd ptsx_car(ref_poses.size());
+  Eigen::VectorXd ptsy_car(ref_poses.size());
+
+  for (int i = 0; i < ref_poses.size(); i++)
+  {
+    ptsx_car[i] = ref_poses[i].getOrigin().getX();
+    ptsy_car[i] = ref_poses[i].getOrigin().getY();
+    //std::cout<<"x,y: "<<ptsx_car[i]<<", "<<ptsy_car[i]<<std::endl;
+  }
+
+  //拟合求斜率与y方向误差
+  auto coeffs = polyfit(ptsx_car, ptsy_car, 3);
+  // if(std::isnan(coeffs[0])||std::isnan(coeffs[1])||std::isnan(coeffs[2])||std::isnan(coeffs[3]))
+  //   return;
+  double cte = polyeval(coeffs, 0);
+  double etheta = atan(coeffs[1]);
+
+  // Predict state after latency
+  // x, y and psi are all zero after transformation above
+  double pred_px = 0.0 + v * dt; // Since psi is zero, cos(0) = 1, can leave out
+  const double pred_py = 0.0;    // Since sin(0) = 0, y stays as 0 (y + v * 0 * dt)
+  double pred_theta = 0.0 + theta_diff;
+  double pred_v_left = v_left + a_left * dt;
+  double pred_v_right = v_right + a_right * dt;
+  double pred_cte = cte + v * sin(etheta) * dt;
+  double pred_etheta = etheta - theta_diff;
+
+  bool ok = true;
+  typedef CPPAD_TESTVECTOR(double) Dvector;
+
+  size_t n_vars = N * 7 + (N - 1) * 2;
+
+  size_t n_constraints = N * 7;
+
+  Dvector vars(n_vars);
+  for (int i = 0; i < n_vars; i++)
+  {
+    vars[i] = 0.0;
+  }
+
+  Dvector vars_lowerbound(n_vars);
+  Dvector vars_upperbound(n_vars);
+
+  for (int i = 0; i < a_left_start; i++) {
+    vars_lowerbound[i] = -1.0e6;
+    vars_upperbound[i] = 1.0e6;
+  }
+  
+  for (int i = a_left_start; i < n_vars; i++) {
+    vars_lowerbound[i] = -5.5;
+    vars_upperbound[i] = 5.5;
+  }
+
+  Dvector constraints_lowerbound(n_constraints);
+  Dvector constraints_upperbound(n_constraints);
+  for (int i = 0; i < n_constraints; i++) {
+    constraints_lowerbound[i] = 0;
+    constraints_upperbound[i] = 0;
+  }
+  
+  // Start lower and upper limits at current values
+  constraints_lowerbound[x_start] = pred_px;
+  constraints_lowerbound[y_start] = pred_py;
+  constraints_lowerbound[psi_start] = pred_theta;
+  constraints_lowerbound[v_left_start] = pred_v_left;
+  constraints_lowerbound[v_right_start] = pred_v_right;
+  constraints_lowerbound[cte_start] = pred_cte;
+  constraints_lowerbound[epsi_start] = pred_etheta;
+  
+  constraints_upperbound[x_start] = pred_px;
+  constraints_upperbound[y_start] = pred_py;
+  constraints_upperbound[psi_start] = pred_theta;
+  constraints_upperbound[v_left_start] = pred_v_left;
+  constraints_upperbound[v_right_start] = pred_v_right;
+  constraints_upperbound[cte_start] = pred_cte;
+  constraints_upperbound[epsi_start] = pred_etheta;
+
+  // object that computes objective and constraints
+  FG_eval fg_eval(coeffs);
+
+  std::string options;
+  // Uncomment this if you'd like more print information
+  options += "Integer print_level  0\n";
+  // NOTE: Setting sparse to true allows the solver to take advantage
+  // of sparse routines, this makes the computation MUCH FASTER. If you
+  // can uncomment 1 of these and see if it makes a difference or not but
+  // if you uncomment both the computation time should go up in orders of
+  // magnitude.
+  options += "Sparse  true        forward\n";
+  options += "Sparse  true        reverse\n";
+  // NOTE: Currently the solver has a maximum time limit of 0.5 seconds.
+  // Change this as you see fit.
+  options += "Numeric max_cpu_time          0.5\n";
+
+  // place to return solution
+  CppAD::ipopt::solve_result<Dvector> solution;
+  // solve the problem
+  // std::cout<<vars.size()<<", "<<vars_lowerbound.size()<<", "<<vars_upperbound.size()<<", "<<constraints_lowerbound.size()<<", "<<constraints_upperbound.size()<<", "<<std::endl;
+  CppAD::ipopt::solve<Dvector, FG_eval>(
+      options, vars, vars_lowerbound, vars_upperbound, constraints_lowerbound,
+      constraints_upperbound, fg_eval, solution);
+  
+  // Check some of the solution values
+  ok &= solution.status == CppAD::ipopt::solve_result<Dvector>::success;
+
+  // Cost
+  auto cost = solution.obj_value;
+  // std::cout <<"model   status "<<ok<< ", Cost " << cost << std::endl;
+
+  //保存状态，根据优化结果获取需下发 v vth
+  last_theta = curr_pose.getRotation().getAngle();
+  std::vector<double> solved;
+  if (ok)
+  {
+    solved.push_back(solution.x[a_left_start]);
+    solved.push_back(solution.x[a_right_start]);
+    for (int i = 0; i < N; ++i)
+    {
+      solved.push_back(solution.x[x_start + i]);
+      solved.push_back(solution.x[y_start + i]);
+    }
+    // std::cout<<"return solved"<<std::endl;
+  }else
+  {
+    return false;
+  }
+
+  //更新 v vth
+  a_left = solved[0];
+  a_right = solved[1];
+  // std::cout<<"v_curr: "<<v_left<<", "<<v_right<<std::endl;
+  v_left += a_left * dt;
+  v_right += a_right * dt;
+  // std::cout<<"a: "<<a_left<<", "<<a_right<<std::endl;
+  // std::cout << "model   v: " << v_left << ", " << v_right << std::endl;
+  v = (v_left + v_right) / 2.0;
+  vth = -(v_right - v_left) / Lf; //turtlesim uses left hand angle, if angle less than 0, it means turn left.
+}

mpc_control/src/MpcDiffModel.h

@@ -0,0 +1,52 @@
+#ifndef MPC_DIFF_MODEL_HH
+#define MPC_DIFF_MODEL_HH 
+
+#include <tf/transform_datatypes.h>
+
+#include <vector>
+#include <Eigen/Core>
+#include <Eigen/QR>
+#include <cppad/cppad.hpp>
+#include <cppad/ipopt/solve.hpp>
+
+class MpcDiffModel{
+
+public:
+    MpcDiffModel();
+    ~MpcDiffModel();
+
+    // std::vector<double> Solve(Eigen::VectorXd state, Eigen::VectorXd coeffs);
+
+    bool Solve(std::vector<tf::Pose> ref_poses,tf::Pose curr_pose, double& v, double& vth);
+private:
+
+    double polyeval(Eigen::VectorXd coeffs, double x);
+
+    Eigen::VectorXd polyfit(Eigen::VectorXd xvals, Eigen::VectorXd yvals, int order);
+public:
+
+    static const size_t N = 10;
+    static constexpr double dt=0.1;
+    static constexpr double Lf=1;
+
+    // x，y； psi旋转角； v速度值； cte实际路径误差； epsi角度误差； a_left左轮加速度； a_right右轮加速度
+    static const size_t x_start = 0;
+    static const size_t y_start = x_start + N;
+    static const size_t psi_start = y_start + N;
+    static const size_t v_left_start = psi_start + N;
+    static const size_t v_right_start = v_left_start + N;
+    static const size_t cte_start = v_right_start + N;
+    static const size_t epsi_start = cte_start + N;
+
+    static const size_t a_left_start = epsi_start + N;
+    static const size_t a_right_start = a_left_start + N - 1;
+
+    // static double ref_v;
+    bool forward = true;
+
+private:
+
+    double last_theta, v_left, v_right, a_left, a_right;
+};
+
+#endif /* MPC_DIFF_MODEL_HH */

mpc_control/src/mpc_controller_node.cpp

@@ -224,6 +224,9 @@ bool mpc_controller::filterPath(std::vector<double> xs, std::vector<double> ys,
 
 void mpc_controller::poseHandler(const turtlesim::Pose::ConstPtr &pose_msg)
 {
+  std::chrono::time_point<std::chrono::system_clock> start = std::chrono::system_clock::now();
+
+  std::cout<<"-------------"<<std::endl;
   //发布全局路径
   nav_msgs::Path path;
   path.header.frame_id = "map";
@@ -330,50 +333,70 @@ void mpc_controller::poseHandler(const turtlesim::Pose::ConstPtr &pose_msg)
   Eigen::VectorXd state(7);
   state << pred_px, pred_py, pred_theta, pred_v_left, pred_v_right, pred_cte, pred_etheta;
   // std::cout<<state<<std::endl;
-  auto vars = Solve(state, coeffs);
-  if(vars.size()==0){
-    // std::cout<<"empty, return"<<std::endl;
-    return;
+
+  // auto vars = Solve(state, coeffs);
+  // if(vars.size()==0){
+  //   // std::cout<<"empty, return"<<std::endl;
+  //   return;
+  // }
+
+  // //更新速度并发布cmd_vel
+  // a_left = vars[0];
+  // a_right = vars[1];
+  // // std::cout<<"v_curr: "<<v_left<<", "<<v_right<<std::endl;
+  // v_left += a_left*dt;
+  // v_right += a_right*dt;
+  // // omega += (a_left-a_right)*dt;
+  // // std::cout<<"a: "<<a_left<<", "<<a_right<<std::endl;
+  // std::cout<<"v: "<<v_left<<", "<<v_right<<std::endl;
+
+  std::chrono::time_point<std::chrono::system_clock> end = std::chrono::system_clock::now();
+  std::cout<<"part1: "<<std::chrono::duration_cast<std::chrono::milliseconds>(end-start).count()<<std::endl;
+
+  std::vector<tf::Pose> poses;
+  for (size_t i = 0; i < ptsx_car.size(); i++)
+  {
+    poses.push_back(tf::Pose(tf::Quaternion(), tf::Vector3(ptsx_car[i],ptsy_car[i],0)));
   }
+  tf::Pose pose;
+  double velo = pose_msg->linear_velocity;
+  double velo_angle = pose_msg->angular_velocity;
+  pose.setOrigin(tf::Vector3(pose_msg->x, pose_msg->y, 0.0));
+  pose.setRotation(tf::Quaternion(0, 0, sin(pose_msg->theta/2.0), cos(pose_msg->theta/2.0)));
+  mdm.Solve(poses, pose, velo, velo_angle);
 
-  //更新速度并发布cmd_vel
-  a_left = vars[0];
-  a_right = vars[1];
-  // std::cout<<"v_curr: "<<v_left<<", "<<v_right<<std::endl;
-  v_left += a_left*dt;
-  v_right += a_right*dt;
-  // omega += (a_left-a_right)*dt;
-  // std::cout<<"a: "<<a_left<<", "<<a_right<<std::endl;
-  std::cout<<"v: "<<v_left<<", "<<v_right<<std::endl;
   geometry_msgs::Twist velocity;
-  velocity.linear.x = (v_left+v_right)/2.0;
+  velocity.linear.x = velo;//(v_left+v_right)/2.0;
   velocity.linear.y = 0;
   velocity.linear.z = 0;
   velocity.angular.x = 0;
   velocity.angular.y = 0;
-  velocity.angular.z = - (v_right-v_left)/Lf;//turtlesim uses left hand angle, if angle less than 0, it means turn left.
+  velocity.angular.z = velo_angle;//- (v_right-v_left)/Lf;//turtlesim uses left hand angle, if angle less than 0, it means turn left.
   // velocity.angular.z = omega;
   vel_publisher_.publish(velocity);
 
-  //发布局部路径
-  nav_msgs::Path local_path;
-  local_path.header = path.header;
-  local_path.header.frame_id = "base_link";
-  for (size_t i = 2; i < vars.size(); i+=2)
-  {
-    geometry_msgs::PoseStamped temp;
-    temp.header = local_path.header;
-    temp.pose.position.x = vars[i];
-    temp.pose.position.y = vars[i+1];
-    double theta = atan2(vars[i+1], vars[i]);
-    // if(vars[i+1]*vars[i]<0)
-    //   theta = theta-M_PI;
-    tf::quaternionTFToMsg(tf::createQuaternionFromRPY(0.0, 0.0, theta),temp.pose.orientation);
-    // temp.pose.orientation.w = cos(theta / 2.0);
-    // temp.pose.orientation.z = sin(theta / 2.0);
-    local_path.poses.push_back(temp);
-  }
-  local_path_publisher_.publish(local_path);
+  std::chrono::time_point<std::chrono::system_clock> end1 = std::chrono::system_clock::now();
+  std::cout<<"part2: "<<std::chrono::duration_cast<std::chrono::milliseconds>(end1-end).count()<<std::endl;
+
+  // //发布局部路径
+  // nav_msgs::Path local_path;
+  // local_path.header = path.header;
+  // local_path.header.frame_id = "base_link";
+  // for (size_t i = 2; i < vars.size(); i+=2)
+  // {
+  //   geometry_msgs::PoseStamped temp;
+  //   temp.header = local_path.header;
+  //   temp.pose.position.x = vars[i];
+  //   temp.pose.position.y = vars[i+1];
+  //   double theta = atan2(vars[i+1], vars[i]);
+  //   // if(vars[i+1]*vars[i]<0)
+  //   //   theta = theta-M_PI;
+  //   tf::quaternionTFToMsg(tf::createQuaternionFromRPY(0.0, 0.0, theta),temp.pose.orientation);
+  //   // temp.pose.orientation.w = cos(theta / 2.0);
+  //   // temp.pose.orientation.z = sin(theta / 2.0);
+  //   local_path.poses.push_back(temp);
+  // }
+  // local_path_publisher_.publish(local_path);
 
   last_theta = pose_msg->theta;
   last_pos = curr_pos;

mpc_control/src/mpc_controller_node.h

@@ -1,6 +1,8 @@
 #ifndef MPC_CONTROLLER_NODE_HH
 #define MPC_CONTROLLER_NODE_HH 
 
+#include  "MpcDiffModel.h"
+
 #include <ros/ros.h>
 #include <geometry_msgs/PoseStamped.h>
 #include <tf/transform_listener.h>
@@ -17,6 +19,7 @@
 #include <cppad/cppad.hpp>
 #include <cppad/ipopt/solve.hpp>
 #include <pcl/point_types.h>
+#include <chrono>
 
 // For converting back and forth between radians and degrees.
 constexpr double pi() { return M_PI; }
@@ -52,6 +55,7 @@ public:
     size_t N = 10;
     double dt = 0.1;
 
+    MpcDiffModel mdm;
     // // This value assumes the model presented in the classroom is used.
     // //
     // // It was obtained by measuring the radius formed by running the vehicle in the