Ego-Planner接MPC（一）

Ego Planner

Ego Planner是一种常用的规划算法，由浙大Fast Lab开发，开源地址为：GitHub
在本任务中，需要将Ego Planner的规划路径发送给MPC，由MPC进行控制，相关控制流可以参考浙大开源的Fast-Drone-250项目，开源地址为：GitHub

优点

1. 有避障、规划、控制程序
2. 使用人数多，稳定
3. 有啥bug网上都能查
4. 占用小，可移植性强

缺点

1. 不是自己写的，有啥坑都得踩一遍，没有自主可控性

修改路径发布

• Ego Planner的路径是通过B样条进行拟合得到的，B样条通过话题planning/bspline进行发布，具体函数在traj_server.cpp的bsplineCallback函数中
• B样条话题只在重规划之后才发布一次，因此不能在回调函数中直接发给MPC，traj_server.cpp中有一个定时执行的函数：

  ros::Timer cmd_timer = node.createTimer(ros::Duration(0.01), cmdCallback);

• cmdCallback函数的发布频率为100Hz，在Fast-Drone-250中其原本的目的是将控制命令发送给px4ctrl功能包进行控制执行，本项目中使用MPC进行控制，因此不需要这个控制命令，那么轨迹的发布可以放在这个函数中进行，具体代码如下：

  // traj_msg_存的是要发给MPC的路径点
  ius_msgs::Trajectory traj_msg_;
  
  void bsplineCallback(ego_planner::BsplineConstPtr msg)
  {
  // parse pos traj
  
  Eigen::MatrixXd pos_pts(3, msg->pos_pts.size());
  
  Eigen::VectorXd knots(msg->knots.size());
  for (size_t i = 0; i < msg->knots.size(); ++i)
  {
      knots(i) = msg->knots[i];
  }
  
  for (size_t i = 0; i < msg->pos_pts.size(); ++i)
  {
      pos_pts(0, i) = msg->pos_pts[i].x;
      pos_pts(1, i) = msg->pos_pts[i].y;
      pos_pts(2, i) = msg->pos_pts[i].z;
  }
  
  traj_msg_.pos.clear();
  traj_msg_.yaw.clear();
  traj_msg_.time.clear();
  traj_msg_.header.stamp = ros::Time::now();
  traj_msg_.header.frame_id = "world";
  for (size_t i = 0; i< msg->pos_pts.size(); ++i) {
      traj_msg_.pos.push_back(msg->pos_pts[i]);
      traj_msg_.yaw.push_back(0.0);
      traj_msg_.time.push_back(msg->knots[i]);
  }
  
  UniformBspline pos_traj(pos_pts, msg->order, 0.1);
  pos_traj.setKnot(knots);
  
  start_time_ = msg->start_time;
  traj_id_ = msg->traj_id;
  
  traj_.clear();
  traj_.push_back(pos_traj);
  traj_.push_back(traj_[0].getDerivative());
  traj_.push_back(traj_[1].getDerivative());
  
  traj_duration_ = traj_[0].getTimeSum();
  
  receive_traj_ = true;
  }
  
  void cmdCallback(const ros::TimerEvent &e)
  {
  /* no publishing before receive traj_ */
  if (!receive_traj_)
      return;
  traj_msg_.header.stamp = ros::Time::now();
  traj_pub.publish(traj_msg_);
  
  ros::Time time_now = ros::Time::now();
  double t_cur = (time_now - start_time_).toSec();
  ........
  }

• 自此，路径从Ego Planner中发送给MPC程序