mpc_debug. add target approaching optimization with smoother control of velocity. need to find a two vehcle model in the future

MPC/launch/MPC.launch

@@ -1,8 +1,6 @@
 <launch>
   <node name="map_server" pkg="map_server" type="map_server" args="$(find rbx1_nav)/maps/blank_map.yaml"/>
   <node pkg="MPC" type="MPC_node" name="MPC_node" output="screen"/>
-  <group ns="turtlesim1">
-     <node pkg="turtlesim" name="sim" type="turtlesim_node"/>
-   </group>
+
 
 </launch>

MPC/launch/MPC_2.launch

@@ -8,7 +8,7 @@
     <node pkg="turtlesim" name="sim" type="turtlesim_node"/>
   </group>
 
-  <node name="map_server" pkg="map_server" type="map_server" args="$(find MPC)/maps/blank_map.yaml"/>
+  <node name="map_server" pkg="map_server" type="map_server" args="$(find rbx1_nav)/maps/blank_map.yaml"/>
   <node pkg="MPC" type="MPC2_node" name="MPC2_node" output="screen"/>
 
 </launch>

MPC/src/mpc/mpc_tools.h

@@ -15,9 +15,9 @@ class mpc_tools{
 public:
     mpc_tools();
     ~mpc_tools();
-    //求参
+
     Eigen::VectorXd polyfit(Eigen::VectorXd xvals, Eigen::VectorXd yvals, int order);
-    //求y
+
     double polyeval(Eigen::VectorXd coeffs, double x);
 
     bool filterPath(tf::Pose pose,std::vector<tf::Pose> poses, std::vector<tf::Pose>& out,

MPC/src/node2.cpp

@@ -152,8 +152,8 @@ void poseCallback2(const turtlesim::Pose::ConstPtr& msg)
 void publish_cmd(geometry_msgs::Twist cmd_vel1,geometry_msgs::Twist cmd_vel2)
 {
     ////
-    // g_vel_pub1.publish(cmd_vel1);
-    // g_vel_pub2.publish(cmd_vel2);
+    g_vel_pub1.publish(cmd_vel1);
+    g_vel_pub2.publish(cmd_vel2);
 
 }
 
@@ -400,7 +400,6 @@ void thread_MPC()
 
 }
 
-// 更新目标，生成路径
 void updateGoal(const geometry_msgs::PoseStamped::ConstPtr& msg)
 {
     printf("update goal recieve ...  \n");

grapher/CMakeLists.txt

@@ -63,19 +63,19 @@ aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/src/ndt NDT_SRC )
 aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/src/cfg CFG_SRC )
 set(SRC_LIST  src/steger/detectlines.cpp src/steger/detectlines.h src/steger/Steger.cpp src/steger/Steger.cu)
 
-cuda_add_executable(grapher_node src/grapher_node.cpp src/scanGrapher.cpp src/locateTask.cpp
-                      src/kalmanFilter.cpp ${TASK_SRC} ${NDT_SRC} ${SRC_LIST} ${CFG_SRC})
-target_include_directories(grapher_node PRIVATE "${CMAKE_CURRENT_LIST_DIR}/src/TaskQueue/threadpp" 
-                            ${PROTOBUF_INCLUDE_DIRS})
-target_link_libraries(grapher_node ${catkin_LIBRARIES} ${CUDA_LIBRARIES} ${PROTOBUF_LIBRARIES}
-            ${PCL_LIBRARIES} ${OpenCV_LIBS} libcsm.so)
+# cuda_add_executable(grapher_node src/grapher_node.cpp src/scanGrapher.cpp src/locateTask.cpp
+#                       src/kalmanFilter.cpp ${TASK_SRC} ${NDT_SRC} ${SRC_LIST} ${CFG_SRC})
+# target_include_directories(grapher_node PRIVATE "${CMAKE_CURRENT_LIST_DIR}/src/TaskQueue/threadpp" 
+#                             ${PROTOBUF_INCLUDE_DIRS})
+# target_link_libraries(grapher_node ${catkin_LIBRARIES} ${CUDA_LIBRARIES} ${PROTOBUF_LIBRARIES}
+#             ${PCL_LIBRARIES} ${OpenCV_LIBS} libcsm.so)
 ###############
 
 
-add_executable(location_node src/location_node.cpp src/locateTask.cpp src/kalmanFilter.cpp ${NDT_SRC} 
-                      ${TASK_SRC} ${CFG_SRC})
-target_include_directories(location_node PRIVATE "${CMAKE_CURRENT_LIST_DIR}/src/TaskQueue/threadpp" 
-                            ${PROTOBUF_INCLUDE_DIRS})
-target_link_libraries(location_node ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBS} 
-                      libcsm.so ${PROTOBUF_LIBRARIES})
+# add_executable(location_node src/location_node.cpp src/locateTask.cpp src/kalmanFilter.cpp ${NDT_SRC} 
+#                       ${TASK_SRC} ${CFG_SRC})
+# target_include_directories(location_node PRIVATE "${CMAKE_CURRENT_LIST_DIR}/src/TaskQueue/threadpp" 
+#                             ${PROTOBUF_INCLUDE_DIRS})
+# target_link_libraries(location_node ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBS} 
+#                       libcsm.so ${PROTOBUF_LIBRARIES})
 

mpc_control/CMakeLists.txt

@@ -149,6 +149,13 @@ catkin_package(
 add_executable(mpc_controller_node src/mpc_controller_node.cpp src/MpcDiffModel.cpp src/make_trajectory.cpp src/mpc_tools.cpp)
 target_link_libraries(mpc_controller_node ${catkin_LIBRARIES} ${PCL_LIBRARIES} ipopt)
 
+add_executable(mpc_two_vehicle_test src/mpc_two_vehicle/trajectory_tool.cpp src/test/two_vehicle_test.cpp)
+target_link_libraries(mpc_two_vehicle_test ${catkin_LIBRARIES} )
+
+aux_source_directory(src/mpc_two_vehicle two_vehicle)
+add_executable(mpc_two_vehicle_node ${two_vehicle})
+target_link_libraries(mpc_two_vehicle_node ${catkin_LIBRARIES} ${PCL_LIBRARIES} ipopt)
+
 # add_executable(mpc_tools src/mpc_tools.cpp)
 # target_link_libraries(mpc_tools ${catkin_LIBRARIES} ${PCL_LIBRARIES})
 

mpc_control/src/launch/turtlesim.launch

@@ -0,0 +1,6 @@
+<?xml version="1.0"?>
+<launch>
+    <node pkg="turtlesim" name="t0" type="turtlesim_node"/>
+    <!-- <node pkg="turtlesim" name="t1" type="turtlesim_node"/> -->
+    <node name="rviz" pkg="rviz" type="rviz" args="-d $(find mpc_controller)/src/launch/default.rviz" />
+</launch>

mpc_control/src/mpc_two_vehicle/MonitorMPC.cpp

@@ -0,0 +1,245 @@
+#include "MonitorMPC.h"
+#include <cppad/cppad.hpp>
+#include <cppad/ipopt/solve.hpp>
+#include "Eigen/Core"
+#include "Eigen/QR"
+
+using CppAD::AD;
+
+// Set the timestep length and duration
+// Currently tuned to predict 1 second worth
+size_t N = 10;
+
+
+// The solver takes all the state variables and actuator
+// variables in a singular vector. Thus, we should to establish
+// when one variable starts and another ends to make our lifes easier.
+size_t nx = 0;
+size_t ny = nx + N;
+size_t nth = ny + N;
+size_t nv = nth + N;
+size_t nvth = nv + N;
+
+
+class FG_eval {
+public:
+    // Fitted polynomial coefficients
+    Eigen::VectorXd coeffs;
+    double m_ref_v,m_dt;
+    double m_v,m_vth;
+    FG_eval(Eigen::VectorXd coeffs,double v,double vth,double ref_v,double dt) {
+        this->coeffs = coeffs;
+        m_v=v;
+        m_vth=vth;
+        m_ref_v=ref_v;
+        m_dt=dt;
+    }
+
+    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;
+        double dt=m_dt;
+        // 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 y_cost_weight = 1500;
+        const int th_cost_weight = 1000;
+        const int v_cost_weight = 20;
+        const int vth_cost_weight = 100;
+        const int a_cost_weight = 1;
+        const int ath_cost_weight=100;
+
+        // Cost for CTE, psi error and velocity
+        for (int t = 0; t < N; t++) {
+            AD<double> xt=vars[nx+t];
+            AD<double> fx = coeffs[0] + coeffs[1] * xt + coeffs[2] * pow(xt, 2) + coeffs[3] * pow(xt, 3);
+            fg[0] += y_cost_weight * CppAD::pow(vars[ny + t]-fx, 2);
+
+            AD<double> fth = CppAD::atan(coeffs[1] + 2*coeffs[2]*xt + 3*coeffs[3]*pow(xt,2));
+            fg[0] += th_cost_weight * CppAD::pow(vars[nth + t]-fth, 2);
+
+            fg[0]+=v_cost_weight*CppAD::pow(vars[nv+t]-m_ref_v,2);
+
+        }
+
+        // Costs for steering (delta) and acceleration (a)
+        for (int t = 0; t < N-1; t++) {
+            fg[0] += vth_cost_weight * CppAD::pow(vars[nvth + t], 2);
+            fg[0] += a_cost_weight * CppAD::pow(vars[nv + t+1]-vars[nv+t], 2);
+            fg[0] += ath_cost_weight * CppAD::pow(vars[nvth + t+1]-vars[nvth+t], 2);
+        }
+
+        /////////////////////
+        fg[1 + nx] = vars[nx]-vars[nv]*dt;
+        fg[1 + ny] = vars[ny];
+        fg[1 + nth] = vars[nth]-vars[nvth]*dt;
+
+        // The rest of the constraints
+        for (int t = 1; t < N; t++) {
+            // State at time t + 1
+            AD<double> x1 = vars[nx + t];
+            AD<double> y1 = vars[ny + t];
+            AD<double> th1 = vars[nth + t];
+            AD<double> v1 = vars[nv + t];
+            AD<double> vth1 = vars[nvth + t];
+
+            // State at time t
+            AD<double> x0 = vars[nx + t -1];
+            AD<double> y0 = vars[ny + t -1];
+            AD<double> th0 = vars[nth + t-1];
+            AD<double> v0 = vars[nv + t-1];
+            AD<double> vth0 = vars[nvth + t-1];
+
+            // Actuator constraints at time t only
+
+            // Setting up the rest of the model constraints
+            fg[1 + nx + t] = x1 - (x0 + v0 * CppAD::cos(th0) * dt);
+            fg[1 + ny + t] = y1 - (y0 + v0 * CppAD::sin(th0) * dt);
+            fg[1 + nth + t] = th1 - (th0 + vth0 * dt);
+
+        }
+
+        fg[1 + nv]=(vars[nv]-m_v)/dt;
+        fg[1+nvth]=(vars[nvth]-m_vth)/dt;
+        for(int t=1;t<N;++t)
+        {
+            fg[1+nv+t]=(vars[nv+t]-vars[nv+t-1])/dt;
+            fg[1+nvth+t]=(vars[nvth+t]-vars[nvth+t-1])/dt;
+        }
+
+        ////  add
+
+    }
+};
+
+//
+// MPC class definition implementation.
+//
+MonitorMPC::MonitorMPC() {}
+MonitorMPC::~MonitorMPC() {}
+
+bool MonitorMPC::Solve(Eigen::VectorXd state, Eigen::VectorXd coeffs, std::vector<double>& out) {
+    bool ok = true;
+    typedef CPPAD_TESTVECTOR(double) Dvector;
+
+    // State vector holds all current values neede for vars below
+    double v = state[0];
+    double vth = state[1];
+    double ref_v = state[2];
+    double max_v=state[3];
+    double dt=state[4];
+
+
+    // Setting the number of model variables (includes both states and inputs).
+    // N * state vector size + (N - 1) * 2 actuators (For steering & acceleration)
+    size_t n_vars = N * 5;
+    // Setting the number of constraints
+    size_t n_constraints = N * 5;
+
+    // Initial value of the independent variables.
+    // SHOULD BE 0 besides initial state.
+    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);
+    // Sets lower and upper limits for variables.
+    // Set all non-actuators upper and lowerlimits
+    // to the max negative and positive values.
+    for (int i = 0; i < n_vars; i++) {
+        vars_lowerbound[i] = -1.0e19;
+        vars_upperbound[i] = 1.0e19;
+    }
+
+    //// limit  v
+    for (int i = nv; i < nv+N; i++) {
+        vars_lowerbound[i] = -max_v;
+        vars_upperbound[i] = max_v;
+    }
+
+    ////limint vth
+    for (int i = nvth; i < nvth+N; i++) {
+        vars_lowerbound[i] = -0.5;
+        vars_upperbound[i] = 0.5;
+    }
+
+    // Lower and upper limits for the constraints
+    // Should be 0 besides initial state.
+    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;
+    }
+    //// acc v
+    for (int i = nv; i < nv+N; i++) {
+        constraints_lowerbound[i] = -5.0;
+        constraints_upperbound[i] = 5.0;
+    }
+    //// acc vth
+    for (int i = nvth; i < nvth+N; i++) {
+        constraints_lowerbound[i] = -2.5;
+        constraints_upperbound[i] = 2.5;
+    }
+
+
+    // object that computes objective and constraints
+    FG_eval fg_eval(coeffs,v,vth,ref_v,dt);
+
+    //
+    // NOTE: You don't have to worry about these options
+    //
+    // options for IPOPT solver
+    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
+    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 << "Cost " << cost << std::endl;
+
+    // Return the first actuator values, along with predicted x and y values to plot in the simulator.
+    out.clear();
+    m_trajectory.clear();
+    out.push_back(solution.x[nv]);
+    out.push_back(solution.x[nvth]);
+    for (int i = 0; i < N; ++i) {
+        out.push_back(solution.x[nx + i]);
+        out.push_back(solution.x[ny + i]);
+        tf::Pose pose;
+        pose.setOrigin(tf::Vector3(solution.x[nx+i],solution.x[ny+i],0));
+        m_trajectory.push_back(pose);
+    }
+    printf(" cost : %.3f\n",cost);
+    return ok;
+
+}

mpc_control/src/mpc_two_vehicle/MonitorMPC.h

@@ -0,0 +1,24 @@
+//
+// Created by zx on 2019/8/28.
+//
+
+#ifndef MONITORMPC_H
+#define MONITORMPC_H
+
+#include <vector>
+#include <Eigen/Core>
+#include <tf/transform_datatypes.h>
+
+class MonitorMPC
+{
+public:
+    MonitorMPC();
+    virtual ~MonitorMPC();
+
+    bool Solve(Eigen::VectorXd state, Eigen::VectorXd coeffs, std::vector<double>& out);
+
+    std::vector<tf::Pose> m_trajectory;
+};
+
+
+#endif //MONITORMPC_H

mpc_control/src/mpc_two_vehicle/mpc_two_vehicle_controller_node.cpp

@@ -0,0 +1,240 @@
+#include "mpc_two_vehicle_controller_node.h"
+
+mpc_two_vehicle_controller::mpc_two_vehicle_controller(ros::NodeHandle nh) : nh_(nh)
+{
+  omega = 0;
+  velocity = 0;
+  m_ref_vel = 2.0;
+  m_path_publish_count = 0;
+  last_pos = pcl::PointXYZ(0, 0, 0);
+  curr_pos = pcl::PointXYZ(0, 0, 0);
+
+  //发布与订阅
+  vel_publisher_ = nh_.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel", 10);
+  global_path_publisher_ = nh_.advertise<nav_msgs::Path>("global_path", 2);
+  local_path_publisher_ = nh_.advertise<nav_msgs::Path>("local_path", 2);
+  pose_sub_ = nh_.subscribe<turtlesim::Pose>("/turtle1/pose", 1, &mpc_two_vehicle_controller::poseHandler, this);
+  target_sub_ = nh_.subscribe<geometry_msgs::PoseStamped>("/move_base_simple/goal", 1, &mpc_two_vehicle_controller::targetHandler, this);
+}
+
+mpc_two_vehicle_controller::~mpc_two_vehicle_controller()
+{
+}
+
+// 目标设置回调
+void mpc_two_vehicle_controller::targetHandler(const geometry_msgs::PoseStamped::ConstPtr &target_pose_msg)
+{
+  m_edge_mutex.lock();
+  tf::poseMsgToTF(target_pose_msg->pose, m_target_pose);
+  Eigen::Matrix2d base_rotation;
+  Eigen::Matrix2d target_rotation;
+
+  base_rotation << m_current_pose.getBasis()[0][0], m_current_pose.getBasis()[0][1], m_current_pose.getBasis()[1][0], m_current_pose.getBasis()[1][1];
+  target_rotation << m_target_pose.getBasis()[0][0], m_target_pose.getBasis()[0][1], m_target_pose.getBasis()[1][0], m_target_pose.getBasis()[1][1];
+
+  m_base.setIdentity();
+  m_base.rotate(base_rotation);
+  m_base.pretranslate(Eigen::Vector2d(m_current_pose.getOrigin().getX(), m_current_pose.getOrigin().getY()));
+
+  m_target.setIdentity();
+  m_target.rotate(target_rotation);
+  m_target.pretranslate(Eigen::Vector2d(m_target_pose.getOrigin().getX(), m_target_pose.getOrigin().getY()));
+  bool create_trajectory_result = m_traj_tool.create_trajectory(m_base, m_target, m_trajectory, 10);
+  m_edge_mutex.unlock();
+
+  // 发布全局路径
+  if (create_trajectory_result)
+  {
+    m_global_path.header.frame_id = "map";
+    m_global_path.header.seq = ++m_path_publish_count;
+    m_global_path.header.stamp = ros::Time::now();
+    m_global_path.poses.clear();
+    for (size_t i = 0; i < m_trajectory.size(); i++)
+    {
+      geometry_msgs::PoseStamped t_pose;
+      t_pose.header = m_global_path.header;
+      t_pose.pose.position.x = m_trajectory[i].translation().x();
+      t_pose.pose.position.y = m_trajectory[i].translation().y();
+      double theta = atan2(m_trajectory[i].rotation()(1, 0), m_trajectory[i].rotation()(0, 0));
+      t_pose.pose.orientation.w = cos(theta / 2.0);
+      t_pose.pose.orientation.z = sin(theta / 2.0);
+      m_global_path.poses.push_back(t_pose);
+    }
+    global_path_publisher_.publish(m_global_path);
+  }
+}
+
+// 当前位姿消息回调
+void mpc_two_vehicle_controller::poseHandler(const turtlesim::Pose::ConstPtr &pose_msg)
+{
+  std::lock_guard<std::mutex> lck(m_edge_mutex);
+  m_current_pose.setOrigin(tf::Vector3(pose_msg->x, pose_msg->y, 0));
+  m_current_pose.setRotation(tf::Quaternion(0, 0, sin(pose_msg->theta/2.0), cos(pose_msg->theta/2.0)));
+
+  std::chrono::time_point<std::chrono::system_clock> start = std::chrono::system_clock::now();
+
+  //发布tf
+  tf::StampedTransform transform_msg(m_current_pose, ros::Time::now(), "map", "base_link");
+  tf_broadcaster_.sendTransform(transform_msg);
+
+  // 空路径
+  if (m_trajectory.size() < 1){
+    // std::cout<<"空路径"<<std::endl;
+    return;
+  }
+
+  // 在目标点附近
+  if((m_current_pose.getOrigin() - m_target_pose.getOrigin()).length() < 0.04){
+    // std::cout<<"目标点附近"<<std::endl;
+    return;
+  }
+
+  // 筛选前方N个点
+  std::vector<tf::Pose> t_poses, t_partial_poses, t_pred_poses;
+  int count = 0;
+  // std::cout<<"current pose:"<<current_pose.getOrigin().getX()<<", "<<current_pose.getOrigin().getY()<<std::endl;
+  for (int i = 0; i < m_trajectory.size(); i++)
+  {
+    t_poses.push_back(tf::Pose(tf::Quaternion(), tf::Vector3(m_trajectory[i].translation().x(), m_trajectory[i].translation().y(), 0)));
+  }
+  if (filterPath(m_current_pose, t_poses, t_partial_poses, 10))
+  {
+    // std::cout<<"thetas: "<<current_pose.getRotation().getX()<<", "<<current_pose.getRotation().getY()<<", "<<current_pose.getRotation().getZ()<<", "<<current_pose.getRotation().getW()<<std::endl;
+    tf::Vector3 orient(cos(pose_msg->theta), sin(pose_msg->theta), 0.0);
+    tf::Vector3 target_direction = m_target_pose.getOrigin() - m_current_pose.getOrigin();
+
+    // 终点附近限制速度
+    if (target_direction.length() < 1)
+    {
+      if (target_direction.length() < 0.02)
+      {
+        m_ref_vel = 0;
+      }
+      // m_ref_vel *= 0.95;
+      m_ref_vel = fmin(m_ref_vel, 0.2);
+      m_ref_vel = fmax(m_ref_vel, 0.02);
+    }
+    else
+    {
+      m_ref_vel = 2.0;
+    }
+
+    // 调整速度方向
+    if (target_direction.dot(orient) > 0)
+    {
+      m_ref_vel = m_ref_vel < 0 ? -m_ref_vel : m_ref_vel;
+    }
+    else
+    {
+      m_ref_vel = m_ref_vel > 0 ? -m_ref_vel : m_ref_vel;
+    }
+    // std::cout<<"m_ref_vel: "<<m_ref_vel<<std::endl;
+
+    // 筛选局部路径
+    double velo = pose_msg->linear_velocity;
+    double velo_angle = pose_msg->angular_velocity;
+
+    std::cout << "start solve" << std::endl;
+    m_mpc_model.set_global_target(m_target_pose);
+    if (m_mpc_model.Solve(m_ref_vel, t_partial_poses, m_current_pose, velo, velo_angle, t_pred_poses))
+    {
+      std::cout << "solve complete" << std::endl;
+
+      geometry_msgs::Twist velocity;
+      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 = velo_angle; //- (v_right-v_left)/Lf;//turtlesim uses left hand angle, if angle less than 0, it means turn left.
+      vel_publisher_.publish(velocity);
+    }
+    std::chrono::time_point<std::chrono::system_clock> end1 = std::chrono::system_clock::now();
+    std::cout << "time consumed: " << std::chrono::duration_cast<std::chrono::milliseconds>(end1 - start).count() << "ms."<< std::endl;
+
+    //发布局部路径
+    nav_msgs::Path m_local_path;
+    m_local_path.header.seq = m_path_publish_count;
+    m_local_path.header.stamp = ros::Time::now();
+    m_local_path.header.frame_id = "map";
+    for (size_t i = 2; i < t_pred_poses.size(); i++)
+    {
+      geometry_msgs::PoseStamped temp;
+      temp.header = m_local_path.header;
+      t_pred_poses[i].setRotation(t_pred_poses[i].getRotation().normalize());
+      tf::poseTFToMsg(t_pred_poses[i], temp.pose);
+      temp.pose.position.x += m_current_pose.getOrigin().getX();
+      temp.pose.position.y += m_current_pose.getOrigin().getY();
+      temp.pose.position.z += m_current_pose.getOrigin().getZ();
+      m_local_path.poses.push_back(temp);
+    }
+    local_path_publisher_.publish(m_local_path);
+
+    m_last_theta = pose_msg->theta;
+    last_pos = curr_pos;
+  }else{
+    std::cout<<"filter path failed."<<std::endl;
+  }
+}
+
+//获取小车坐标系中，x前进方向 num 个点，用于拟合
+bool mpc_two_vehicle_controller::filterPath(tf::Pose pose, std::vector<tf::Pose> poses, std::vector<tf::Pose> &out, int numOfPoints, bool forward)
+{
+  if (poses.size() < numOfPoints)
+  {
+    std::cout<<"lack of poses."<<std::endl;
+    return false;
+  }
+
+  out.clear();
+  // 平移加反向旋转到小车坐标系
+  for (int i = 0; i < poses.size(); i++)
+  {
+    double x = poses[i].getOrigin().getX() - pose.getOrigin().getX();
+    double y = poses[i].getOrigin().getY() - pose.getOrigin().getY();
+    double trans_x = x * pose.getBasis()[0][0] + y * pose.getBasis()[1][0];
+    double trans_y = x * pose.getBasis()[0][1] + y * pose.getBasis()[1][1];
+    bool direction_condition = forward ? (trans_x >= 0) : (trans_x < 0);
+    if (direction_condition && out.size() < numOfPoints)
+    {
+      tf::Pose point;
+      float nx = trans_x * pose.getBasis()[0][0] + trans_y * pose.getBasis()[0][1];
+      float ny = trans_x * pose.getBasis()[1][0] + trans_y * pose.getBasis()[1][1];
+      point.setOrigin(tf::Vector3(nx + pose.getOrigin().getX(),
+                                  ny + pose.getOrigin().getY(), 0));
+      out.push_back(point);
+    }
+  }
+  if (out.size() <= 0){
+    std::cout<<"cannot find forward poses."<<std::endl;
+    return false;
+  }
+  return true;
+}
+
+int main(int argc, char **argv)
+{
+  ros::init(argc, argv, "mpc_two_vehicle_controller_node");
+  ros::NodeHandle nh;
+  mpc_two_vehicle_controller controller(nh);
+  ros::Rate loop_rate(15);
+
+  // double px = 5.54;
+  // double py = 5.54;
+  // double psi = 0;
+  // double v_l = 0;
+  // double v_r = 0;
+  // double a_l = 0;
+  // double a_r = 0;
+
+  while (ros::ok())
+  {
+
+    //controller.processData();
+
+    ros::spinOnce();
+    loop_rate.sleep();
+  }
+  //controller.~mpc_two_vehicle_controller();
+  return 0;
+}

mpc_control/src/mpc_two_vehicle/mpc_two_vehicle_controller_node.h

@@ -0,0 +1,80 @@
+#ifndef MPC_TWO_VEHICLE_CONTROLLER_NODE_HH
+#define MPC_TWO_VEHICLE_CONTROLLER_NODE_HH 
+
+#include "mpc_two_vehicle_model.h"
+#include "trajectory_tool.h"
+
+#include <ros/ros.h>
+#include <geometry_msgs/PoseStamped.h>
+#include <tf/transform_listener.h>
+#include <tf/transform_broadcaster.h>
+#include <tf/transform_datatypes.h>
+#include <geometry_msgs/Twist.h>
+#include <nav_msgs/Path.h>
+
+#include <turtlesim/Pose.h>
+
+#include <vector>
+#include <Eigen/Core>
+#include <Eigen/QR>
+#include <cppad/cppad.hpp>
+#include <cppad/ipopt/solve.hpp>
+#include <pcl/point_types.h>
+#include <chrono>
+#include <mutex>
+
+// 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_two_vehicle_controller{
+
+public:
+    mpc_two_vehicle_controller(ros::NodeHandle nh);
+    ~mpc_two_vehicle_controller();
+    // 小乌龟位姿回调，筛选局部路径，mpc优化生成v与vth，下发到小乌龟
+    void poseHandler(const turtlesim::Pose::ConstPtr& pose_msg);
+    // 目标设置回调，rviz上设置目标后，更新全局路径
+    void targetHandler(const geometry_msgs::PoseStamped::ConstPtr& target_pose_msg);
+    // 筛选局部路径
+    bool filterPath(tf::Pose pose,std::vector<tf::Pose> poses, std::vector<tf::Pose>& out, int numOfPoints, bool forward = true);
+    
+public:
+    size_t N = 10;
+    double dt = 0.1;
+
+private:
+    Trajectory_tool m_traj_tool;
+    Mpc_two_vehicle_model m_mpc_model;
+    std::mutex m_edge_mutex; // 读写端点位姿互斥锁
+    Eigen::Isometry2d m_base;
+    Eigen::Isometry2d m_target;
+    trajectory_type m_trajectory;
+
+    nav_msgs::Path m_global_path;
+    nav_msgs::Path m_local_path;
+    int m_path_publish_count;
+
+    ros::NodeHandle nh_;
+    tf::TransformBroadcaster tf_broadcaster_;
+    ros::Publisher vel_publisher_;
+    ros::Publisher global_path_publisher_;
+    ros::Publisher local_path_publisher_;
+    ros::Subscriber pose_sub_;
+    ros::Subscriber target_sub_;
+
+    // 从话题获取到的当前位姿
+    tf::Pose m_current_pose, m_target_pose;
+
+    double velocity, omega, m_ref_vel, m_last_theta;
+    pcl::PointXYZ last_pos, curr_pos;
+    std::vector<double> ptsx;
+    std::vector<double> ptsy;
+};
+
+#endif /* MPC_TWO_VEHICLE_CONTROLLER_NODE_HH */

mpc_control/src/mpc_two_vehicle/mpc_two_vehicle_model.cpp

@@ -0,0 +1,369 @@
+#include "mpc_two_vehicle_model.h"
+
+using CppAD::AD;
+
+// 设置静态变量值
+double Mpc_two_vehicle_model::ref_v = 2.0;
+double Mpc_two_vehicle_model::max_vel = 3.0;
+double Mpc_two_vehicle_model::dt = 0.1;
+
+class FG_eval_model
+{
+public:
+  // Fitted polynomial coefficients
+  Eigen::VectorXd coeffs;
+  double m_ref_v, m_dt;
+  double m_v, m_vth;
+  double m_global_target_x, m_global_target_y;
+  FG_eval_model(Eigen::VectorXd coeffs,double v,double vth,double ref_v,double dt, 
+                double global_target_x, double global_target_y)
+  {
+    this->coeffs = coeffs;
+    m_v = v;
+    m_vth = vth;
+    m_ref_v = ref_v;
+    m_dt = dt;
+    m_global_target_x = global_target_x;
+    m_global_target_y = global_target_y;
+  }
+
+  typedef CPPAD_TESTVECTOR(AD<double>) ADvector;
+  void operator()(ADvector &fg, const ADvector &vars)
+  {
+    fg[0] = 0;
+
+    // Weights for how "important" each cost is - can be tuned
+    const int y_cost_weight = 2000;
+    const int th_cost_weight = 1000;
+    const int v_cost_weight = 20;
+    const int vth_cost_weight = 100;
+    const int a_cost_weight = 5;
+    const int ath_cost_weight = 100;
+
+    const int target_cost_weight = 100;
+
+    std::cout << "-------------"<< std::endl;
+    // Cost for CTE, psi error and velocity
+    for (int t = 0; t < Mpc_two_vehicle_model::N; t++)
+    {
+      AD<double> xt = vars[ Mpc_two_vehicle_model::x_start + t];
+      AD<double> fx = coeffs[0] + coeffs[1] * xt + coeffs[2] * CppAD::pow(xt, 2) + coeffs[3] * CppAD::pow(xt, 3);
+      fg[0] += y_cost_weight * CppAD::pow(vars[Mpc_two_vehicle_model::y_start + t] - fx, 2);
+
+      AD<double> fth = CppAD::atan(coeffs[1] + 2 * coeffs[2] * xt + 3 * coeffs[3] * CppAD::pow(xt, 2));
+      fg[0] += th_cost_weight * CppAD::pow(vars[Mpc_two_vehicle_model::theta_start + t] - fth, 2);
+      fg[0] += v_cost_weight * CppAD::pow(vars[Mpc_two_vehicle_model::v_start + t] - m_ref_v, 2);
+      fg[0] += vth_cost_weight * CppAD::pow(vars[Mpc_two_vehicle_model::vth_start + t], 2);
+      // std::cout<<"x, y, theta, v_ref: "<< xt << ", "<<fx<<", " << fth << ", " << m_ref_v <<std::endl;
+    }
+
+    // 增加对接近目标时的速度限制, 接近1m内才生效
+    AD<double> x0 = vars[Mpc_two_vehicle_model::x_start + Mpc_two_vehicle_model::N - 1];
+    AD<double> y0 = vars[Mpc_two_vehicle_model::y_start + Mpc_two_vehicle_model::N - 1];
+    AD<double> th0 = vars[Mpc_two_vehicle_model::theta_start + Mpc_two_vehicle_model::N - 1];
+    AD<double> v0 = vars[Mpc_two_vehicle_model::v_start + Mpc_two_vehicle_model::N - 1];
+    AD<double> the_last_predict_x = x0 + v0 * CppAD::cos(th0) * m_dt;
+    AD<double> the_last_predict_y = y0 + v0 * CppAD::sin(th0) * m_dt;
+    AD<double> dist = CppAD::sqrt(CppAD::pow(the_last_predict_x - m_global_target_x, 2) + CppAD::pow(the_last_predict_y - m_global_target_y, 2));
+    AD<double> inv_sigmoid = 1 + CppAD::exp(5 * dist - 5);
+    std::cout << "dist, inv sig， calc: " << dist << ", "<< inv_sigmoid << ", " << 1 / (inv_sigmoid * dist) << std::endl;
+    fg[0] += target_cost_weight * dist / inv_sigmoid ;
+
+    // Costs for steering (delta) and acceleration (a)
+    for (int t = 0; t < Mpc_two_vehicle_model::N - 1; t++)
+    {
+      fg[0] += a_cost_weight * CppAD::pow(vars[Mpc_two_vehicle_model::acc_start + t], 2);
+      fg[0] += ath_cost_weight * CppAD::pow(vars[Mpc_two_vehicle_model::acc_vth_start + t], 2);
+    }
+
+    /////////////////////
+    fg[1 + Mpc_two_vehicle_model::x_start] = vars[Mpc_two_vehicle_model::x_start];
+    fg[1 + Mpc_two_vehicle_model::y_start] = vars[Mpc_two_vehicle_model::y_start];
+    fg[1 + Mpc_two_vehicle_model::theta_start] = vars[Mpc_two_vehicle_model::theta_start];
+    fg[1 + Mpc_two_vehicle_model::v_start] = vars[Mpc_two_vehicle_model::v_start];
+    fg[1 + Mpc_two_vehicle_model::vth_start] = vars[Mpc_two_vehicle_model::vth_start];
+
+    // The rest of the constraints
+    for (int t = 1; t < Mpc_two_vehicle_model::N; t++)
+    {
+      // State at time t + 1
+      AD<double> x1 = vars[Mpc_two_vehicle_model::x_start + t];
+      AD<double> y1 = vars[Mpc_two_vehicle_model::y_start + t];
+      AD<double> th1 = vars[Mpc_two_vehicle_model::theta_start + t];
+      AD<double> v1 = vars[Mpc_two_vehicle_model::v_start + t];
+      AD<double> vth1 = vars[Mpc_two_vehicle_model::vth_start + t];
+
+      // State at time t
+      AD<double> x0 = vars[Mpc_two_vehicle_model::x_start + t - 1];
+      AD<double> y0 = vars[Mpc_two_vehicle_model::y_start + t - 1];
+      AD<double> th0 = vars[Mpc_two_vehicle_model::theta_start + t - 1];
+      AD<double> v0 = vars[Mpc_two_vehicle_model::v_start + t - 1];
+      AD<double> vth0 = vars[Mpc_two_vehicle_model::vth_start + t - 1];
+
+      // Actuator constraints at time t only
+      AD<double> acc0 = vars[Mpc_two_vehicle_model::acc_start + t - 1];
+      AD<double> acc_vth0 = vars[Mpc_two_vehicle_model::acc_vth_start + t - 1];
+
+      // Setting up the rest of the model constraints
+      // fg[1 + Mpc_two_vehicle_model::x_start + t] = x1 - (x0 + (v0 + 0.5 * acc0 * m_dt) * CppAD::cos(th0 + 0.5 * vth0 * m_dt) * m_dt);
+      // fg[1 + Mpc_two_vehicle_model::y_start + t] = y1 - (y0 + (v0 + 0.5 * acc0 * m_dt) * CppAD::sin(th0 + 0.5 * vth0 * m_dt) * m_dt);
+      fg[1 + Mpc_two_vehicle_model::x_start + t] = x1 - (x0 + v0 * CppAD::cos(th0) * m_dt);
+      fg[1 + Mpc_two_vehicle_model::y_start + t] = y1 - (y0 + v0 * CppAD::sin(th0) * m_dt);
+      fg[1 + Mpc_two_vehicle_model::theta_start + t] = th1 - (th0 + vth0 * m_dt);
+      fg[1 + Mpc_two_vehicle_model::v_start + t] = v1 - (v0 + acc0 * m_dt);
+      fg[1 + Mpc_two_vehicle_model::vth_start + t] = vth1 - (vth0 + acc_vth0 * m_dt);
+    }
+  }
+};
+
+Mpc_two_vehicle_model::Mpc_two_vehicle_model()
+{
+  // ref_v = 2.0;
+  last_velocity = 0.0;
+  last_velocity_theta = 0.0;
+  m_last_pose.setIdentity();
+  m_global_target.setIdentity();
+  mb_has_global_target = false;
+}
+
+Mpc_two_vehicle_model::~Mpc_two_vehicle_model()
+{
+}
+
+void Mpc_two_vehicle_model::set_global_target(tf::Pose target){
+  m_global_target = target;
+  mb_has_global_target = true;
+}
+
+// 求y
+double Mpc_two_vehicle_model::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_two_vehicle_model::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 Mpc_two_vehicle_model::Solve(double ref_velocity, std::vector<tf::Pose> ref_poses, tf::Pose curr_pose, double &v, double &vth, std::vector<tf::Pose> &pred_poses)
+{
+  ref_v = ref_velocity;
+  // 转换坐标后的局部路径与当前位姿
+  Eigen::VectorXd ptsx_car(ref_poses.size());
+  Eigen::VectorXd ptsy_car(ref_poses.size());
+  double t_global_target_trans_x, t_global_target_trans_y;
+  // 反向旋转
+  for (int i = 0; i < ref_poses.size(); i++)
+  {
+    double t_transformed_x = ref_poses[i].getOrigin().getX() - curr_pose.getOrigin().getX();
+    double t_transformed_y = ref_poses[i].getOrigin().getY() - curr_pose.getOrigin().getY();
+    ptsx_car[i] = t_transformed_x * curr_pose.getBasis()[0][0] + t_transformed_y * curr_pose.getBasis()[1][0];
+    ptsy_car[i] = t_transformed_x * curr_pose.getBasis()[0][1] + t_transformed_y * curr_pose.getBasis()[1][1];
+    // 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;
+  }
+  double trans_x = m_global_target.getOrigin().getX() - curr_pose.getOrigin().getX();
+  double trans_y = m_global_target.getOrigin().getY() - curr_pose.getOrigin().getY();
+  t_global_target_trans_x = trans_x * curr_pose.getBasis()[0][0] + trans_y * curr_pose.getBasis()[1][0];
+  t_global_target_trans_y = trans_x * curr_pose.getBasis()[0][1] + trans_y * curr_pose.getBasis()[1][1];
+
+  //拟合求斜率与y方向误差
+  if(ptsx_car.size() < 3 || ptsy_car.size() < 3){
+    std::cout<<"点数过少，无法拟合"<<std::endl;
+    return false;
+  }
+  auto coeffs = polyfit(ptsx_car, ptsy_car, 3);
+  // std::cout<<"coeffs: "<<coeffs<<std::endl;
+  // if(std::isnan(coeffs[0])||std::isnan(coeffs[1])||std::isnan(coeffs[2])||std::isnan(coeffs[3]))
+  //   return;
+
+  bool ok = true;
+  typedef CPPAD_TESTVECTOR(double) Dvector;
+
+  size_t n_vars = N * 5 + (N - 1) * 2;
+
+  size_t n_constraints = N * 5;
+
+  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);
+  // x, y constrains
+  for (int i = 0; i < theta_start; i++)
+  {
+    vars_lowerbound[i] = -1.0e2;
+    vars_upperbound[i] = 1.0e2;
+  }
+  // theta constrains
+  for (int i = theta_start; i < v_start; i++)
+  {
+    vars_lowerbound[i] = -M_PI;
+    vars_upperbound[i] = M_PI;
+  }
+  // velocity constrains
+  for (int i = v_start; i < vth_start; i++)
+  {
+    vars_lowerbound[i] = -max_vel;
+    vars_upperbound[i] = max_vel;
+  }
+  // velocity theta constrains
+  for (int i = vth_start; i < acc_start; i++)
+  {
+    vars_lowerbound[i] = -M_PI;
+    vars_upperbound[i] = M_PI;
+  }
+  // acceleration of velocity constrains
+  for (int i = acc_start; i < acc_vth_start; i++)
+  {
+    vars_lowerbound[i] = -2.0;
+    vars_upperbound[i] = 2.0;
+  }
+  // acceleration of velocity theta
+  for (int i = acc_vth_start; i < n_vars; i++)
+  {
+    vars_lowerbound[i] = -M_PI;
+    vars_upperbound[i] = M_PI;
+  }
+
+  // 函数约束
+  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;
+  }
+  // v
+  for (int i = v_start; i < v_start + N; i++)
+  {
+    constraints_lowerbound[i] = -max_vel;
+    constraints_upperbound[i] = max_vel;
+  }
+  // vth
+  for (int i = vth_start; i < vth_start + N; i++)
+  {
+    constraints_lowerbound[i] = -M_PI;
+    constraints_upperbound[i] = M_PI;
+  }
+
+  double theta_diff = curr_pose.getRotation().getAngle() - m_last_pose.getRotation().getAngle();
+  // Predict state after latency
+  double pred_v = (curr_pose.getOrigin() - m_last_pose.getOrigin()).length() / dt;
+  double pred_theta = theta_diff / dt;
+
+  // object that computes objective and constraints
+  FG_eval_model fg_eval(coeffs, pred_v, pred_theta, ref_v, dt, t_global_target_trans_x, t_global_target_trans_y);
+
+  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 += "String  sb           yes\n";
+  // maximum number of iterations
+  // options += "Integer max_iter     80\n";
+  // approximate accuracy in first order necessary conditions;
+  // see Mathematical Programming, Volume 106, Number 1,
+  // Pages 25-57, Equation (6)
+  // options += "Numeric tol          1e-4\n";
+  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_model>(
+      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
+  std::vector<double> solved;
+  if (ok)
+  {
+    // // 先存速度、角速度，后存预测路径
+    // solved.push_back(solution.x[acc_start]);
+    // solved.push_back(solution.x[acc_omega_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<<"path: "<<solution.x[x_start + i]<<", "<<solution.x[y_start + i]<<std::endl;
+    // }
+    // // std::cout<<"return solved"<<std::endl;
+
+    //更新 v vth
+    v = solution.x[v_start];// + fabs(solution.x[acc_start]) * dt;
+    vth = solution.x[vth_start];// + fabs(solution.x[acc_omega_start]) * dt;
+    pred_poses.clear();
+    for (size_t i = 0; i < N; i++)
+    {
+      double real_x = solution.x[x_start + i]*curr_pose.getBasis()[0][0] + solution.x[y_start + i]*curr_pose.getBasis()[0][1];
+      double real_y = solution.x[x_start + i]*curr_pose.getBasis()[1][0] + solution.x[y_start + i]*curr_pose.getBasis()[1][1];
+      // pred_poses.push_back(tf::Pose(tf::Quaternion(), tf::Vector3(solution.x[x_start + i], solution.x[y_start + i], 0.0)));      
+      pred_poses.push_back(tf::Pose(
+        tf::Quaternion(0, 0, sin(solution.x[theta_start]/2.0), cos(solution.x[theta_start]/2.0)),
+        tf::Vector3(real_x, real_y, 0.0)
+      ));
+      // std::cout<<"path: "<<solution.x[x_start + i]<<", "<<solution.x[y_start + i]<<std::endl;
+    }
+
+    std::cout << "model   v: " << v << ", " << vth << ", " << ref_v << std::endl;
+    m_last_pose = curr_pose;
+    last_velocity = v;
+    last_velocity_theta = vth;
+    return true;
+  }
+  else
+  {
+    m_last_pose.setIdentity();
+    return false;
+  }
+}

mpc_control/src/mpc_two_vehicle/mpc_two_vehicle_model.h

@@ -0,0 +1,68 @@
+#ifndef MPC_TWO_VEHICLE_MODEL_HH
+#define MPC_TWO_VEHICLE_MODEL_HH 
+
+#include <tf/transform_datatypes.h>
+
+#include <vector>
+#include <Eigen/Core>
+#include <Eigen/QR>
+#include <cppad/cppad.hpp>
+#include <cppad/ipopt/solve.hpp>
+/**
+ * mpc 两车模型。每车定义为矩形，双前轮主动，双后轮从动。两车头朝外，中间软性连接。
+ * 
+ * */
+class Mpc_two_vehicle_model{
+
+public:
+    // 构造函数
+    Mpc_two_vehicle_model();
+    // 析构函数
+    ~Mpc_two_vehicle_model();
+    // 模型解析
+    // 参数:
+    //      ref_velocity 参考速度，两车具有相同的参考速度
+    //      ref_poses 规划的路径
+    //      curr_pose 当前位姿
+    //      v 下发速度
+    //      vth 下发角速度
+    bool Solve(double ref_velocity, std::vector<tf::Pose> ref_poses,tf::Pose curr_pose, double& v, double& vth, std::vector<tf::Pose> &pred_poses);
+
+    void set_global_target(tf::Pose target);
+
+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 = 12;
+    // static constexpr double Lf=1;
+
+    // x，y； theta旋转角； v速度值； distance_error实际路径误差； epsi角度误差； a加速度
+    // N个点，N个旋转角，N个速度值，N个角速度值, N个加速度值
+    static const size_t x_start = 0;
+    static const size_t y_start = x_start + N;
+    static const size_t theta_start = y_start + N;
+    static const size_t v_start = theta_start + N;
+    static const size_t vth_start = v_start + N;
+
+    static const size_t acc_start = vth_start + N - 1;
+    static const size_t acc_vth_start = acc_start + N - 1;
+
+    static double dt;
+    static double ref_v;
+    static double max_vel;
+
+    bool forward = true;
+
+private:
+
+    double last_velocity, last_velocity_theta;
+    tf::Pose m_last_pose;
+    tf::Pose m_global_target;
+    bool mb_has_global_target;
+};
+
+#endif /* MPC_TWO_VEHICLE_MODEL_HH */

mpc_control/src/mpc_two_vehicle/trajectory_tool.cpp

@@ -0,0 +1,139 @@
+#include "trajectory_tool.h"
+#include <iostream>
+// 构造函数
+Trajectory_tool::Trajectory_tool()
+{
+}
+
+// 析构函数
+Trajectory_tool::~Trajectory_tool()
+{
+}
+
+// 处理90度附近斜率无限大的情况,角度(sin,cos)近似为89.5度(0.99995, 0.01)或90.5度(0.99995,-0.01)
+void Trajectory_tool::max_gradient_limit(Eigen::Matrix2d &rotation)
+{
+    if (rotation(0, 0) > 0 && rotation(0, 0) < 0.01)
+    {
+        rotation << 0.01, -0.9995, 0.9995, 0.01;
+    }
+    else if (rotation(0, 0) < 0 && rotation(0, 0) > -0.01)
+    {
+        rotation << -0.01, -0.9995, 0.9995, -0.01;
+    }
+}
+
+// 生成路径
+bool Trajectory_tool::create_trajectory(Eigen::Isometry2d base, Eigen::Isometry2d target, trajectory_type &trajectory, int extend_num)
+{
+    // 1.将两点变换到x轴，base移到原点，先平移后旋转
+    Eigen::Vector2d t_base_point(0, 0);
+    Eigen::Vector2d t_target_point = target.translation() - base.translation();
+    double t_euclidean_distance = sqrt(t_target_point.transpose() * t_target_point);
+    // 基点与目标点相同，无法生成路径，直接退出
+    if (t_euclidean_distance <= 0)
+    {
+        return false;
+    }
+    double t_rotation_cos = t_target_point.x() / t_euclidean_distance;
+    double t_rotation_sin = t_target_point.y() / t_euclidean_distance;
+    Eigen::Isometry2d t_origin_transformation; // 用于保存对齐x轴产生的变换
+    t_origin_transformation.setIdentity();
+    Eigen::Matrix2d t_rotation_matrix;
+    t_rotation_matrix.setIdentity();
+    t_rotation_matrix << t_rotation_cos, -t_rotation_sin, t_rotation_sin, t_rotation_cos;
+    t_origin_transformation.rotate(t_rotation_matrix);
+    t_origin_transformation.pretranslate(base.translation());
+    t_target_point = t_rotation_matrix.transpose() * t_target_point; // 目标点反向旋转到x轴上
+    // std::cout << "target: \n"<< t_target_point<<std::endl;
+
+    // 2.处理90度附近斜率无限大的情况
+    // 首先调整自身朝向到新坐标系
+    Eigen::Matrix2d t_base_rotation(t_rotation_matrix.transpose() * base.rotation());
+    Eigen::Matrix2d t_target_rotation(t_rotation_matrix.transpose() * target.rotation());
+    max_gradient_limit(t_base_rotation);
+    max_gradient_limit(t_target_rotation);
+    // std::cout << "br: "<< t_base_rotation<<std::endl;
+    // std::cout << "tr: "<< t_target_rotation<<std::endl;
+
+    // 3.三阶多项式拟合，生成参数矩阵并计算参数
+    Eigen::Matrix3d t_input_data;
+    Eigen::Vector3d t_output_data;
+    Eigen::Vector3d t_parameters;
+    // t_input_data.col(0) << pow(t_base_point.x(), 3.0), pow(t_base_point.x(), 2.0), t_base_point.x();
+    t_input_data.col(0) << pow(t_target_point.x(), 3.0), pow(t_target_point.x(), 2.0), t_target_point.x();
+    t_input_data.col(1) << 3 * pow(t_base_point.x(), 2.0), 2 * t_base_point.x(), 1;
+    t_input_data.col(2) << 3 * pow(t_target_point.x(), 2.0), 2 * t_target_point.x(), 1;
+    t_output_data << t_target_point.y(), -(t_base_rotation(0, 1) / t_base_rotation(0, 0)), -(t_target_rotation(0, 1) / t_target_rotation(0, 0));
+    t_parameters = (t_output_data.transpose() * t_input_data.inverse()).transpose();
+    // std::cout << "input: \n"<< t_input_data<<std::endl;
+    // std::cout << "output: \n"<< t_output_data<<std::endl;
+    // std::cout << "params: \n"<< t_parameters<<std::endl;
+
+    // 4.根据参数进行多项式插值(cos = dx/d, cos2 = 1/(1+tan2))，并转换回原坐标系
+    double t_delta = 0.05;
+    double t_current_x = t_base_point.x();
+    double t_extend_front = 0;
+    double t_extend_end = 0;
+    std::lock_guard<std::mutex> lck(m_mutex_trajectory);
+    m_trajectory.clear();
+    trajectory.clear();
+    while (t_current_x < t_target_point.x() || t_extend_end < extend_num)
+    {
+        double t_gradient = t_parameters.transpose() * Eigen::Vector3d(3 * pow(t_current_x, 2.0), 2 * t_current_x, 1.0);
+        double t_delta_x = sqrt(pow(t_delta, 2.0) / (1 + pow(t_gradient, 2.0)));
+        if (t_extend_front < extend_num)
+        {
+            t_current_x -= t_delta_x;
+            t_extend_front++;
+        }
+        else
+        {
+            t_current_x += t_delta_x;
+            if (t_current_x >= t_target_point.x())
+                t_extend_end++;
+            double t_current_y = t_parameters.transpose() * Eigen::Vector3d(pow(t_current_x, 3.0), pow(t_current_x, 2.0), t_current_x);
+            Eigen::Isometry2d t_current_pose;
+            Eigen::Matrix2d t_current_rotation;
+            t_current_pose.setIdentity();
+            t_current_rotation << t_delta_x / t_delta, -t_gradient * t_delta_x / t_delta, t_gradient * t_delta_x / t_delta, t_delta_x / t_delta;
+            // 将自身朝向调整回来
+            t_current_pose.rotate(t_origin_transformation.rotation() * t_current_rotation);
+            // 平移时将坐标系变换回去
+            t_current_pose.pretranslate(t_origin_transformation.rotation() * Eigen::Vector2d(t_current_x, t_current_y) + t_origin_transformation.translation());
+            m_trajectory.push_back(t_current_pose);
+            trajectory.push_back(t_current_pose);
+        }
+    }
+    if(trajectory.size() * t_delta <= t_euclidean_distance * 5)
+        return true;
+    else
+        return false;
+}
+
+// 获取已生成的路径
+bool Trajectory_tool::get_trajectory(trajectory_type &trajectory)
+{
+    std::lock_guard<std::mutex> lck(m_mutex_trajectory);
+    if (m_trajectory.size() <= 0)
+        return false;
+    trajectory.clear();
+    for (size_t i = 0; i < m_trajectory.size(); i++)
+    {
+        Eigen::Isometry2d t_pose;
+        t_pose.pretranslate(m_trajectory[i].translation());
+        t_pose.rotate(m_trajectory[i].rotation());
+        trajectory.push_back(t_pose);
+    }
+    return true;
+}
+
+// 获取下标对应的路径上的点
+Eigen::Isometry2d Trajectory_tool::operator[](int trajectory_index)
+{
+    std::lock_guard<std::mutex> lck(m_mutex_trajectory);
+    if (m_trajectory.size() <= trajectory_index)
+        return Eigen::Isometry2d::Identity();
+    else
+        return m_trajectory[trajectory_index];
+}

mpc_control/src/mpc_two_vehicle/trajectory_tool.h

@@ -0,0 +1,40 @@
+#ifndef TRAJECTORY_TOOL_HH
+#define TRAJECTORY_TOOL_HH
+
+#include <mutex>
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+// #include <opencv2/core.hpp>
+// 使用eigen中向量构成路径类型
+typedef std::vector<Eigen::Isometry2d, Eigen::aligned_allocator<Eigen::Isometry2d>> trajectory_type;
+/**
+ * 路径功能类
+ * 用于根据起始点与朝向、目标点与朝向生成路径
+ * */
+class Trajectory_tool
+{
+public:
+    // 构造函数
+    Trajectory_tool();
+    // 析构函数
+    ~Trajectory_tool();
+    // 生成路径
+    bool create_trajectory(Eigen::Isometry2d base, Eigen::Isometry2d target, trajectory_type &trajectory, int extend_num=0);
+    // 获取已生成的路径
+    bool get_trajectory(trajectory_type &trajectory);
+    // 获取下标对应的路径上的点
+    Eigen::Isometry2d operator[](int trajectory_index);
+
+private:
+    // 处理90度附近斜率无限大的情况,角度(sin,cos)近似为89.5度(0.99995, 0.01)或90.5度(0.99995,-0.01)
+    void max_gradient_limit(Eigen::Matrix2d &rotation);
+
+private:
+    // 路径访问互斥锁
+    std::mutex m_mutex_trajectory;
+    // 当前生成路径
+    trajectory_type m_trajectory;
+
+};
+
+#endif // !TRAJECTORY_TOOL_HH

mpc_control/src/test/traj.txt

@@ -0,0 +1,44 @@
+1.05 0.998817
+1.09995 0.995389
+1.14974 0.989913
+1.19933 0.98259
+1.24866 0.973625
+1.29771 0.963215
+1.34648 0.951555
+1.39497 0.938826
+1.44321 0.925201
+1.49121 0.910846
+1.53902 0.895916
+1.58667 0.880558
+1.6342 0.864917
+1.68167 0.849131
+1.7291 0.833337
+1.77655 0.817671
+1.82407 0.802268
+1.87169 0.787269
+1.91945 0.772818
+1.96741 0.759062
+2.01558 0.74616
+2.06401 0.734275
+2.1127 0.723582
+2.16168 0.714264
+2.21093 0.706514
+2.26045 0.700532
+2.3102 0.69652
+2.36011 0.694683
+2.4101 0.695215
+2.46007 0.698296
+2.50988 0.704078
+2.55937 0.712676
+2.60836 0.724158
+2.65668 0.73854
+2.70416 0.755783
+2.75064 0.775796
+2.79598 0.798442
+2.84007 0.823554
+2.88286 0.85094
+2.92428 0.880402
+2.96434 0.911741
+3.00304 0.944763
+3.04041 0.979291
+3.07649 1.01516

mpc_control/src/test/two_vehicle_test.cpp

@@ -0,0 +1,76 @@
+#include <ros/ros.h>
+#include "../mpc_two_vehicle/trajectory_tool.h"
+#include <tf/transform_listener.h>
+#include <tf/transform_broadcaster.h>
+#include <tf/transform_datatypes.h>
+#include <nav_msgs/Path.h>
+#include <turtlesim/Pose.h>
+
+void position_callback(const turtlesim::Pose::ConstPtr& msg)
+{
+    // 发布tf模板
+    tf::TransformBroadcaster tf_broadcaster_;
+    tf::Transform transform;
+    transform.setOrigin(tf::Vector3(msg->x, msg->y, 0.0));
+    transform.setRotation(tf::Quaternion(0, 0, cos(msg->theta/2.0), sin(msg->theta/2.0)));
+    tf::StampedTransform transform_msg(transform, ros::Time::now(), "map", "base_link");
+    tf_broadcaster_.sendTransform(transform_msg);
+}
+
+
+int main(int argc, char **argv)
+{
+    ros::init(argc, argv, "mpc_two_vehicle_test_node");
+    ros::NodeHandle nh;
+    ros::Subscriber position_subscriber = nh.subscribe("/turtule1/pose", 10, position_callback);
+    // ros::Subscriber target_sub_ = nh.subscribe<geometry_msgs::PoseStamped>("/move_base_simple/goal", 1, targetHandler);
+    ros::Publisher global_trajectory_publisher = nh.advertise<nav_msgs::Path>("global_path", 2);
+    ros::Publisher local_trajectory_publisher = nh.advertise<nav_msgs::Path>("local_path", 2);
+
+    nav_msgs::Path path;
+    int path_count = 0;
+
+    // 路径测试，从(1,1)到(3,1.5),theta=pi/4.0
+    Trajectory_tool traj_tool;
+    Eigen::Isometry2d base;
+    Eigen::Isometry2d target;
+    Eigen::Matrix2d rotation;
+    trajectory_type trajectory;
+    rotation << cos(M_PI_4), -sin(M_PI_4), sin(M_PI_4), cos(M_PI_4);
+    base.setIdentity();
+    base.pretranslate(Eigen::Vector2d(1, 1));
+    target.setIdentity();
+    target.rotate(rotation);
+    target.pretranslate(Eigen::Vector2d(9, -1));
+    bool create_trajectory_result = traj_tool.create_trajectory(base, target, trajectory);
+
+    ros::Rate loop_rate(20);
+    while (ros::ok())
+    {
+        // 发布全局路径
+        if (create_trajectory_result)
+        {
+            path.header.frame_id = "map";
+            path.header.seq = ++path_count;
+            path.header.stamp = ros::Time::now();
+            path.poses.clear();
+            for (size_t i = 0; i < trajectory.size(); i++)
+            {
+                geometry_msgs::PoseStamped t_pose;
+                t_pose.header = path.header;
+                t_pose.pose.position.x = trajectory[i].translation().x();
+                t_pose.pose.position.y = trajectory[i].translation().y();
+                double theta = atan2(trajectory[i].rotation()(1, 0), trajectory[i].rotation()(0, 0));
+                t_pose.pose.orientation.w = cos(theta / 2.0);
+                t_pose.pose.orientation.z = sin(theta / 2.0);
+                path.poses.push_back(t_pose);
+            }
+            global_trajectory_publisher.publish(path);
+        }
+
+        ros::spinOnce();
+        loop_rate.sleep();
+    }
+
+    return 0;
+}