mpc_node with limited control ability

cartographer_ros/cartographer_ros/configuration_files/demo_sick.lua

@@ -43,7 +43,7 @@ options = {
   landmarks_sampling_ratio = 1.,
 }
 
-MAP_BUILDER.num_background_threads=6
+MAP_BUILDER.num_background_threads=4
 MAP_BUILDER.use_trajectory_builder_2d = true
 
 TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true

cartographer_ros/cartographer_ros/launch/demo_sick.launch

@@ -4,8 +4,8 @@
   <node name="cartographer_node" pkg="cartographer_ros"
       type="cartographer_node" args="
           -configuration_directory $(find cartographer_ros)/configuration_files
-          -configuration_basename demo_sick_location.lua
-	-load_state_filename /home/zx/Downloads/map_part3.pbstream"
+          -configuration_basename demo_sick.lua
+          -save_state_filename /home/youchen/Documents/map1.pbstream"
       output="screen">
 	  
   </node>

cartographer_ros/cartographer_ros/launch/demo_sick_location.launch

@@ -0,0 +1,18 @@
+<launch>
+  <param name="/use_sim_time" value="false" />
+
+  <node name="cartographer_node" pkg="cartographer_ros"
+      type="cartographer_node" args="
+          -configuration_directory $(find cartographer_ros)/configuration_files
+          -configuration_basename demo_sick_location.lua
+	-load_state_filename /home/zx/Downloads/map_part3.pbstream"
+      output="screen">
+	  
+  </node>
+
+  <!--<node name="cartographer_occupancy_grid_node" pkg="cartographer_ros"
+      type="cartographer_occupancy_grid_node" args="-resolution 0.05" />-->
+
+  <!--><--><node name="rviz" pkg="rviz" type="rviz" required="true"
+      args="-d $(find cartographer_ros)/configuration_files/demo_sick.rviz" />
+</launch>

grapher/scripts/demo_sick.launch

@@ -0,0 +1,31 @@
+<!--
+Example launch file: launches the scan matcher with pre-recorded data
+-->
+
+<launch>
+  <!-- <arg name="IS_TWISTSTAMPED" default="true" /> -->
+  <arg name="use_rviz" default="true" />
+  #### set up data playback from bag #############################
+
+  <param name="/use_sim_time" value="true"/>
+  <!-- <param name="/stamped_vel" value="$(arg IS_TWISTSTAMPED)"/> -->
+
+  <group if="$(arg use_rviz)">
+    <node pkg="rviz" type="rviz" name="rviz"
+          args="-d $(find grapher)/scripts/demo.rviz"/>
+  </group>
+
+  <!-- <node pkg="rosbag" type="play" name="play"
+  args="$(find laser_scan_matcher)/demo/scan1.bag"/> -->
+
+  #### publish an example base_link -> laser transform ###########
+
+  <!-- <node pkg="tf" type="static_transform_publisher" name="base_link_to_laser"
+    args="0.0 0.0 0.0 0.0 0.0 0.0 1.0 /base_link /laser 40" />-->
+
+  #### start the laser scan_matcher ##############################
+  <node pkg="grapher" type="location_node"
+    name="location_node" output="screen">
+  </node>
+
+</launch>

grapher/src/grapher_node.cpp

@@ -6,7 +6,7 @@
 #include <nav_msgs/OccupancyGrid.h>
 #include <tf/transform_datatypes.h>
 #include <tf/transform_broadcaster.h>
-#include <pcl/common/transforms.h>	
+#include <pcl/common/transforms.h>
 #include <pcl_conversions/pcl_conversions.h>
 #include <pcl/io/pcd_io.h>
 #include <boost/thread.hpp>
@@ -22,16 +22,15 @@ ros::Publisher pubCloudL;
 ros::Publisher pubCloudG;
 ros::Publisher grid_pub;
 
-
-pcl::PointXYZ minp=pcl::PointXYZ(-10.0,-10.0,0);
-pcl::PointXYZ maxp=pcl::PointXYZ(10.0,10.0,0);
-double resolution=0.02;
-ScanGrapher* pGrapher=new ScanGrapher(minp,maxp,resolution);
+pcl::PointXYZ minp = pcl::PointXYZ(-10.0, -10.0, 0);
+pcl::PointXYZ maxp = pcl::PointXYZ(10.0, 10.0, 0);
+double resolution = 0.02;
+ScanGrapher *pGrapher = new ScanGrapher(minp, maxp, resolution);
 KalmanFilter klmFilter;
 
-pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud (new pcl::PointCloud<pcl::PointXYZ>);
+pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud(new pcl::PointCloud<pcl::PointXYZ>);
 
-void laserScanToLDP(const sensor_msgs::LaserScan::ConstPtr& scan_msg,LDP& ldp)
+void laserScanToLDP(const sensor_msgs::LaserScan::ConstPtr &scan_msg, LDP &ldp)
 {
   unsigned int n = scan_msg->ranges.size();
   ldp = ld_alloc_new(n);
@@ -52,16 +51,16 @@ void laserScanToLDP(const sensor_msgs::LaserScan::ConstPtr& scan_msg,LDP& ldp)
     else
     {
       ldp->valid[i] = 0;
-      ldp->readings[i] = -1;  // for invalid range
+      ldp->readings[i] = -1; // for invalid range
     }
 
-    ldp->theta[i]    = scan_msg->angle_min + i * scan_msg->angle_increment;
+    ldp->theta[i] = scan_msg->angle_min + i * scan_msg->angle_increment;
 
-    ldp->cluster[i]  = -1;
+    ldp->cluster[i] = -1;
   }
 
   ldp->min_theta = ldp->theta[0];
-  ldp->max_theta = ldp->theta[n-1];
+  ldp->max_theta = ldp->theta[n - 1];
 
   ldp->odometry[0] = 0.0;
   ldp->odometry[1] = 0.0;
@@ -72,9 +71,10 @@ void laserScanToLDP(const sensor_msgs::LaserScan::ConstPtr& scan_msg,LDP& ldp)
   ldp->true_pose[2] = 0.0;
 }
 
-void PointCloudToLDP(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,LDP& ldp)
+//点云转ldp
+void PointCloudToLDP(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, LDP &ldp)
 {
-  double max_d2 = 0.01*0.01;
+  double max_d2 = 0.01 * 0.01;
 
   pcl::PointCloud<pcl::PointXYZ> cloud_f;
 
@@ -82,16 +82,15 @@ void PointCloudToLDP(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,LDP& ldp)
 
   for (unsigned int i = 1; i < cloud->points.size(); ++i)
   {
-    const pcl::PointXYZ& pa = cloud_f.points[cloud_f.points.size() - 1];
-    const pcl::PointXYZ& pb = cloud->points[i];
-    double theta_pa=atan2(pa.y,pa.x);
-    double theta_pb=atan2(pb.y,pb.x);
+    const pcl::PointXYZ &pa = cloud_f.points[cloud_f.points.size() - 1];
+    const pcl::PointXYZ &pb = cloud->points[i];
+    double theta_pa = atan2(pa.y, pa.x);
+    double theta_pb = atan2(pb.y, pb.x);
     double dx = pa.x - pb.x;
     double dy = pa.y - pb.y;
-    double d2 = dx*dx + dy*dy;
+    double d2 = dx * dx + dy * dy;
 
     cloud_f.points.push_back(pb);
-    
   }
 
   unsigned int n = cloud_f.points.size();
@@ -111,7 +110,7 @@ void PointCloudToLDP(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,LDP& ldp)
       double r = sqrt(cloud_f.points[i].x * cloud_f.points[i].x +
                       cloud_f.points[i].y * cloud_f.points[i].y);
 
-      if (1/* r > -135.0/180.0*M_PI && r < 35.0/180.0*M_PI*/)
+      if (1 /* r > -135.0/180.0*M_PI && r < 35.0/180.0*M_PI*/)
       {
         ldp->valid[i] = 1;
         ldp->readings[i] = r;
@@ -119,16 +118,16 @@ void PointCloudToLDP(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,LDP& ldp)
       else
       {
         ldp->valid[i] = 0;
-        ldp->readings[i] = -1;  // for invalid range
+        ldp->readings[i] = -1; // for invalid range
       }
     }
 
     ldp->theta[i] = atan2(cloud_f.points[i].y, cloud_f.points[i].x);
-    ldp->cluster[i]  = -1;
+    ldp->cluster[i] = -1;
   }
 
   ldp->min_theta = ldp->theta[0];
-  ldp->max_theta = ldp->theta[n-1];
+  ldp->max_theta = ldp->theta[n - 1];
 
   //std::cout<<" scan theta :"<<ldp->min_theta<<","<<ldp->max_theta<<"."<<n<<std::endl;
 
@@ -142,12 +141,12 @@ void PointCloudToLDP(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,LDP& ldp)
 }
 
 pcl::PointCloud<pcl::PointXYZ>::Ptr NDT(pcl::PointCloud<pcl::PointXYZ>::Ptr ori_ptr,
-      pcl::PointCloud<pcl::PointXYZ>::Ptr cur_ptr,Eigen::Matrix4f& init_param)
+                                        pcl::PointCloud<pcl::PointXYZ>::Ptr cur_ptr, Eigen::Matrix4f &init_param)
 {
   // Initializing Normal Distributions Transform (NDT).
 
   pclomp::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>::Ptr ndt_omp(new pclomp::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>());
-  
+
   int n = 2;
   ndt_omp->setInputTarget(ori_ptr);
   ndt_omp->setInputSource(cur_ptr);
@@ -159,328 +158,311 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr NDT(pcl::PointCloud<pcl::PointXYZ>::Ptr ori_
   ndt_omp->setNeighborhoodSearchMethod(pclomp::DIRECT7);
 
   // Calculating required rigid transform to align the input cloud to the target cloud.
-  pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud (new pcl::PointCloud<pcl::PointXYZ>);
-  ndt_omp->align (*output_cloud, init_param);
+  pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud(new pcl::PointCloud<pcl::PointXYZ>);
+  ndt_omp->align(*output_cloud, init_param);
   /* std::cout << "target size"<<ori_ptr->size()<<", input size:"<<cur_ptr->size()<<"," << ndt_omp->hasConverged ()
             << " score: " << ndt_omp->getFitnessScore () << std::endl;*/
 
-  init_param= ndt_omp->getFinalTransformation();
-  
-  Eigen::Matrix3f rotation=init_param.block<3,3>(0,0);
-  Eigen::Quaterniond eigen_quat(rotation.cast<double>());
-  Eigen::Vector3d eigen_trans(init_param.block<3,1>(0,3).cast<double>());
+  init_param = ndt_omp->getFinalTransformation();
 
+  Eigen::Matrix3f rotation = init_param.block<3, 3>(0, 0);
+  Eigen::Quaterniond eigen_quat(rotation.cast<double>());
+  Eigen::Vector3d eigen_trans(init_param.block<3, 1>(0, 3).cast<double>());
 
   tf::Quaternion tf_quat(eigen_quat.x(), eigen_quat.y(), eigen_quat.z(), eigen_quat.w());
-  
-    static  tf::TransformBroadcaster tfBroadcaster;
-    tf::StampedTransform laserOdometryTrans;
-    laserOdometryTrans.frame_id_ = "/map";
-    laserOdometryTrans.child_frame_id_ = "/NDT";
 
-    laserOdometryTrans.stamp_ = ros::Time::now();
-    laserOdometryTrans.setRotation(tf_quat);
-    laserOdometryTrans.setOrigin(tf::Vector3(eigen_trans(0), eigen_trans(1), 0));
-    tfBroadcaster.sendTransform(laserOdometryTrans);
+  static tf::TransformBroadcaster tfBroadcaster;
+  tf::StampedTransform laserOdometryTrans;
+  laserOdometryTrans.frame_id_ = "/map";
+  laserOdometryTrans.child_frame_id_ = "/NDT";
 
-  return output_cloud;
+  laserOdometryTrans.stamp_ = ros::Time::now();
+  laserOdometryTrans.setRotation(tf_quat);
+  laserOdometryTrans.setOrigin(tf::Vector3(eigen_trans(0), eigen_trans(1), 0));
+  tfBroadcaster.sendTransform(laserOdometryTrans);
 
+  return output_cloud;
 }
 
-bool ICP(LDP& ldp,LDP& ldp_target,double* T)
+//csm icp配准
+bool ICP(LDP &ldp, LDP &ldp_target, double *T)
 {
 
-  
   struct sm_params param;
-  param.laser_ref=ldp_target;
-  param.laser_sens=ldp;
-  param.first_guess[0]=T[0];
-  param.first_guess[1]=T[1];
-  param.first_guess[2]=T[2];
-  param.max_angular_correction_deg=45.0;
-  param.max_linear_correction=0.5;
-  param.max_iterations=50;
-
-  param.epsilon_xy=0.000001;
-  param.epsilon_theta=0.000001;
-
-  param.max_correspondence_dist=0.3;
-  param.use_corr_tricks=1;
-  param.restart=0;
-  param.restart_threshold_mean_error=0.01;
-  param.restart_dt=1.0;
-  param.restart_dtheta=0.1;
-  param.outliers_maxPerc=0.9;
-  param.outliers_adaptive_order=0.7;
-  param.outliers_adaptive_mult=2.0;
-  param.outliers_remove_doubles=1;
-  param.clustering_threshold=0.25;
-  param.orientation_neighbourhood=20;
-  param.do_alpha_test=0;
-  param.do_alpha_test_thresholdDeg=20.0;
-  param.do_visibility_test=0;
-  param.use_point_to_line_distance=1;
-  param.use_ml_weights=0;
-  param.use_sigma_weights=0;
-  param.do_compute_covariance=0;
-  param.debug_verify_tricks=0;
-
-  param.laser[0]=0;
-  param.laser[1]=0;
-  param.laser[2]=0;
-
-  param.sigma=0.01;
-  param.min_reading=0.02;
-  param.max_reading=30.0;
-
-  param.gpm_theta_bin_size_deg=0.0;
-  param.gpm_extend_range_deg=0.0;
-  param.gpm_interval=280;
-
-  param.hsm.max_norm=0.0;
-  param.hsm.linear_cell_size=0.0;
-  param.hsm.angular_cell_size_deg=0.0;
-  param.hsm.num_angular_hypotheses=4502664;
-  param.hsm.angular_hyp_min_distance_deg=0.0;
-  param.hsm.xc_ndirections=6668808;
-  param.hsm.xc_directions_min_distance_deg=0.0;
-  param.hsm.linear_xc_max_npeaks=6673640;
-
-  param.hsm.linear_xc_peaks_min_distance=0.0;
-  param.hsm.max_translation=0.0;
-  param.hsm.debug_true_x_valid=-531946640;
-
-  param.hsm.debug_true_x[0]=0.0;
-  param.hsm.debug_true_x[1]=0.0;
-  param.hsm.debug_true_x[2]=0.0;
-  
-  
-  std::chrono::time_point<std::chrono::system_clock> start=std::chrono::system_clock::now();
+  param.laser_ref = ldp_target;
+  param.laser_sens = ldp;
+  param.first_guess[0] = T[0];
+  param.first_guess[1] = T[1];
+  param.first_guess[2] = T[2];
+  param.max_angular_correction_deg = 45.0;
+  param.max_linear_correction = 0.5;
+  param.max_iterations = 50;
+
+  param.epsilon_xy = 0.000001;
+  param.epsilon_theta = 0.000001;
+
+  param.max_correspondence_dist = 0.3;
+  param.use_corr_tricks = 1;
+  param.restart = 0;
+  param.restart_threshold_mean_error = 0.01;
+  param.restart_dt = 1.0;
+  param.restart_dtheta = 0.1;
+  param.outliers_maxPerc = 0.9;
+  param.outliers_adaptive_order = 0.7;
+  param.outliers_adaptive_mult = 2.0;
+  param.outliers_remove_doubles = 1;
+  param.clustering_threshold = 0.25;
+  param.orientation_neighbourhood = 20;
+  param.do_alpha_test = 0;
+  param.do_alpha_test_thresholdDeg = 20.0;
+  param.do_visibility_test = 0;
+  param.use_point_to_line_distance = 1;
+  param.use_ml_weights = 0;
+  param.use_sigma_weights = 0;
+  param.do_compute_covariance = 0;
+  param.debug_verify_tricks = 0;
+
+  param.laser[0] = 0;
+  param.laser[1] = 0;
+  param.laser[2] = 0;
+
+  param.sigma = 0.01;
+  param.min_reading = 0.02;
+  param.max_reading = 30.0;
+
+  param.gpm_theta_bin_size_deg = 0.0;
+  param.gpm_extend_range_deg = 0.0;
+  param.gpm_interval = 280;
+
+  param.hsm.max_norm = 0.0;
+  param.hsm.linear_cell_size = 0.0;
+  param.hsm.angular_cell_size_deg = 0.0;
+  param.hsm.num_angular_hypotheses = 4502664;
+  param.hsm.angular_hyp_min_distance_deg = 0.0;
+  param.hsm.xc_ndirections = 6668808;
+  param.hsm.xc_directions_min_distance_deg = 0.0;
+  param.hsm.linear_xc_max_npeaks = 6673640;
+
+  param.hsm.linear_xc_peaks_min_distance = 0.0;
+  param.hsm.max_translation = 0.0;
+  param.hsm.debug_true_x_valid = -531946640;
+
+  param.hsm.debug_true_x[0] = 0.0;
+  param.hsm.debug_true_x[1] = 0.0;
+  param.hsm.debug_true_x[2] = 0.0;
+
+  std::chrono::time_point<std::chrono::system_clock> start = std::chrono::system_clock::now();
   struct sm_result res;
-  sm_icp(&param,&res);
-  std::chrono::time_point<std::chrono::system_clock> end=std::chrono::system_clock::now();
+  sm_icp(&param, &res);
+  std::chrono::time_point<std::chrono::system_clock> end = std::chrono::system_clock::now();
 
   //std::cout<<std::chrono::duration_cast<std::chrono::milliseconds>(end-start).count()<<std::endl;
-  if(res.valid!=1)
+  if (res.valid != 1)
     return false;
-  T[0]=res.x[0];
-  T[1]=res.x[1];
-  T[2]=res.x[2];
+  T[0] = res.x[0];
+  T[1] = res.x[1];
+  T[2] = res.x[2];
 
+  tf::Quaternion tf_quat(tf::Vector3(0, 0, 1), T[2]);
 
-     tf::Quaternion tf_quat(tf::Vector3(0,0,1),T[2]);
-  
-    static  tf::TransformBroadcaster tfBroadcaster;
-    tf::StampedTransform laserOdometryTrans;
-    laserOdometryTrans.frame_id_ = "/map";
-    laserOdometryTrans.child_frame_id_ = "/ICP";
+  static tf::TransformBroadcaster tfBroadcaster;
+  tf::StampedTransform laserOdometryTrans;
+  laserOdometryTrans.frame_id_ = "/map";
+  laserOdometryTrans.child_frame_id_ = "/ICP";
 
-    laserOdometryTrans.stamp_ = ros::Time::now();
-    laserOdometryTrans.setRotation(tf_quat);
-    laserOdometryTrans.setOrigin(tf::Vector3(T[0], T[1], 0));
-    tfBroadcaster.sendTransform(laserOdometryTrans);
-  
+  laserOdometryTrans.stamp_ = ros::Time::now();
+  laserOdometryTrans.setRotation(tf_quat);
+  laserOdometryTrans.setOrigin(tf::Vector3(T[0], T[1], 0));
+  tfBroadcaster.sendTransform(laserOdometryTrans);
 
   return true;
-
 }
 
-pcl::PointCloud<pcl::PointXYZ>::Ptr transformCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr input,double* T)
+//点云tf变换
+pcl::PointCloud<pcl::PointXYZ>::Ptr transformCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr input, double *T)
 {
-    double tx=T[0];
-    double ty=T[1];
-    double theta=T[2];
-    Eigen::Affine3f transform_2 = Eigen::Affine3f::Identity();
-    transform_2.rotate (Eigen::AngleAxisf (theta, Eigen::Vector3f::UnitZ()));
-    transform_2.translation() << tx, ty, 0.0;
-    pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud (new pcl::PointCloud<pcl::PointXYZ> ());
-    pcl::transformPointCloud (*input, *transformed_cloud, transform_2);
-    return transformed_cloud;
-
+  double tx = T[0];
+  double ty = T[1];
+  double theta = T[2];
+  Eigen::Affine3f transform_2 = Eigen::Affine3f::Identity();
+  transform_2.rotate(Eigen::AngleAxisf(theta, Eigen::Vector3f::UnitZ()));
+  transform_2.translation() << tx, ty, 0.0;
+  pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud(new pcl::PointCloud<pcl::PointXYZ>());
+  pcl::transformPointCloud(*input, *transformed_cloud, transform_2);
+  return transformed_cloud;
 }
 
-void scanHandler(const sensor_msgs::LaserScan::ConstPtr& msg)
+void scanHandler(const sensor_msgs::LaserScan::ConstPtr &msg)
 {
 
-  
-    static int systemInitCount=0;
-    static bool systemInited=false;
-    int systemDelay=100;
-    if (!systemInited) 
-    {//丢弃前20个点云数据
-        systemInitCount++;
-        if (systemInitCount >= systemDelay) {
-        systemInited = true;
-        }
-        return;
+  static int systemInitCount = 0;
+  static bool systemInited = false;
+  int systemDelay = 100;
+  if (!systemInited)
+  { //丢弃前20个点云数据
+    systemInitCount++;
+    if (systemInitCount >= systemDelay)
+    {
+      systemInited = true;
     }
+    return;
+  }
 
   //当前点云时间
-    double timeScanCur = msg->header.stamp.toSec();
-    static bool first_frame=true;
-    int N = int( (msg->angle_max-msg->angle_min)/msg->angle_increment )+1;
-    
-    //ICP 算法 参数 
-    pcl::PointCloud<pcl::PointXYZ>  laserCloud1;
-    static double param_sum[3]={0};
-
-    // ndt 算法参数
-    Eigen::AngleAxisf init_rotation (0, Eigen::Vector3f::UnitZ ());
-    Eigen::Translation3f init_translation (0, 0, 0);
-    static Eigen::Matrix4f init_tf = (init_translation * init_rotation).matrix ();
- 
-
-    for(int i=0;i<N;++i)
-    {
-        float distance = msg->ranges[i];
-        
-        float angle = msg->angle_min+ i*msg->angle_increment;
-        if(distance >20.0)
-          continue;
-        //第一帧 用作 target  使用所有点，非第一帧只保留前方一半点
-        if(first_frame==false)
-        {
-          if(angle<-M_PI_2 || angle> M_PI_2)
-            continue;
-        }
-        pcl::PointXYZ point ;
-        point.x= -sin(angle-M_PI_2)*distance;
-        point.y= cos(angle-M_PI_2)*distance;
-        point.z=0;
-        if(fabs(point.x)>1.0 || fabs(point.y)>1.0)
-        {   
-          laserCloud1.points.push_back(point);
-
-          if(first_frame)
-          {
-            target_cloud->push_back(point);
-          }
-        }
-
-        
-    }
-    if(laserCloud1.size()<10)
-        return ;
-    
-    std::vector<tf::Pose> poses;
-    tf::Pose lastPose;
-    // ndt 算法 
-    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_T=NDT(target_cloud,laserCloud1.makeShared(),init_tf);
-    Eigen::Matrix3f rotation=init_tf.block<3,3>(0,0);
-    Eigen::Quaterniond eigen_quat(rotation.cast<double>());
-    Eigen::Vector3d eigen_trans(init_tf.block<3,1>(0,3).cast<double>());
-    tf::Quaternion tf_quat_NDT(eigen_quat.x(), eigen_quat.y(), eigen_quat.z(), eigen_quat.w());
-    tf::Vector3 offset_NDT(eigen_trans(0),eigen_trans(1),0);
-    tf::Pose pose_NDT(tf_quat_NDT,offset_NDT);
-    lastPose=pose_NDT;
-    poses.push_back(pose_NDT);
-    
-    //// icp  算法 
-    
-    /*   LDP source_ldp;
-    //PointCloudToLDP(laserCloud1.makeShared(),source_ldp);
-    laserScanToLDP(msg,source_ldp);
-    static LDP target_ldp;
-    
-    
-    if(first_frame==true)
-    {
-      //PointCloudToLDP(target_cloud,target_ldp);
-      //PointCloudToLDP(laserCloud1.makeShared(),target_ldp);
-      laserScanToLDP(msg,target_ldp);
-      
-    }
-    
+  double timeScanCur = msg->header.stamp.toSec();
+  static bool first_frame = true;
+  int N = int((msg->angle_max - msg->angle_min) / msg->angle_increment) + 1;
 
-    
-    if(!ICP(source_ldp,target_ldp,param_sum))
-    {
-        std::cout<<" ICP failed...   "<<std::endl;
-        ld_free(source_ldp);
-        
-    }
-    else
-    {
-      ld_free(source_ldp);
-      tf::Quaternion tf_quat_ICP(tf::Vector3(0,0,1),param_sum[2]);
-      tf::Vector3 offset_ICP(param_sum[0],param_sum[1],0);
-      tf::Pose pose_ICP(tf_quat_ICP,offset_ICP);
-      poses.push_back(pose_ICP);
-      lastPose=pose_ICP;
-    }
+  //ICP 算法 参数
+  pcl::PointCloud<pcl::PointXYZ> laserCloud1;
+  static double param_sum[3] = {0};
 
-    */
-    first_frame=false;
-    if(poses.size()==2)
-      lastPose=klmFilter.filter(poses[0],poses[1]);
+  // ndt 算法参数
+  Eigen::AngleAxisf init_rotation(0, Eigen::Vector3f::UnitZ());
+  Eigen::Translation3f init_translation(0, 0, 0);
+  static Eigen::Matrix4f init_tf = (init_translation * init_rotation).matrix();
 
-    static  tf::TransformBroadcaster tfBroadcaster;
-    tf::StampedTransform laserOdometryTrans;
-    laserOdometryTrans.frame_id_ = "/map";
-    laserOdometryTrans.child_frame_id_ = "/laser";
+  for (int i = 0; i < N; ++i)
+  {
+    float distance = msg->ranges[i];
 
-    laserOdometryTrans.stamp_ = ros::Time::now();
-    laserOdometryTrans.setRotation(lastPose.getRotation());
-    laserOdometryTrans.setOrigin(lastPose.getOrigin());
-    tfBroadcaster.sendTransform(laserOdometryTrans);
+    float angle = msg->angle_min + i * msg->angle_increment;
+    if (distance > 20.0)
+      continue;
+    //第一帧 用作 target  使用所有点，非第一帧只保留前方一半点
+    if (first_frame == false)
+    {
+      if (angle < -M_PI_2 || angle > M_PI_2)
+        continue;
+    }
+    pcl::PointXYZ point;
+    point.x = -sin(angle - M_PI_2) * distance;
+    point.y = cos(angle - M_PI_2) * distance;
+    point.z = 0;
+    if (fabs(point.x) > 1.0 || fabs(point.y) > 1.0)
+    {
+      laserCloud1.points.push_back(point);
 
+      if (first_frame)
+      {
+        target_cloud->push_back(point);
+      }
+    }
+  }
+  if (laserCloud1.size() < 10)
+    return;
+
+  std::vector<tf::Pose> poses;
+  tf::Pose lastPose;
+  // ndt 算法
+  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_T = NDT(target_cloud, laserCloud1.makeShared(), init_tf);
+  Eigen::Matrix3f rotation = init_tf.block<3, 3>(0, 0);
+  Eigen::Quaterniond eigen_quat(rotation.cast<double>());
+  Eigen::Vector3d eigen_trans(init_tf.block<3, 1>(0, 3).cast<double>());
+  tf::Quaternion tf_quat_NDT(eigen_quat.x(), eigen_quat.y(), eigen_quat.z(), eigen_quat.w());
+  tf::Vector3 offset_NDT(eigen_trans(0), eigen_trans(1), 0);
+  tf::Pose pose_NDT(tf_quat_NDT, offset_NDT);
+  lastPose = pose_NDT;
+  poses.push_back(pose_NDT);
 
-    ///变换点云
-     pGrapher->pushScan(cloud_T,pcl::PointXYZ(lastPose.getOrigin().x(),lastPose.getOrigin().y(),0));
+  //// icp  算法
 
-    sensor_msgs::PointCloud2 cloudLocalMsg;
-    pcl::toROSMsg(*target_cloud, cloudLocalMsg);
-    cloudLocalMsg.header.stamp = msg->header.stamp;
-    cloudLocalMsg.header.frame_id = "/map";
-    pubCloudL.publish(cloudLocalMsg);
+  LDP source_ldp;
+  //PointCloudToLDP(laserCloud1.makeShared(),source_ldp);
+  laserScanToLDP(msg, source_ldp);
+  static LDP target_ldp;
 
+  if (first_frame == true)
+  {
+    //PointCloudToLDP(target_cloud,target_ldp);
+    //PointCloudToLDP(laserCloud1.makeShared(),target_ldp);
+    laserScanToLDP(msg, target_ldp);
+  }
 
+  if (!ICP(source_ldp, target_ldp, param_sum))
+  {
+    std::cout << " ICP failed...   " << std::endl;
+    ld_free(source_ldp);
+  }
+  else
+  {
+    ld_free(source_ldp);
+    tf::Quaternion tf_quat_ICP(tf::Vector3(0, 0, 1), param_sum[2]);
+    tf::Vector3 offset_ICP(param_sum[0], param_sum[1], 0);
+    tf::Pose pose_ICP(tf_quat_ICP, offset_ICP);
+    poses.push_back(pose_ICP);
+    lastPose = pose_ICP;
+  }
+  first_frame = false;
+  if (poses.size() == 2)
+    lastPose = klmFilter.filter(poses[0], poses[1]);
+
+  static tf::TransformBroadcaster tfBroadcaster;
+  tf::StampedTransform laserOdometryTrans;
+  laserOdometryTrans.frame_id_ = "/map";
+  laserOdometryTrans.child_frame_id_ = "/laser";
+
+  laserOdometryTrans.stamp_ = ros::Time::now();
+  laserOdometryTrans.setRotation(lastPose.getRotation());
+  laserOdometryTrans.setOrigin(lastPose.getOrigin());
+  tfBroadcaster.sendTransform(laserOdometryTrans);
+
+  ///变换点云
+  pGrapher->pushScan(cloud_T, pcl::PointXYZ(lastPose.getOrigin().x(), lastPose.getOrigin().y(), 0));
+
+  sensor_msgs::PointCloud2 cloudLocalMsg;
+  pcl::toROSMsg(*target_cloud, cloudLocalMsg);
+  cloudLocalMsg.header.stamp = msg->header.stamp;
+  cloudLocalMsg.header.frame_id = "/map";
+  pubCloudL.publish(cloudLocalMsg);
 }
 
-void transforMatToGrid(cv::Mat& image,nav_msgs::OccupancyGrid& map)
+//图转换成地图
+void transforMatToGrid(cv::Mat &image, nav_msgs::OccupancyGrid &map)
 {
 
-  map.header.frame_id="map";
-  map.header.stamp = ros::Time::now(); 
-  map.info.resolution = pGrapher->resolution_;         // float32
-  map.info.width      = image.cols;           // uint32
-  map.info.height     = image.rows;           // uint32
-  map.info.origin.position.x=-image.cols*pGrapher->resolution_/2.0;
-  map.info.origin.position.y=-image.rows*pGrapher->resolution_/2.0;
-  static signed char* pdata=0; 
-  if(pdata!=0) free(pdata);
-  pdata=(signed char*)malloc(image.cols*image.rows*sizeof(signed char));
-  for(int i=0;i<image.rows;++i)
+  map.header.frame_id = "map";
+  map.header.stamp = ros::Time::now();
+  map.info.resolution = pGrapher->resolution_; // float32
+  map.info.width = image.cols;                 // uint32
+  map.info.height = image.rows;                // uint32
+  map.info.origin.position.x = -image.cols * pGrapher->resolution_ / 2.0;
+  map.info.origin.position.y = -image.rows * pGrapher->resolution_ / 2.0;
+  static signed char *pdata = 0;
+  if (pdata != 0)
+    free(pdata);
+  pdata = (signed char *)malloc(image.cols * image.rows * sizeof(signed char));
+  for (int i = 0; i < image.rows; ++i)
   {
-    for(int j=0;j<image.cols;++j)
+    for (int j = 0; j < image.cols; ++j)
     {
-      if(image.at<uchar>(i,j)==0)
-          pdata[i*image.cols+j]=-1;
-      else if(image.at<uchar>(i,j)<10)
-          pdata[i*image.cols+j]=0;
+      if (image.at<uchar>(i, j) == 0)
+        pdata[i * image.cols + j] = -1;
+      else if (image.at<uchar>(i, j) < 10)
+        pdata[i * image.cols + j] = 0;
       else
-        pdata[i*image.cols+j]=int(double(image.at<uchar>(i,j))*(100./255.0));
+        pdata[i * image.cols + j] = int(double(image.at<uchar>(i, j)) * (100. / 255.0));
     }
   }
-  map.data = std::vector<signed char>(pdata,pdata+image.cols*image.rows);
-
+  map.data = std::vector<signed char>(pdata, pdata + image.cols * image.rows);
 }
 
+//发布地图
 void publish()
 {
-    ros::Rate loop_rate(10);
-    
-    int last_cloud_size=-1;
-    while(ros::ok())
-    {
-      
-        cv::Mat grid=pGrapher->getGrid();
-        nav_msgs::OccupancyGrid map;
-        transforMatToGrid(grid,map);
-        grid_pub.publish(map);
-
-        ///发布地图点云
-        /*pcl::PointCloud<pcl::PointXYZ>::Ptr cloud=pGrapher->getCloud();
+  ros::Rate loop_rate(10);
+
+  int last_cloud_size = -1;
+  while (ros::ok())
+  {
+
+    cv::Mat grid = pGrapher->getGrid();
+    nav_msgs::OccupancyGrid map;
+    transforMatToGrid(grid, map);
+    grid_pub.publish(map);
+
+    ///发布地图点云
+    /*pcl::PointCloud<pcl::PointXYZ>::Ptr cloud=pGrapher->getCloud();
         if(cloud->size()>0)
         {
           sensor_msgs::PointCloud2 cloudglobalMsg;
@@ -490,36 +472,31 @@ void publish()
           pubCloudG.publish(cloudglobalMsg);
         }*/
 
-        ros::spinOnce();
-        loop_rate.sleep();
-    }
+    ros::spinOnce();
+    loop_rate.sleep();
+  }
 }
 
-
-int main(int argc, char** argv)
+int main(int argc, char **argv)
 {
-    ros::init(argc, argv, "grapher_node");
-    ros::NodeHandle nh;
+  ros::init(argc, argv, "grapher_node");
+  ros::NodeHandle nh;
 
-    
-    pubCloudL = nh.advertise<sensor_msgs::PointCloud2>
-                                 ("/cloud_local", 2);
-    pubCloudG = nh.advertise<sensor_msgs::PointCloud2>
-                                 ("/cloud_global", 2);
-    grid_pub=nh.advertise<nav_msgs::OccupancyGrid>("/grid",1);
-    //pcl::io::loadPCDFile("/home/zx/zzw/ws/src/grapher/map.pcd", *target_cloud);
-    //std::cout<<" target cloud size :"<<target_cloud->size()<<std::endl;
+  pubCloudL = nh.advertise<sensor_msgs::PointCloud2>("/cloud_local", 2);
+  pubCloudG = nh.advertise<sensor_msgs::PointCloud2>("/cloud_global", 2);
+  grid_pub = nh.advertise<nav_msgs::OccupancyGrid>("/grid", 1);
+  //pcl::io::loadPCDFile("/home/zx/zzw/ws/src/grapher/map.pcd", *target_cloud);
+  //std::cout<<" target cloud size :"<<target_cloud->size()<<std::endl;
 
-    boost::thread* thread_=new boost::thread(publish);
+  boost::thread *thread_ = new boost::thread(publish);
 
-    ros::Subscriber subLaserCloud = nh.subscribe<sensor_msgs::LaserScan>("/scan", 2,scanHandler);
+  ros::Subscriber subLaserCloud = nh.subscribe<sensor_msgs::LaserScan>("/scan", 2, scanHandler);
 
-    ros::spin();
+  ros::spin();
 
-    pGrapher->saveImage("/home/zx/zzw/ws/src/grapher/map.bmp");
-    pGrapher->saveCloud("/home/zx/zzw/ws/src/grapher/map.pcd");
-    delete pGrapher;
-    
+  pGrapher->saveImage("/home/youchen/Documents/git_ws/src/grapher/map1.bmp");
+  pGrapher->saveCloud("/home/youchen/Documents/git_ws/src/grapher/map.pcd");
+  delete pGrapher;
 
-    return 0;
+  return 0;
 }

grapher/src/location_node.cpp

@@ -1,3 +1,4 @@
+#include "scanGrapher.h"
 
 #include <ros/ros.h>
 #include <sensor_msgs/PointCloud2.h>
@@ -19,6 +20,7 @@
 
 ros::Publisher pubCloudL;
 ros::Publisher pubCloudG;
+// ros::NodeHandle nh;
 
 double param[3]={0};
 
@@ -361,7 +363,7 @@ void scanHandler(const sensor_msgs::LaserScan::ConstPtr& msg)
     
     //// icp  算法 
     
-    /*   LDP source_ldp;
+    LDP source_ldp;
     //PointCloudToLDP(laserCloud1.makeShared(),source_ldp);
     laserScanToLDP(msg,source_ldp);
     static LDP target_ldp;
@@ -393,7 +395,7 @@ void scanHandler(const sensor_msgs::LaserScan::ConstPtr& msg)
       lastPose=pose_ICP;
     }
 
-    */
+
     first_frame=false;
     if(poses.size()==2)
       lastPose=klmFilter.filter(poses[0],poses[1]);
@@ -412,10 +414,42 @@ void scanHandler(const sensor_msgs::LaserScan::ConstPtr& msg)
 
 }
 
-void publish()
+//图转换成地图
+void transforMatToGrid(cv::Mat &image, nav_msgs::OccupancyGrid &map)
+{
+
+  map.header.frame_id = "map";
+  map.header.stamp = ros::Time::now();
+  map.info.resolution = 0.02; // float32
+  map.info.width = image.cols;                 // uint32
+  map.info.height = image.rows;                // uint32
+  map.info.origin.position.x = -image.cols * 0.02 / 2.0;
+  map.info.origin.position.y = -image.rows * 0.02 / 2.0;
+  static signed char *pdata = 0;
+  if (pdata != 0)
+    free(pdata);
+  pdata = (signed char *)malloc(image.cols * image.rows * sizeof(signed char));
+  for (int i = 0; i < image.rows; ++i)
+  {
+    for (int j = 0; j < image.cols; ++j)
+    {
+      if (image.at<uchar>(i, j) == 0)
+        pdata[i * image.cols + j] = -1;
+      else if (image.at<uchar>(i, j) < 10)
+        pdata[i * image.cols + j] = 0;
+      else
+        pdata[i * image.cols + j] = int(double(image.at<uchar>(i, j)) * (100. / 255.0));
+    }
+  }
+  map.data = std::vector<signed char>(pdata, pdata + image.cols * image.rows);
+  // std::cout<<image.cols * 0.02<<", "<<image.rows * 0.02<<std::endl;
+}
+
+void publish(ros::NodeHandle nh)
 {
     ros::Rate loop_rate(10);
-    
+    ros::Publisher grid_pub;
+    grid_pub = nh.advertise<nav_msgs::OccupancyGrid>("/grid", 1);
     while(ros::ok())
     {
         if(target_cloud->size()>0)
@@ -426,6 +460,12 @@ void publish()
             cloudLocalMsg.header.frame_id = "/map";
             pubCloudG.publish(cloudLocalMsg);
         }
+
+        cv::Mat grid = cv::imread("/home/youchen/Documents/git_ws/src/grapher/map.bmp", 1);
+        nav_msgs::OccupancyGrid map;
+        transforMatToGrid(grid, map);
+        grid_pub.publish(map);
+
         ros::spinOnce();
         loop_rate.sleep();
     }
@@ -444,8 +484,8 @@ int main(int argc, char** argv)
     pubCloudG = nh.advertise<sensor_msgs::PointCloud2>
                                  ("/cloud_global", 2);
 
-    pcl::io::loadPCDFile("/home/zx/zzw/ws/src/grapher/map.pcd", *target_cloud);
-    boost::thread* thread_=new boost::thread(publish);
+    pcl::io::loadPCDFile("/home/youchen/Documents/git_ws/src/grapher/map.pcd", *target_cloud);
+    boost::thread* thread_=new boost::thread(publish, nh);
 
     ros::Subscriber subLaserCloud = nh.subscribe<sensor_msgs::LaserScan>("/scan", 2,scanHandler);
 

grapher/src/scanGrapher.cpp

@@ -62,33 +62,32 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr ScanGrapher::getCloud()
     Steger steger(3.1, 0.5, 3.5);
 	std::vector<cv::Point2f> points;
     cv::Mat image=getGrid();
-        cv::Mat fimage;
-	    image.convertTo(fimage, CV_32FC1);
-        points.clear();
-        auto start = std::chrono::system_clock::now();
-        steger.detect(fimage,points);
-        auto end   = std::chrono::system_clock::now();
-        auto duration = std::chrono::duration_cast<std::chrono::microseconds>(end - start);
-        double time=double(duration.count()) * std::chrono::microseconds::period::num / std::chrono::microseconds::period::den;
-        std::cout<<" steger time : "<<time<<"  s"<<std::endl;
-        double resolution=resolution_;
-        if(points.size()!=0)
+    cv::Mat fimage;
+    image.convertTo(fimage, CV_32FC1);
+    points.clear();
+    auto start = std::chrono::system_clock::now();
+    steger.detect(fimage, points);
+    auto end = std::chrono::system_clock::now();
+    auto duration = std::chrono::duration_cast<std::chrono::microseconds>(end - start);
+    double time = double(duration.count()) * std::chrono::microseconds::period::num / std::chrono::microseconds::period::den;
+    std::cout << " steger time : " << time << "  s" << std::endl;
+    double resolution = resolution_;
+    if (points.size() != 0)
+    {
+        pcl::PointCloud<pcl::PointXYZ> cloud;
+        for (int i = 0; i < points.size(); ++i)
         {
-            pcl::PointCloud<pcl::PointXYZ> cloud;
-	        for (int i = 0; i < points.size(); ++i)
-	        {
-		        pcl::PointXYZ point((points[i].x-fimage.cols/2)*resolution, 
-			                (points[i].y-fimage.rows/2)*resolution, 0.0);
-		        cloud.push_back(point);
-	        }
-
-	        pcl::VoxelGrid<pcl::PointXYZ> vgfilter;
-	        vgfilter.setInputCloud(cloud.makeShared());
-	        vgfilter.setLeafSize(resolution, resolution, resolution);
-	        vgfilter.filter(cloud_);
+            pcl::PointXYZ point((points[i].x - fimage.cols / 2) * resolution,
+                                (points[i].y - fimage.rows / 2) * resolution, 0.0);
+            cloud.push_back(point);
         }
-        return cloud_.makeShared();
 
+        pcl::VoxelGrid<pcl::PointXYZ> vgfilter;
+        vgfilter.setInputCloud(cloud.makeShared());
+        vgfilter.setLeafSize(resolution, resolution, resolution);
+        vgfilter.filter(cloud_);
+    }
+    return cloud_.makeShared();
 }
 
 
@@ -227,7 +226,7 @@ void ScanGrapher::updateCell(int idx,int idy,double prob_occ)
     int size_h=int((max_point_.y-min_point_.y)/resolution_);
     if(idx<0 || idx>=size_w || idy<0 || idy>=size_h) 
         return ;
-    int gray=grid_.at<uchar>(idy,idx)+int(log(prob_occ/(1.0-prob_occ))*255.0);
+    int gray=255 - (grid_.at<uchar>(idy,idx)+int(log(prob_occ/(1.0-prob_occ))*255.0));
     gray=gray>255?255:gray;
     gray=gray<0?0:gray;
     grid_.at<uchar>(idy,idx)=gray;

mpc_control/CMakeLists.txt

@@ -0,0 +1,212 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(mpc_controller)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+add_definitions(-std=c++11 -O3)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  sensor_msgs
+  tf
+  geometry_msgs
+  nav_msgs
+)
+find_package(Eigen3 REQUIRED)
+find_package(PCL REQUIRED)
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+include_directories(
+  include 
+  ${catkin_INCLUDE_DIRS}
+  ${EIGEN3_INCLUDE_DIRS}
+  ${PCL_INCLUDE_DIRS}
+  /usr/local/include
+  ./src/
+  )
+
+link_directories(/usr/local/lib)
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+##   * add a build_depend tag for "message_generation"
+##   * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
+##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+##     but can be declared for certainty nonetheless:
+##     * add a exec_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+##   * add "message_generation" and every package in MSG_DEP_SET to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * add "message_runtime" and every package in MSG_DEP_SET to
+##     catkin_package(CATKIN_DEPENDS ...)
+##   * uncomment the add_*_files sections below as needed
+##     and list every .msg/.srv/.action file to be processed
+##   * uncomment the generate_messages entry below
+##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+#   FILES
+#   Message1.msg
+#   Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+#   FILES
+#   Service1.srv
+#   Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+#   FILES
+#   Action1.action
+#   Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+#   DEPENDENCIES
+#   std_msgs  # Or other packages containing msgs
+# )
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+##   * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+##   * add "dynamic_reconfigure" to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * uncomment the "generate_dynamic_reconfigure_options" section below
+##     and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+#   cfg/DynReconf1.cfg
+#   cfg/DynReconf2.cfg
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if your package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+#  LIBRARIES scan_matcher_ICP
+  CATKIN_DEPENDS roscpp sensor_msgs tf geometry_msgs nav_msgs 
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+#   src/${PROJECT_NAME}/scan_matcher_ICP.cpp
+# )
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+## With catkin_make all packages are built within a single CMake context
+## The recommended prefix ensures that target names across packages don't collide
+# add_executable(${PROJECT_NAME}_node src/scan_matcher_ICP_node.cpp)
+
+# add_executable(laser_scan_matcher src/laser_scan_matcher.cpp)
+# target_link_libraries(laser_scan_matcher ${catkin_LIBRARIES} ${csm_LIBRARIES} )
+
+#Create node
+add_executable(mpc_controller_node src/mpc_controller_node.cpp)
+target_link_libraries(mpc_controller_node ${catkin_LIBRARIES} ${PCL_LIBRARIES} ipopt)
+## 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
+## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
+# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+# target_link_libraries(${PROJECT_NAME}_node
+#   ${catkin_LIBRARIES}
+# )
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# install(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables and/or libraries for installation
+# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node
+#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+#   FILES_MATCHING PATTERN "*.h"
+#   PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+#   # myfile1
+#   # myfile2
+#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_scan_matcher_ICP.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)

mpc_control/package.xml

@@ -0,0 +1,57 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>mpc_controller</name>
+  <version>0.0.0</version>
+  <description>The mpc_controller package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="youchen@todo.todo">youchen</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+  <!-- The *depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+  <!--   <depend>roscpp</depend> -->
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use exec_depend for packages you need at runtime: -->
+  <!--   <exec_depend>message_runtime</exec_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>geometry_msgs</build_depend>
+  <build_depend>nav_msgs</build_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>sensor_msgs</build_depend>
+  <build_depend>tf</build_depend>
+
+  <exec_depend>geometry_msgs</exec_depend>
+  <exec_depend>nav_msgs</exec_depend>
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>sensor_msgs</exec_depend>
+  <exec_depend>tf</exec_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

mpc_control/src/MPC.cpp

@@ -0,0 +1,260 @@
+#include "MPC.h"
+#include <cppad/cppad.hpp>
+#include <cppad/ipopt/solve.hpp>
+#include "Eigen-3.3/Eigen/Core"
+#include "Eigen-3.3/Eigen/QR"
+
+using CppAD::AD;
+
+// Set the timestep length and duration
+// Currently tuned to predict 1 second worth
+size_t N = 10;
+double dt = 0.1;
+
+// 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
+// simulator around in a circle with a constant steering angle and velocity on a
+// flat terrain.
+//
+// Lf was tuned until the the radius formed by the simulating the model
+// presented in the classroom matched the previous radius.
+//
+// This is the length from front to CoG that has a similar radius.
+const double Lf = 2.67;
+
+// Set desired speed for the cost function (i.e. max speed)
+const double ref_v = 120;
+
+// 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 x_start = 0;
+size_t y_start = x_start + N;
+size_t psi_start = y_start + N;
+size_t v_start = psi_start + N;
+size_t cte_start = v_start + N;
+size_t epsi_start = cte_start + N;
+size_t delta_start = epsi_start + N;
+size_t a_start = delta_start + N - 1;
+
+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 = 2000;
+    const int epsi_cost_weight = 2000;
+    const int v_cost_weight = 1;
+    const int delta_cost_weight = 10;
+    const int a_cost_weight = 10;
+    const int delta_change_cost_weight = 100;
+    const int a_change_cost_weight = 10;
+    
+    // Cost for CTE, psi error and velocity
+    for (int t = 0; t < N; t++) {
+      fg[0] += cte_cost_weight * CppAD::pow(vars[cte_start + t], 2);
+      fg[0] += epsi_cost_weight * CppAD::pow(vars[epsi_start + t], 2);
+      fg[0] += v_cost_weight * CppAD::pow(vars[v_start + t] - ref_v, 2);
+    }
+    
+    // Costs for steering (delta) and acceleration (a)
+    for (int t = 0; t < N-1; t++) {
+      fg[0] += delta_cost_weight * CppAD::pow(vars[delta_start + t], 2);
+      fg[0] += a_cost_weight * CppAD::pow(vars[a_start + t], 2);
+    }
+    
+    // Costs related to the change in steering and acceleration (makes the ride smoother)
+    for (int t = 0; t < 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[a_start + t + 1] - vars[a_start + t], 2);
+    }
+    
+    // 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 + x_start] = vars[x_start];
+    fg[1 + y_start] = vars[y_start];
+    fg[1 + psi_start] = vars[psi_start];
+    fg[1 + v_start] = vars[v_start];
+    fg[1 + cte_start] = vars[cte_start];
+    fg[1 + epsi_start] = vars[epsi_start];
+    
+    // The rest of the constraints
+    for (int t = 1; t < N; t++) {
+      // State at time t + 1
+      AD<double> x1 = vars[x_start + t];
+      AD<double> y1 = vars[y_start + t];
+      AD<double> psi1 = vars[psi_start + t];
+      AD<double> v1 = vars[v_start + t];
+      AD<double> cte1 = vars[cte_start + t];
+      AD<double> epsi1 = vars[epsi_start + t];
+      
+      // State at time t
+      AD<double> x0 = vars[x_start + t - 1];
+      AD<double> y0 = vars[y_start + t - 1];
+      AD<double> psi0 = vars[psi_start + t - 1];
+      AD<double> v0 = vars[v_start + t - 1];
+      AD<double> cte0 = vars[cte_start + t - 1];
+      AD<double> epsi0 = vars[epsi_start + t - 1];
+      
+      // Actuator constraints at time t only
+      AD<double> delta0 = vars[delta_start + t - 1];
+      AD<double> a0 = vars[a_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
+      fg[1 + x_start + t] = x1 - (x0 + v0 * CppAD::cos(psi0) * dt);
+      fg[1 + y_start + t] = y1 - (y0 + v0 * CppAD::sin(psi0) * dt);
+      fg[1 + psi_start + t] = psi1 - (psi0 - v0 * delta0 / Lf * dt);
+      fg[1 + v_start + t] = v1 - (v0 + a0 * dt);
+      fg[1 + cte_start + t] = cte1 - ((f0-y0) + (v0 * CppAD::sin(epsi0) * dt));
+      fg[1 + epsi_start + t] = epsi1 - ((psi0 - psi_des0) - v0 * delta0 / Lf * dt);
+    }
+  }
+};
+
+//
+// MPC class definition implementation.
+//
+MPC::MPC() {}
+MPC::~MPC() {}
+
+vector<double> MPC::Solve(Eigen::VectorXd state, Eigen::VectorXd coeffs) {
+  bool ok = true;
+  typedef CPPAD_TESTVECTOR(double) Dvector;
+  
+  // State vector holds all current values neede for vars below
+  double x = state[0];
+  double y = state[1];
+  double psi = state[2];
+  double v = state[3];
+  double cte = state[4];
+  double epsi = state[5];
+
+  // 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 * 6 + (N - 1) * 2;
+  // Setting the number of constraints
+  size_t n_constraints = N * 6;
+
+  // 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 < delta_start; i++) {
+    vars_lowerbound[i] = -1.0e19;
+    vars_upperbound[i] = 1.0e19;
+  }
+  
+  // The upper and lower limits of delta are set to -25 and 25
+  // degrees (values in radians).
+  for (int i = delta_start; i < a_start; i++) {
+    vars_lowerbound[i] = -0.436332;
+    vars_upperbound[i] = 0.436332;
+  }
+  
+  // Acceleration/decceleration upper and lower limits.
+  for (int i = a_start; i < n_vars; i++) {
+    vars_lowerbound[i] = -1.0;
+    vars_upperbound[i] = 1.0;
+  }
+
+  // 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;
+  }
+  
+  // Start lower and upper limits at current values
+  constraints_lowerbound[x_start] = x;
+  constraints_lowerbound[y_start] = y;
+  constraints_lowerbound[psi_start] = psi;
+  constraints_lowerbound[v_start] = v;
+  constraints_lowerbound[cte_start] = cte;
+  constraints_lowerbound[epsi_start] = epsi;
+  
+  constraints_upperbound[x_start] = x;
+  constraints_upperbound[y_start] = y;
+  constraints_upperbound[psi_start] = psi;
+  constraints_upperbound[v_start] = v;
+  constraints_upperbound[cte_start] = cte;
+  constraints_upperbound[epsi_start] = epsi;
+
+  // object that computes objective and constraints
+  FG_eval fg_eval(coeffs);
+
+  //
+  // 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.
+  vector<double> solved;
+  solved.push_back(solution.x[delta_start]);
+  solved.push_back(solution.x[a_start]);
+  for (int i = 0; i < N; ++i) {
+    solved.push_back(solution.x[x_start + i]);
+    solved.push_back(solution.x[y_start + i]);
+  }
+  
+  return solved;
+
+}

mpc_control/src/MPC.h

@@ -0,0 +1,20 @@
+#ifndef MPC_H
+#define MPC_H
+
+#include <vector>
+#include "Eigen-3.3/Eigen/Core"
+
+using namespace std;
+
+class MPC {
+ public:
+  MPC();
+
+  virtual ~MPC();
+
+  // Solve the model given an initial state and polynomial coefficients.
+  // Return the first actuations.
+  vector<double> Solve(Eigen::VectorXd state, Eigen::VectorXd coeffs);
+};
+
+#endif /* MPC_H */

mpc_control/src/main.cpp

@@ -0,0 +1,243 @@
+#include <math.h>
+#include <uWS/uWS.h>
+#include <chrono>
+#include <iostream>
+#include <thread>
+#include <vector>
+#include "Eigen-3.3/Eigen/Core"
+#include "Eigen-3.3/Eigen/QR"
+#include "MPC.h"
+#include "json.hpp"
+
+// for convenience
+using json = nlohmann::json;
+
+// 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(); }
+
+// Checks if the SocketIO event has JSON data.
+// If there is data the JSON object in string format will be returned,
+// else the empty string "" will be returned.
+string hasData(string s) {
+  auto found_null = s.find("null");
+  auto b1 = s.find_first_of("[");
+  auto b2 = s.rfind("}]");
+  if (found_null != string::npos) {
+    return "";
+  } else if (b1 != string::npos && b2 != string::npos) {
+    return s.substr(b1, b2 - b1 + 2);
+  }
+  return "";
+}
+
+// Evaluate a polynomial.
+double 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;
+}
+
+// Fit a polynomial.
+// Adapted from
+// https://github.com/JuliaMath/Polynomials.jl/blob/master/src/Polynomials.jl#L676-L716
+Eigen::VectorXd 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() {
+  uWS::Hub h;
+
+  // MPC is initialized here!
+  MPC mpc;
+
+  h.onMessage([&mpc](uWS::WebSocket<uWS::SERVER> ws, char *data, size_t length,
+                     uWS::OpCode opCode) {
+    // "42" at the start of the message means there's a websocket message event.
+    // The 4 signifies a websocket message
+    // The 2 signifies a websocket event
+    string sdata = string(data).substr(0, length);
+    cout << sdata << endl;
+    if (sdata.size() > 2 && sdata[0] == '4' && sdata[1] == '2') {
+      string s = hasData(sdata);
+      if (s != "") {
+        auto j = json::parse(s);
+        string event = j[0].get<string>();
+        if (event == "telemetry") {
+          // j[1] is the data JSON object
+          vector<double> ptsx = j[1]["ptsx"];
+          vector<double> ptsy = j[1]["ptsy"];
+          double px = j[1]["x"];
+          double py = j[1]["y"];
+          double psi = j[1]["psi"];
+          double v = j[1]["speed"];
+          double delta = j[1]["steering_angle"];
+          double a = j[1]["throttle"];
+          
+          // Need Eigen vectors for polyfit
+          Eigen::VectorXd ptsx_car(ptsx.size());
+          Eigen::VectorXd ptsy_car(ptsy.size());
+          
+          // Transform the points to the vehicle's orientation
+          for (int i = 0; i < ptsx.size(); i++) {
+            double x = ptsx[i] - px;
+            double y = ptsy[i] - py;
+            ptsx_car[i] = x * cos(-psi) - y * sin(-psi);
+            ptsy_car[i] = x * sin(-psi) + y * cos(-psi);
+          }
+          
+          /*
+          * Calculate steering angle and throttle using MPC.
+          * Both are in between [-1, 1].
+          * Simulator has 100ms latency, so will predict state at that point in time.
+          * This will help the car react to where it is actually at by the point of actuation.
+          */
+          
+          // Fits a 3rd-order polynomial to the above x and y coordinates
+          auto coeffs = polyfit(ptsx_car, ptsy_car, 3);
+          
+          // Calculates the cross track error
+          // Because points were transformed to vehicle coordinates, x & y equal 0 below.
+          // 'y' would otherwise be subtracted from the polyeval value
+          double cte = polyeval(coeffs, 0);
+          
+          // Calculate the orientation error
+          // Derivative of the polyfit goes in atan() below
+          // Because x = 0 in the vehicle coordinates, the higher orders are zero
+          // Leaves only coeffs[1]
+          double epsi = -atan(coeffs[1]);
+          
+          // Center of gravity needed related to psi and epsi
+          const double Lf = 2.67;
+          
+          // Latency for predicting time at actuation
+          const double dt = 0.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_psi = 0.0 + v * -delta / Lf * dt;
+          double pred_v = v + a * dt;
+          double pred_cte = cte + v * sin(epsi) * dt;
+          double pred_epsi = epsi + v * -delta / Lf * dt;
+          
+          // Feed in the predicted state values
+          Eigen::VectorXd state(6);
+          state << pred_px, pred_py, pred_psi, pred_v, pred_cte, pred_epsi;
+          
+          // Solve for new actuations (and to show predicted x and y in the future)
+          auto vars = mpc.Solve(state, coeffs);
+          
+          // Calculate steering and throttle
+          // Steering must be divided by deg2rad(25) to normalize within [-1, 1].
+          // Multiplying by Lf takes into account vehicle's turning ability
+          double steer_value = vars[0] / (deg2rad(25) * Lf);
+          double throttle_value = vars[1];
+          
+          // Send values to the simulator
+          json msgJson;
+          msgJson["steering_angle"] = steer_value;
+          msgJson["throttle"] = throttle_value;
+
+          // Display the MPC predicted trajectory
+          vector<double> mpc_x_vals = {state[0]};
+          vector<double> mpc_y_vals = {state[1]};
+
+          // add (x,y) points to list here, points are in reference to the vehicle's coordinate system
+          // the points in the simulator are connected by a Green line
+          
+          for (int i = 2; i < vars.size(); i+=2) {
+            mpc_x_vals.push_back(vars[i]);
+            mpc_y_vals.push_back(vars[i+1]);
+          }
+
+          msgJson["mpc_x"] = mpc_x_vals;
+          msgJson["mpc_y"] = mpc_y_vals;
+
+          // Display the waypoints/reference line
+          vector<double> next_x_vals;
+          vector<double> next_y_vals;
+
+          // add (x,y) points to list here, points are in reference to the vehicle's coordinate system
+          // the points in the simulator are connected by a Yellow line
+          double poly_inc = 2.5;
+          int num_points = 25;
+          
+          for (int i = 1; i < num_points; i++) {
+            next_x_vals.push_back(poly_inc * i);
+            next_y_vals.push_back(polyeval(coeffs, poly_inc * i));
+          }
+          
+          msgJson["next_x"] = next_x_vals;
+          msgJson["next_y"] = next_y_vals;
+
+          auto msg = "42[\"steer\"," + msgJson.dump() + "]";
+          std::cout << msg << std::endl;
+          // Latency
+          // The purpose is to mimic real driving conditions where
+          // the car doesn't actuate the commands instantly.
+          this_thread::sleep_for(chrono::milliseconds(100));
+          ws.send(msg.data(), msg.length(), uWS::OpCode::TEXT);
+        }
+      } else {
+        // Manual driving
+        std::string msg = "42[\"manual\",{}]";
+        ws.send(msg.data(), msg.length(), uWS::OpCode::TEXT);
+      }
+    }
+  });
+
+  // We don't need this since we're not using HTTP but if it's removed the
+  // program
+  // doesn't compile :-(
+  h.onHttpRequest([](uWS::HttpResponse *res, uWS::HttpRequest req, char *data,
+                     size_t, size_t) {
+    const std::string s = "<h1>Hello world!</h1>";
+    if (req.getUrl().valueLength == 1) {
+      res->end(s.data(), s.length());
+    } else {
+      // i guess this should be done more gracefully?
+      res->end(nullptr, 0);
+    }
+  });
+
+  h.onConnection([&h](uWS::WebSocket<uWS::SERVER> ws, uWS::HttpRequest req) {
+    std::cout << "Connected!!!" << std::endl;
+  });
+
+  h.onDisconnection([&h](uWS::WebSocket<uWS::SERVER> ws, int code,
+                         char *message, size_t length) {
+    ws.close();
+    std::cout << "Disconnected" << std::endl;
+  });
+
+  int port = 4567;
+  if (h.listen(port)) {
+    std::cout << "Listening to port " << port << std::endl;
+  } else {
+    std::cerr << "Failed to listen to port" << std::endl;
+    return -1;
+  }
+  h.run();
+}

mpc_control/src/mpc_controller_node.cpp

@@ -0,0 +1,543 @@
+#include  "mpc_controller_node.h"
+using CppAD::AD;
+
+// Set the timestep length and duration
+// Currently tuned to predict 1 second worth
+size_t N = 10;
+double dt = 0.1;
+
+// 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
+// simulator around in a circle with a constant steering angle and velocity on a
+// flat terrain.
+//
+// Lf was tuned until the the radius formed by the simulating the model
+// presented in the classroom matched the previous radius.
+//
+// This is the length from front to CoG that has a similar radius.
+const double Lf = 1;
+
+// Set desired speed for the cost function (i.e. max speed)
+double ref_v = 2.6;
+
+bool forward = true;
+
+// 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.
+// x，y； psi旋转角； v速度值； cte实际路径误差； epsi角度误差； a_left左轮加速度； a_right右轮加速度
+size_t x_start = 0;
+size_t y_start = x_start + N;
+size_t psi_start = y_start + N;
+size_t v_left_start = psi_start + N;
+size_t v_right_start = v_left_start + N;
+size_t cte_start = v_right_start + N;
+size_t epsi_start = cte_start + N;
+// size_t delta_start = epsi_start + N;
+size_t a_left_start = epsi_start + N;
+size_t a_right_start = a_left_start + N - 1;
+
+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 = 5000;
+    const int epsi_cost_weight = 5000;
+    const int v_cost_weight = 1;
+    // const int delta_cost_weight = 10;
+    const int a_cost_weight = 10;
+    // const int delta_change_cost_weight = 100;
+    const int a_change_cost_weight = 10;
+
+    // std::cout<<"fg0: "<<fg[0]<<std::endl;
+    // Cost for CTE, psi error and velocity
+    for (int t = 0; t < N; t++) {
+      fg[0] += cte_cost_weight * CppAD::pow(vars[cte_start + t], 2);
+      fg[0] += epsi_cost_weight * CppAD::pow(vars[epsi_start + t], 2);
+      fg[0] += v_cost_weight * CppAD::pow(vars[v_left_start + t] - ref_v, 2);
+      fg[0] += v_cost_weight * CppAD::pow(vars[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 < N-1; t++) {
+      // fg[0] += delta_cost_weight * CppAD::pow(vars[delta_start + t], 2);
+      fg[0] += a_cost_weight * CppAD::pow(vars[a_left_start + t], 2);
+      fg[0] += a_cost_weight * CppAD::pow(vars[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 < 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[a_left_start + t + 1] - vars[a_left_start + t], 2);
+      fg[0] += a_change_cost_weight * pow(vars[a_right_start + t + 1] - vars[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 + x_start] = vars[x_start];
+    fg[1 + y_start] = vars[y_start];
+    fg[1 + psi_start] = vars[psi_start];
+    fg[1 + v_left_start] = vars[v_left_start];
+    fg[1 + v_right_start] = vars[v_right_start];
+    fg[1 + cte_start] = vars[cte_start];
+    fg[1 + epsi_start] = vars[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 < N; t++) {
+      // State at time t + 1
+      AD<double> x1 = vars[x_start + t];
+      AD<double> y1 = vars[y_start + t];
+      AD<double> psi1 = vars[psi_start + t];
+      AD<double> v_left1 = vars[v_left_start + t];
+      AD<double> v_right1 = vars[v_right_start + t];
+      AD<double> cte1 = vars[cte_start + t];
+      AD<double> epsi1 = vars[epsi_start + t];
+      
+      // State at time t
+      AD<double> x0 = vars[x_start + t - 1];
+      AD<double> y0 = vars[y_start + t - 1];
+      AD<double> psi0 = vars[psi_start + t - 1];
+      AD<double> v_left0 = vars[v_left_start + t - 1];
+      AD<double> v_right0 = vars[v_right_start + t - 1];
+      AD<double> cte0 = vars[cte_start + t - 1];
+      AD<double> epsi0 = vars[epsi_start + t - 1];
+      
+      // Actuator constraints at time t only
+      AD<double> a_left0 = vars[a_left_start + t - 1];
+      AD<double> a_right0 = vars[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 + x_start + t] = x1 - (x0 + v0 * CppAD::cos(psi0) * dt);
+      fg[1 + y_start + t] = y1 - (y0 + v0 * CppAD::sin(psi0) * dt);
+      fg[1 + psi_start + t] = psi1 - (psi0 - v_diff0 / Lf * dt);
+      // fg[1 + v_start + t] = v1 - (v0 + a0 * dt);
+      fg[1 + v_left_start + t] = v_left1 - (v_left0 + a_left0 * dt);
+      fg[1 + v_right_start + t] = v_right1 - (v_right0 + a_right0 * dt);
+      fg[1 + cte_start + t] = cte1 - ((f0-y0) + (v0 * CppAD::sin(epsi0) * dt));// 预测路径误差 - (y方向与路径差 + 当前角度误差带来的y方向移动)
+      fg[1 + epsi_start + t] = epsi1 - ((psi0 - psi_des0) - v_diff0 / Lf * dt);// 预测角度误差 - (当前与目标角度差 - 当前角度偏转或补偿)
+    }    
+  }
+};
+
+mpc_controller::mpc_controller(ros::NodeHandle nh):nh_(nh)
+{
+  omega = 0;
+  v_left = 0;
+  v_right = 0;
+  a_left = 0;
+  a_right = 0;
+  last_theta = 0;
+  path_count = 0;
+  last_pos = pcl::PointXYZ(0, 0, 0);
+  curr_pos = pcl::PointXYZ(0, 0, 0);
+
+  //创建全局路径
+  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;
+    ptsx.push_back(x);
+    ptsy.push_back(y);
+    // std::cout<<"x,y: "<<x<<", "<<y<<std::endl;
+  }
+
+  //发布与订阅
+  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_controller::poseHandler, this);
+}
+
+mpc_controller::~mpc_controller()
+{
+}
+
+void mpc_controller::poseHandler(const turtlesim::Pose::ConstPtr &pose_msg)
+{
+  //发布全局路径
+  nav_msgs::Path path;
+  path.header.frame_id = "map";
+  path.header.seq = ++path_count;
+  path.header.stamp = ros::Time::now();
+  for (size_t i = 0; i < ptsx.size(); i++)
+  {
+    geometry_msgs::PoseStamped temp;
+    temp.header = path.header;
+    temp.pose.position.x = ptsx[i];
+    temp.pose.position.y = ptsy[i];
+    double theta = atan2(ptsy[i], ptsx[i]);
+    // if(ptsx[i]*ptsy[i]<0)
+    //   theta = theta-M_PI;
+    // std::cout<<"x,y,theta: "<<ptsx[i]<<", "<<ptsy[i]<<", "<<theta<<std::endl;
+    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);
+    path.poses.push_back(temp);
+  }
+  global_path_publisher_.publish(path);
+
+  //发布tf
+  tf::Transform transform;
+  transform.setOrigin(tf::Vector3(pose_msg->x, pose_msg->y, 0.0));
+  transform.setRotation(tf::Quaternion(0, 0, sin(pose_msg->theta/2.0), cos(pose_msg->theta/2.0)));
+  tf::StampedTransform transform_msg(transform, ros::Time::now(), "map", "base_link");
+  tf_broadcaster_.sendTransform(transform_msg);
+  
+  curr_pos = pcl::PointXYZ(pose_msg->x, pose_msg->y, 0);
+  double v = pose_msg->linear_velocity;
+  omega = pose_msg->angular_velocity;
+  // std::cout<<"---------------"<<std::endl;
+  double theta_diff = pose_msg->theta - last_theta;//omega*dt;
+  // std::cout<<"linear velo: "<<v<<", theta_diff: "<<theta_diff<<std::endl;
+  v_left = v - (theta_diff*Lf)/(2*dt);
+  v_right = v + (theta_diff*Lf)/(2*dt);
+
+  // Need Eigen vectors for polyfit
+  // 筛选前方6点
+  Eigen::VectorXd ptsx_car(N);
+  Eigen::VectorXd ptsy_car(N);
+  int count=0;
+  // Transform the points to the vehicle's orientation
+  // 平移加反向旋转到小车坐标系
+  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(-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)
+    {
+      ptsx_car[count] = trans_x;
+      ptsy_car[count] = trans_y;
+      // std::cout << "path " << count << " : " << ptsx_car[count] << ", " << ptsy_car[count] << std::endl;
+      count++;
+    }
+    // if(ptsx_car[i]>0)
+    //   std::cout<<"path "<<i<<" : "<<ptsx_car[i]<<", "<<ptsy_car[i]<<std::endl;
+  }
+  if (count < N)
+  {
+    ref_v /= (N - count);
+  }
+
+  if (pose_msg->x > 10 && forward)
+  {
+    ref_v = -2.6;
+    forward = false;
+  }
+  else if(pose_msg->x < 1 && !forward)
+  {
+    ref_v = 2.6;
+    forward = true;
+  }
+  std::cout<<ref_v<<", "<<forward<<std::endl;
+
+  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]);
+  // std::cout<<"****************"<<std::endl;
+  // // std::cout<<"coeffs: "<<coeffs<<std::endl;
+  // std::cout<<"cte: "<<cte<<std::endl;
+  // std::cout<<"etheta: "<<etheta<<std::endl;
+  // std::cout<<"theta_diff: "<<theta_diff<<std::endl;
+  // std::cout<<"****************"<<std::endl;
+
+  // 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;
+
+  // Feed in the predicted state values
+  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;
+  }
+
+  //更新速度并发布cmd_vel
+  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;
+  // 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.y = 0;
+  velocity.linear.z = 0;
+  velocity.angular.x = 0;
+  velocity.angular.y = 0;
+  velocity.angular.z = (v_right-v_left)/Lf;
+  // 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);
+
+  last_theta = pose_msg->theta;
+  last_pos = curr_pos;
+}
+
+void mpc_controller::processData(){
+    ros::Time t = ros::Time::now();
+    tf::StampedTransform tf; //Transform类的子类
+    try
+    {
+        tf_listener_.waitForTransform(
+            "laser", "map", t, ros::Duration(1.0));
+        tf_listener_.lookupTransform(
+            "laser", "map", t, tf);
+    }
+    catch (tf::TransformException ex)
+    {
+        ROS_WARN("Could not get initial transform from world to base frame, %s", ex.what());
+        return;
+    }
+    // ros::Time t2 = ros::Time::now();
+    // std::cout << "milli sec: " << (t2 - t).toSec() * 1000 << std::endl;
+}
+
+// Evaluate a polynomial.求y
+double mpc_controller::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;
+}
+
+// Fit a polynomial.求参数
+Eigen::VectorXd mpc_controller::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;
+}
+
+std::vector<double> mpc_controller::Solve(Eigen::VectorXd state, Eigen::VectorXd coeffs) {
+  bool ok = true;
+  typedef CPPAD_TESTVECTOR(double) Dvector;
+  // State vector holds all current values neede for vars below
+  double x = state[0];
+  double y = state[1];
+  double psi = state[2];
+  double v_left = state[3];
+  double v_right = state[4];
+  double cte = state[5];
+  double epsi = state[6];
+
+  // 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 * 7 + (N - 1) * 2;
+  // Setting the number of constraints
+  size_t n_constraints = N * 7;
+
+  // 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 < a_left_start; i++) {
+    vars_lowerbound[i] = -1.0e6;
+    vars_upperbound[i] = 1.0e6;
+  }
+  
+  // // The upper and lower limits of delta are set to -25 and 25
+  // // degrees (values in radians).
+  // for (int i = delta_start; i < a_start; i++) {
+  //   vars_lowerbound[i] = -0.436332;
+  //   vars_upperbound[i] = 0.436332;
+  // }
+  
+  // Acceleration/decceleration upper and lower limits.
+  for (int i = a_left_start; i < n_vars; i++) {
+    vars_lowerbound[i] = -5.5;
+    vars_upperbound[i] = 5.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;
+  }
+  
+  // Start lower and upper limits at current values
+  constraints_lowerbound[x_start] = x;
+  constraints_lowerbound[y_start] = y;
+  constraints_lowerbound[psi_start] = psi;
+  constraints_lowerbound[v_left_start] = v_left;
+  constraints_lowerbound[v_right_start] = v_right;
+  constraints_lowerbound[cte_start] = cte;
+  constraints_lowerbound[epsi_start] = epsi;
+  
+  constraints_upperbound[x_start] = x;
+  constraints_upperbound[y_start] = y;
+  constraints_upperbound[psi_start] = psi;
+  constraints_upperbound[v_left_start] = v_left;
+  constraints_upperbound[v_right_start] = v_right;
+  constraints_upperbound[cte_start] = cte;
+  constraints_upperbound[epsi_start] = epsi;
+
+  // object that computes objective and constraints
+  FG_eval fg_eval(coeffs);
+  //
+  // 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
+  // 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 <<"status "<<ok<< ", Cost " << cost << std::endl;
+
+  // Return the first actuator values, along with predicted x and y values to plot in the simulator.
+  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;
+  }
+  return solved;
+}
+
+int main(int argc, char** argv){
+    ros::init(argc, argv, "mpc_controller_node");
+    ros::NodeHandle nh;
+    mpc_controller controller(nh);
+    ros::Rate loop_rate(50);
+        
+    // 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_controller();
+    return 0;
+}

mpc_control/src/mpc_controller_node.h

@@ -0,0 +1,84 @@
+#ifndef MPC_CONTROLLER_NODE_HH
+#define MPC_CONTROLLER_NODE_HH 
+
+#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>
+
+// 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_controller{
+
+public:
+    mpc_controller(ros::NodeHandle nh);
+    ~mpc_controller();
+
+    void processData();
+
+    void poseHandler(const turtlesim::Pose::ConstPtr& pose_msg);
+
+    // Solve the model given an initial state and polynomial coefficients.
+    // Return the first actuations.
+    std::vector<double> Solve(Eigen::VectorXd state, Eigen::VectorXd coeffs);
+
+    // Evaluate a polynomial.
+    double polyeval(Eigen::VectorXd coeffs, double x);
+
+    // Fit a polynomial.
+    // Adapted from
+    // https://github.com/JuliaMath/Polynomials.jl/blob/master/src/Polynomials.jl#L676-L716
+    Eigen::VectorXd polyfit(Eigen::VectorXd xvals, Eigen::VectorXd yvals, int order);
+public:
+    // Set the timestep length and duration
+    // Currently tuned to predict 1 second worth
+    size_t N = 10;
+    double dt = 0.1;
+
+    // // 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
+    // // simulator around in a circle with a constant steering angle and velocity on a
+    // // flat terrain.
+    // //
+    // // Lf was tuned until the the radius formed by the simulating the model
+    // // presented in the classroom matched the previous radius.
+    // //
+    // // This is the length from front to CoG that has a similar radius.
+    // const double Lf = 1.0;
+
+    // // Set desired speed for the cost function (i.e. max speed)
+    // double ref_v = 3;
+
+private:
+    ros::NodeHandle nh_;
+    tf::TransformListener tf_listener_;
+    tf::TransformBroadcaster tf_broadcaster_;
+    ros::Publisher vel_publisher_;
+    ros::Publisher global_path_publisher_;
+    ros::Publisher local_path_publisher_;
+    int path_count;
+    ros::Subscriber pose_sub_;
+
+    double v_left, v_right, last_theta, a_left, a_right, omega;
+    pcl::PointXYZ last_pos, curr_pos;
+    std::vector<double> ptsx;
+    std::vector<double> ptsy;
+};
+
+#endif /* MPC_CONTROLLER_NODE_HH */

rbx1/rbx1_nav/launch/amcl_sick.launch

@@ -2,8 +2,9 @@
    <!-->
   <node pkg="beginner_tutorials" type="talker" name="talker"/>
    <-->
-  <node pkg="map_server" type="map_server" name="map_server" args="/home/zx/Downloads/map.yaml"/>
-  <node pkg="tf" type="static_transform_publisher" name="link1_broadcaster" args="0 0 0 0 0 0 1 base_link laser 30" />
+  <param name="use_sim_time" value="true" />
+  <node pkg="map_server" type="map_server" name="map_server" args="/home/youchen/Documents/git_ws/map1.yaml"/>
+  <node pkg="tf" type="static_transform_publisher" name="link1_broadcaster" args="0 0 0 0 0 0 1 /base_link /laser 30" />
   <!--><node pkg="tf" type="static_transform_publisher" name="odom_base_link_broadcaster" 
 args="0.1 0 0 0 0 0 1 odom base_link 30" /><-->
 	<!--><node pkg="tf" type="static_transform_publisher" name="odom_raw_to_map_broadcaster" 
@@ -12,57 +13,56 @@ args="0.1 0 0 0 0 0 1 map odom_raw 30" /><-->
 	<node pkg="laser_scan_matcher" type="laser_scan_matcher_node"
     name="laser_scan_matcher_node" output="screen">
 
-    <param name="max_iterations" value="20"/>
-	<param name="fixed_frame" value="odom"/>
+    <param name="max_iterations" value="40"/>
+    <param name="fixed_frame" value="/odom"/>
+    <param name="base_frame" value="/base_link"/>
   </node>
 	
-	
 	<!-- amcl node -->
   <node pkg="amcl" type="amcl" name="amcl" output="screen">
 
-  <!--<remap from="scan" to="scan"/>-->
-  <!-- Publish scans from best pose at a max of 10 Hz -->
-  <param name="use_map_topic" value="true"/>
-  <param name="odom_model_type" value="omni"/>
-  <param name="odom_alpha5" value="0.1"/>
-  <param name="transform_tolerance" value="0.5" />
-  <param name="gui_publish_rate" value="10.0"/>
-  <param name="laser_max_beams" value="300"/>
-  <param name="min_particles" value="100"/>
-  <param name="max_particles" value="5000"/>
-  <param name="kld_err" value="0.05"/>
-  <param name="kld_z" value="0.99"/>
-  <param name="odom_alpha1" value="0.1"/>
-  <param name="odom_alpha2" value="0.1"/>
-  <!-- translation std dev, m -->
-  <param name="odom_alpha3" value="0.1"/>
-  <param name="odom_alpha4" value="0.1"/>
-  <param name="laser_z_hit" value="0.9"/>
-  <param name="laser_z_short" value="0.05"/>
-  <param name="laser_z_max" value="0.05"/>
-  <param name="laser_z_rand" value="0.5"/>
-  <param name="laser_sigma_hit" value="0.2"/>
-  <param name="laser_lambda_short" value="0.1"/>
-  <param name="laser_lambda_short" value="0.1"/>
-  <param name="laser_model_type" value="likelihood_field"/>
-  <!-- <param name="laser_model_type" value="beam"/> -->
- <param name="laser_min_range" value="0.5"/>
- <param name="laser_max_range" value="5"/>
-  <param name="laser_likelihood_max_dist" value="2.0"/>
-  <param name="update_min_d" value="0.1"/>
-  <param name="update_min_a" value="0.5"/>
-  <param name="resample_interval" value="1"/>
-  <param name="transform_tolerance" value="0.1"/>
-  <param name="recovery_alpha_slow" value="0.0"/>
-  <param name="recovery_alpha_fast" value="0.0"/>
-	  
-	  
-  <param name="initial_cov_xx" value="25."/>
-  <param name="initial_cov_yy" value="25."/>
-	  <param name="initial_cov_aa" value="0.25"/>
+    <param name="odom_frame_id" value="odom"/>
+    <param name="base_frame_id" value="base_link"/>
+    <param name="global_frame_id" value="map"/>
+    <!-- Publish scans from best pose at a max of 10 Hz -->
+    <param name="use_map_topic" value="false"/>
+    <param name="odom_model_type" value="omni"/>
+    <param name="odom_alpha5" value="0.1"/>
+    <param name="transform_tolerance" value="0.05" />
+    <param name="gui_publish_rate" value="5.0"/>
+    <param name="laser_max_beams" value="100"/>
+    <param name="min_particles" value="100"/>
+    <param name="max_particles" value="1000"/>
+    <param name="kld_err" value="0.05"/>
+    <param name="kld_z" value="0.99"/>
+    <param name="odom_alpha1" value="0.1"/>
+    <param name="odom_alpha2" value="0.1"/>
+    <!-- translation std dev, m -->
+    <param name="odom_alpha3" value="0.1"/>
+    <param name="odom_alpha4" value="0.1"/>
+    <param name="laser_z_hit" value="0.9"/>
+    <param name="laser_z_short" value="0.05"/>
+    <param name="laser_z_max" value="0.05"/>
+    <param name="laser_z_rand" value="0.5"/>
+    <param name="laser_sigma_hit" value="0.2"/>
+    <param name="laser_lambda_short" value="0.1"/>
+    <param name="laser_lambda_short" value="0.1"/>
+    <param name="laser_model_type" value="likelihood_field"/>
+    <!-- <param name="laser_model_type" value="beam"/> -->
+  <param name="laser_min_range" value="-1.0"/>
+  <param name="laser_max_range" value="-1.0"/>
+    <param name="laser_likelihood_max_dist" value="2.0"/>
+    <param name="update_min_d" value="0.1"/>
+    <param name="update_min_a" value="0.5"/>
+    <param name="resample_interval" value="1"/>
+    <param name="transform_tolerance" value="0.1"/>
+    <param name="recovery_alpha_slow" value="0.0"/>
+    <param name="recovery_alpha_fast" value="0.0"/>
+      
+    <param name="initial_cov_xx" value="16"/>
+    <param name="initial_cov_yy" value="16"/>
+    <param name="initial_cov_aa" value="0.00685"/>
 	  
-
-
-</node>
+  </node>
 
 </launch>

scan_matcher_ICP/scripts/test.launch

@@ -0,0 +1,11 @@
+<!--
+Example launch file: launches the scan matcher with pre-recorded data
+-->
+<launch>
+
+  <!--node pkg="tf" type="static_transform_publisher" name="map_to_laser"
+    args="0.0 0.0 0.0 0.0 0.0 0.0 1.0 /map /laser 40" /-->
+
+  <node name="map_server" pkg="map_server" type="map_server" args="$(find rbx1_nav)/maps/blank_map.yaml"/>
+
+</launch>