Ego-Planner接MPC（二）

Ego Planner中一些注意事项

1. 规划程序设定了规划的最高高度，变量名为mp_.virtual_ceil_height_
   在advanced_param.xml中进行配置，参数名为
   <param name="grid_map/virtual_ceil_height" value="2.5"/>
   实现方式为：直接加一个天花板障碍物

   // add virtual ceiling to limit flight height
   if (mp_.virtual_ceil_height_ > -0.5)
   {
       int ceil_id = floor((mp_.virtual_ceil_height_ - mp_.map_origin_(2)) * mp_.resolution_inv_);
       for (int x = md_.local_bound_min_(0); x <= md_.local_bound_max_(0); ++x)
       for (int y = md_.local_bound_min_(1); y <= md_.local_bound_max_(1); ++y)
       {
           md_.occupancy_buffer_inflate_[toAddress(x, y, ceil_id)] = 1;
       }
   }

2. 在通过WayPoint设定规划目标时，z轴不能小于0.1:

   void EGOReplanFSM::waypointCallback(const nav_msgs::PathConstPtr &msg)
   {
       if (msg->poses[0].pose.position.z < -0.1)
       return;
   
       cout << "Triggered!" << endl;
       trigger_ = true;
       .....
   }

3. Ego Planner状态机的切换：

               INIT
              /    \
           设点    指点
           /         \
   GEN_NEW_TRAJ <-- WAIT_TARGET       EMERGENCY_STOP
         |<-------------
         |             |
     EXEC_TRAJ --> REPLAN_TRAJ