mpc model theta bug fixed

mpc_control/CMakeLists.txt

@@ -148,6 +148,10 @@ catkin_package(
 #Create node
 add_executable(mpc_controller_node src/mpc_controller_node.cpp)
 target_link_libraries(mpc_controller_node ${catkin_LIBRARIES} ${PCL_LIBRARIES} ipopt)
+
+add_executable(mpc_tools src/mpc_tools.cpp)
+target_link_libraries(mpc_tools ${catkin_LIBRARIES} ${PCL_LIBRARIES})
+
 ## Rename C++ executable without prefix
 ## The above recommended prefix causes long target names, the following renames the
 ## target back to the shorter version for ease of user use

mpc_control/src/mpc_controller_node.cpp

@@ -57,13 +57,13 @@ class FG_eval {
     // any anything you think may be beneficial.
     
     // Weights for how "important" each cost is - can be tuned
-    const int cte_cost_weight = 5000;
-    const int epsi_cost_weight = 5000;
-    const int v_cost_weight = 1;
+    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 = 10;
+    const int a_cost_weight = 7;
     // const int delta_change_cost_weight = 100;
-    const int a_change_cost_weight = 10;
+    const int a_change_cost_weight = 7;
 
     // std::cout<<"fg0: "<<fg[0]<<std::endl;
     // Cost for CTE, psi error and velocity
@@ -164,11 +164,21 @@ mpc_controller::mpc_controller(ros::NodeHandle nh):nh_(nh)
   last_pos = pcl::PointXYZ(0, 0, 0);
   curr_pos = pcl::PointXYZ(0, 0, 0);
 
-  //创建全局路径
+  // //创建全局路径
+  // pcl::PointXYZ base_point(0,0,0);
+  // pcl::PointXYZ target_point(10,0,0);
+  // for (int i = 0; i < 100; i++)
+  // {
+  //   double x = i * (target_point.x - base_point.x) / 100.0 + base_point.x;
+  //   double y = i * (target_point.y - base_point.y) / 100.0 + base_point.y;
+  //   ptsx.push_back(x);
+  //   ptsy.push_back(y);
+  // }
+
   for (int i = -30; i < 100; i++)
   {
     double x = i * (15.0 / 100);
-    double y = -0.015 * x * x * x + x * x / 8.0 + x / 2.0;
+    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;
@@ -185,6 +195,33 @@ mpc_controller::~mpc_controller()
 {
 }
 
+//获取小车坐标系中，x前进方向 num 个点，用于拟合
+bool mpc_controller::filterPath(std::vector<double> xs, std::vector<double> ys, Eigen::VectorXd x_car, Eigen::VectorXd y_car, double theta, int numOfPoints){
+  if(x_car.size() != numOfPoints)
+    return false;
+  if(y_car.size() != numOfPoints)
+    return false;
+  
+  int count=0;
+  // 平移加反向旋转到小车坐标系
+  for (int i = 0; i < xs.size(); i++)
+  {
+    double x = xs[i] - curr_pos.x;
+    double y = ys[i] - curr_pos.y;
+    double trans_x = x * cos(-theta) - y * sin(-theta);
+    double trans_y = x * sin(-theta) + y * cos(-theta);
+    double dist = sqrt(pow(trans_x,2)+pow(trans_y,2));
+    if (trans_x>=0  && count < numOfPoints)
+    {
+      x_car[count] = trans_x;
+      y_car[count] = trans_y;
+      // std::cout << "path " << count << " : " << ptsx_car[count] << ", " << ptsy_car[count] << std::endl;
+      count++;
+    }
+  }
+
+}
+
 void mpc_controller::poseHandler(const turtlesim::Pose::ConstPtr &pose_msg)
 {
   //发布全局路径
@@ -239,7 +276,7 @@ void mpc_controller::poseHandler(const turtlesim::Pose::ConstPtr &pose_msg)
     double trans_x = x * cos(-pose_msg->theta) - y * sin(-pose_msg->theta);
     double trans_y = x * sin(-pose_msg->theta) + y * cos(-pose_msg->theta);
     double dist = sqrt(pow(trans_x,2)+pow(trans_y,2));
-    if (trans_x>=0 && dist < 5 && count < N)
+    if (trans_x>=0 && count < N)
     {
       ptsx_car[count] = trans_x;
       ptsy_car[count] = trans_y;
@@ -249,6 +286,7 @@ void mpc_controller::poseHandler(const turtlesim::Pose::ConstPtr &pose_msg)
     // if(ptsx_car[i]>0)
     //   std::cout<<"path "<<i<<" : "<<ptsx_car[i]<<", "<<ptsy_car[i]<<std::endl;
   }
+  
   if (count < N)
   {
     ref_v /= (N - count);
@@ -256,12 +294,12 @@ void mpc_controller::poseHandler(const turtlesim::Pose::ConstPtr &pose_msg)
 
   if (pose_msg->x > 10 && forward)
   {
-    ref_v = -2.6;
+    ref_v = -2.0;
     forward = false;
   }
-  else if(pose_msg->x < 1 && !forward)
+  else if(pose_msg->x < 2 && !forward)
   {
-    ref_v = 2.6;
+    ref_v = 2.0;
     forward = true;
   }
   std::cout<<ref_v<<", "<<forward<<std::endl;
@@ -302,8 +340,8 @@ void mpc_controller::poseHandler(const turtlesim::Pose::ConstPtr &pose_msg)
   a_left = vars[0];
   a_right = vars[1];
   // std::cout<<"v_curr: "<<v_left<<", "<<v_right<<std::endl;
-  v_left += a_right*dt;
-  v_right += a_left*dt;
+  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;
@@ -313,7 +351,7 @@ void mpc_controller::poseHandler(const turtlesim::Pose::ConstPtr &pose_msg)
   velocity.linear.z = 0;
   velocity.angular.x = 0;
   velocity.angular.y = 0;
-  velocity.angular.z = (v_right-v_left)/Lf;
+  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 = omega;
   vel_publisher_.publish(velocity);
 

mpc_control/src/mpc_controller_node.h

@@ -40,6 +40,8 @@ public:
     // Evaluate a polynomial.
     double polyeval(Eigen::VectorXd coeffs, double x);
 
+    bool filterPath(std::vector<double> x, std::vector<double> y, Eigen::VectorXd x_car, Eigen::VectorXd y_car, double theta, int numOfPoints);
+
     // Fit a polynomial.
     // Adapted from
     // https://github.com/JuliaMath/Polynomials.jl/blob/master/src/Polynomials.jl#L676-L716

mpc_control/src/mpc_tools.cpp

@@ -0,0 +1,97 @@
+#include  "mpc_tools.h"
+
+mpc_tools::mpc_tools()
+{
+}
+
+mpc_tools::~mpc_tools()
+{
+}
+
+//获取小车坐标系中，x前进方向 num 个点，用于拟合
+bool mpc_tools::filterPath(pcl::PointXYZ curr_pos, double theta, Eigen::VectorXd *x_car, Eigen::VectorXd *y_car, int numOfPoints){
+  if(x_car->size() != numOfPoints)
+    return false;
+  if(y_car->size() != numOfPoints)
+    return false;
+  
+  int count=0;
+  // 平移加反向旋转到小车坐标系
+  for (int i = 0; i < ptsx.size(); i++)
+  {
+    double x = ptsx[i] - curr_pos.x;
+    double y = ptsy[i] - curr_pos.y;
+    double trans_x = x * cos(-theta) - y * sin(-theta);
+    double trans_y = x * sin(-theta) + y * cos(-theta);
+    double dist = sqrt(pow(trans_x,2)+pow(trans_y,2));
+    if (trans_x>=0  && count < numOfPoints)
+    {
+      (*x_car)[count] = trans_x;
+      (*y_car)[count] = trans_y;
+      //std::cout << "local path " << count << " : " << x_car[count] << ", " << y_car[count] << std::endl;
+      count++;
+    }
+  }
+
+}
+
+// 求y
+double mpc_tools::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 mpc_tools::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;
+}
+
+int main(int argc, char** argv){
+    mpc_tools mpc;
+    //创建全局路径
+    pcl::PointXYZ base_point(0, 0, 0);
+    pcl::PointXYZ target_point(5, 2.5, 0);
+    for (int i = 0; i < 100; i++)
+    {
+        double x = i * (target_point.x - base_point.x) / 100.0 + base_point.x;
+        double y = i * (target_point.y - base_point.y) / 100.0 + base_point.y;
+        mpc.ptsx.push_back(x);
+        mpc.ptsy.push_back(y);
+        //std::cout << "path " << i << " : " << mpc.ptsx[i] << ", " << mpc.ptsx[i] << std::endl;
+    }
+    pcl::PointXYZ curr_pos(2,0,0);
+    Eigen::VectorXd x_car(10);
+    Eigen::VectorXd y_car(10);
+    for (size_t i = 0; i < 10; i++)
+    {
+        std::cout<<"x0,y0: "<<x_car[i]<<", "<<y_car[i]<<std::endl;
+    }
+    mpc.filterPath(curr_pos,0,&x_car,&y_car,10);
+    for (size_t i = 0; i < 10; i++)
+    {
+        std::cout<<"x,y: "<<x_car[i]<<", "<<y_car[i]<<std::endl;
+    }
+    Eigen::VectorXd coeffs = mpc.polyfit(x_car,y_car,3);
+    double y = mpc.polyeval(coeffs, 0);
+    std::cout<<"fit result y: "<<y<<std::endl;
+    std::cout<<"---------------"<<std::endl;
+}

mpc_control/src/mpc_tools.h

@@ -0,0 +1,32 @@
+#ifndef MPC_TOOLS_HH
+#define MPC_TOOLS_HH 
+
+#include <vector>
+#include <Eigen/Core>
+#include <Eigen/QR>
+#include <pcl/point_types.h>
+
+// For converting back and forth between radians and degrees.
+constexpr double pi() { return M_PI; }
+double deg2rad(double x) { return x * pi() / 180; }
+double rad2deg(double x) { return x * 180 / pi(); }
+
+class mpc_tools{
+
+public:
+    mpc_tools();
+    ~mpc_tools();
+
+    Eigen::VectorXd polyfit(Eigen::VectorXd xvals, Eigen::VectorXd yvals, int order);
+
+    double polyeval(Eigen::VectorXd coeffs, double x);
+
+    bool filterPath(pcl::PointXYZ curr_pos, double theta, Eigen::VectorXd *x_car, Eigen::VectorXd *y_car, int numOfPoints);
+
+public:
+
+    std::vector<double> ptsx;
+    std::vector<double> ptsy;
+};
+
+#endif /* MPC_TOOLS_HH */