ego_planner代码解读

admin管理员组

文章数量:1622542

概述

• bspline_opt 优化函数

• drone_detect单机忽略

• path_searching A*算法

• plan_env建图代码

• plan_manage代码入口

• rosmsg_tcp_bridge多机集群通信

• traj_utils轨迹生成及可视化相关代码

single_run_in_exp.launch |odom、map信息 |调用advanced_param_exp.xml (基本参数、设备） | |ego_planner_node可执行文件（ego_planner_node、ego_replan_fsm、planner_manage.cpp文件） | |camera信息、速度加速度设置 | |flaght_type 飞行类别 | |use_distinctive_trajs | |_planning_horizon |traj_server可执行文件（接收bspline结果轨迹，发布给飞控）

planner/plan_manage

launch

• roslaunch ego_planner single_run_in_exp.launch文件中

      <node pkg="ego_planner" name="drone_$(arg drone_id)_traj_server" type="traj_server" output="screen">
          <!-- <remap from="position_cmd" to="/setpoints_cmd"/> -->
          <remap from="pose_cmd" to="/mavros/setpoint_position/local"/>
          <remap from="~planning/bspline" to="drone_$(arg drone_id)_planning/bspline"/>
          <param name="traj_server/time_forward" value="1.0" type="double"/>
      </node>

• pkg定义节点所在功能包名称，type定义节点的可执行文件名称，这两个属性等同于rosrun执行时输入的参数，name用来定义节点运行的名称

• 上面片段可以看出运行的是traj_server

traj_server.cpp

1->创建节点 traj_server
2->订阅规划轨迹 planning/bsline
    回调函数 bsplineCallback
    2.1->收到轨迹，创建两个变量pos_pts,knots，分别接受geometry_msgs/Point[]类型的pos_pts和时间变量konts
    2.2->用收到的pos_pts和konts创建新的均匀B样条曲线pos_traj
    2.3->计算后面要用的变量
        start_time_  //msg->start_time
        traj_id_    //msg->traj_id
        traj_    //插入pos_traj及其一二阶倒数
        traj_duration_  //总时间
        receive_traj_ = true
3-> 创建发布者 pos_cmd_pub，往/position_cmd发布PositionCommand类型的数据
4-> 创建的定时器，间隔10ms进入一次回调函数
    回调函数cmdCallback():
    4.1-> 是否收到了receive_traj_ = true，没有就return跳出函数
    4.2-> 计算现在时间和起始时间的间隔t_cur = (time_now - start_time_).toSec();
    4.3-> 判断t_cur在不在总时间区间内
        4.3.1-> t_cur < traj_duration_ && t_cur >= 0.0
            计算当前t_cur的pos，vel,acc,yaw,pos_f
        4.3.2-> t_cur >= traj_duration_
            计算终点pos,vel,acc设0，yaw不变
        4.3.3-> t_cur<0
            报错
    4.4-> 把pos vel acc等信息装入cmd里，并发布pos_cmd_pub.publish(cmd)
5-> 在cmd指令里面设置控制器增益系数
6-> sleep，ros::spin
​
函数calculate_yaw（）
->参数：double t_cur, Eigen::Vector3d &pos, ros::Time &time_now, ros::Time &time_last
->功能：计算yaw角方向，变化率，并对输出进行限幅，把yaw输出限制在[-PI,PI]
->输出：pair of<yaw,yaw_dot>

• 作用：将轨迹规划器发布的轨迹planner/bspline转化为控制器指令cmd并发布到position_cmd话题上（发给飞控）

EGO-Planner轨迹规划器流程

ego_planner_node节点的启动流程
//进入函数主接口ego_planner_node.cpp
1-> 初始化ros节点
2-> 创建ego_planner状态机：rebo_replan
3-> rebo_replan.init(nh);
4-> ros::spin();
-> 跳转到rebo_replan.init(nh);
->
-> ego_replan_fsm.cpp
1-> 初始化参数，当前的路标点、进程状态、收到目标标志位、收到里程计数据标志位、收到pre_agent标志位
2-> 参数服务器载入状态机fsm的参数
    ---------------------------------------------------------------------------------------------
    参数           变量名                               默认值                       取值
    ---------------------------------------------------------------------------------------------
    飞行模式            target_type_                   -1                      1:选点，2：waypoint
    重规划时间阈值       replan_thresh_                 -1.0                     1.0
    停止规划距离        no_replan_thresh_               -1.0                     1.0
    规划视界            planning_horizen_              -1.0                     6.0
    规划时间视界         planning_horizen_time_         -1.0                     3.0
    应急时间            emergency_time_                 1.0                     1.0
    真机运行标志位       flag_realworld_experiment_      false                    true
    使能fail_safe      enable_fail_safe_               true                     true
    路标点数量          waypoint_num                    -1                       >1
    路标点xyz          waypoints_[i][j]                  /                      /
    ----------------------------------------------------------------------------------------------
3-> 初始化主模块：       //可视化模块，规划管理器
    //均为fsm的子类
    //第一个子类visualization_
    -> 初始化可视类指针：visualization_（构造函数）
        -> 创建5个发布者
    //第二个子类planner_manager_
    -> 初始化规划管理器类指针：planner_manager_
        //初始化规划管理器需要的各个模块
        -> planner_manager_->initPlanModules(nh, visualization_);
            -> 从参数服务器初始化规划器参数PlanParameters pp_:
                --------------------------------------------------------------
                参数          变量名                     默认值     取值
                --------------------------------------------------------------
                最大速度            max_vel                     -1.0        0.5
                最大加速度       max_acc                     -1.0        6.0
                最大加加速度      max_jerk                    -1.0        4
                可行公差？       feasibility_tolerance        0.0        0.05
                控制点距离       control_points_distance     -1.0        0.4
                规划视界            planning_horizon            5.0         6
                使用独特轨迹？ use_distinctive_trajs       false       false
                飞机id            drone_id                    -1          0
                ---------------------------------------------------------------
            -> 初始化grid_map    //建图部分
                -> grid_map->initMap(nh)
            -> 初始化bspline_optimizer_ //曲线优化器
                ->  bspline_optimizer->setParam(&nh) //从参数服务器载入优化器的参数
                --------------------------------------------------------------
                参数          变量名             默认值     取值
                --------------------------------------------------------------
                sommth权重        lambdal1_           -1.0        1.0
                safe权重      lambdal2_           -1.0        0.5
                feasible权重  lambdal3_           -1.0        0.1
                fitness权重       lambdal4_           -1.0        1.0
                距离？         dist0_              -1.0        0.5
                集群无碰撞距离 swarm_clearnce_     -1.0        0.5
                最大速度            max_vel_            -1.0        0.5
                最大加速度       max_acc_            -1.0        6
                -------------------------------------------------------------
            -> bspline_optimizer->setEnvironment( grid_map_,obj_predictor_) //配置地图
                //ta也有子类，无线套娃
            -> 初始化a_star_ //a*搜索
            -> bspline_optimizer->a_star_.initGridMap()
            //fsm的子类vis，也是planner_manager_的子类
        -> visulization_ = vis  //值传递
    ->  planner_manager_->deliverTrajToOptimizer()  //存储轨迹 
            //swarm_trajs_buf_的数据是从哪来的，是从父类传来的还是有回调函数来接收集群轨迹数据
            //盲猜后面的回调函数会做多机轨迹的处理
        ->  把成员变量swarm_trajs_buf_传给子类bspline_optimizer的私有成员变量swarm_trajs_
            
            //这种类间传递的方式可以学习一下
    ->  planner_manager_->setDroneIdtoOpt()     //设置飞机id
        ->  把成员变量pp_drone_id传给子类bspline_optimizer的私有成员变量drone_id_
//把需要的子类都设置完了，简单总结一下各个类之间的传递关系
        --------->visualization_
        |
fsm----->                                       -------->grid_map_
        |                                       |                               ------>grid_map_
        --------->planner_manager_------------> |------->bspline_optimizer_---> |   
                                                |                               ------->a_star_                         
                                                ------->visualization_  
4-> 回调函数部分
    -> 创建定时器： exec_timer_，间隔10ms进入一次回调函数execFSMCallback()//状态机
    -> 创建定时器： safety_timer_，间隔50ms进入一次回调函数checkCollisionCallback()
//回调函数部分结束，重要！
-> 创建订阅者odmo_sub_,订阅里程计数据
​
->  if 飞机id大于等于1
    创建订阅者swarm_trajs_sub_，从 /drone_(id-1)_planning/swarm_trajs上订阅集群轨迹       //果然回调函数是在处理多机轨迹并传给了planner_manager_->swarm_trajs_buf_
->  创建发布者swarm_trajs_pub_，将traj_utils::MultiBsplines 类型数据发布到/drone_(id)_planning/swarm_trajs话题上 
->  创建发布者broadcast_bspline_pub_，将traj_utils::Bsplines 类型数据发布到/planning/broadcast_bspline_from_planner话题上    
->  创建订阅者broadcast_bspline_sub_，从/planning/broadcast_bspline_to_planner话题上订阅数据
​
//发布给traj_server的
-> 创建发布者bspline_pub_，将traj_utils::Bsplines 类型数据发布到/planning/bspline话题上
->  创建发布者data_disp_pub_，将traj_utils::DataDisp 类型数据发布到/planning/data_display话题上
​
-> if 飞行模式为手动模式 //rviz选点
    -> 创建订阅者waypoint_sub_，订阅路标点话题/move_base_simple/goal
-> else if 飞行模式为与设模式 //提前设置路标点
    -> 创建订阅者trigger_sub_，订阅话题/traj_start_trigger    //进入悬停状态一段时间，触发！！！（多机的话可以设置一个同步触发机制）
    ->  等待1秒
    ->  等待收到里程计信息，若在真实环境下还需等待触发
    ->  处理路标点 readGivenWps()
        -> 将路标点存入新的变量wps_里
        -> 可视化
        -> 规划第一个路标点
-> else 报错
//初始化结束

• 回调函数流程

execFSMCallback():
-> 关闭定时器exec_timer_.stop(); // 避免堵塞
-> 每秒打印一次当前状态
    -> if 没有odom_数据，报错
    -> if 没有收到目标点，报错
//状态机流程
-> switch(exec_state_)
    //初始状态，在状态机初始化的时候就被设置为INIT
    ->  INIT：
        -> if 没有odom_ 数据，goto force_return跳转到函数末尾
        -> 状态切换为WAIT_TARGET //如果两次跳转的状态相同，累加计数
        -> break
    -> WAIT_TARGET:
        -> if 没有收到目标点或没有收到触发，goto force_return跳转到函数末尾
        -> 状态切换为SEQUENTIAL_START        //如果两次跳转的状态相同，累加计数
        -> break
    ->  SEQUENTIAL_START:   
        //for swarm
        //swarm_trajs_sub_的回调函数末尾会把have_recv_pre_agent_置1   
        ->  if drone_id<=0   或   drone_id>=1并且have_recv_pre_agent_ 
            ->  if 里程计信息、目标点、触发都到位了
                ->  bool success = planFromGloablTraj(10);
                    ->  判断状态是否变化，是 flag = false ,否 flag = true
                    ->  for 循环 10次
                        ->  if(callReboundReplan(true,flag)==1) 
                            ->return true;
                    ->  return false        
                ->  if success：     
                    ->  状态切换为EXEC_TRAJ      //如果两次跳转的状态相同，累加计数
                    ->  publishSwarmTrajs(true)     //根据规划器局部信息发布集群轨迹以及广播轨迹
                        ->  根据规划器局部信息生成bspline曲线
                        ->  if bool ==1
                            //多机模式，要向下一架飞机发布集群轨迹
                            //集群轨迹buf_的数据由swarm_trajs_sub_的回调函数处理，接收的是上架飞机发布的集群数据
                            //记录是哪个飞机发送的
                            ->  buf_.drone_id_from = drone_id
​
                            //例子 ：0发布了2条轨迹
                            ->  if  buf_.traj.size == id+1 ，buf_.traj.back() = bspline                  
                            //例子： 0发布了1条轨迹
                            ->  else if buf_.traj.size == id ，buf_.traj.push_back() = bspline
                            //上个无人机发布的轨迹数量只能等于本机id-1或本机id
                            ->  else ，报错
​
                            ->  swarm_trajs_pub_发布buf_的数据
                        ->  broadcast_bspline_pub_发布bspline     //广播地只是单个轨迹吗？
                ->  else unsuccess：
                    ->  报错
                    ->  状态切换为SEQUENTIAL_START       //如果两次跳转的状态相同，累加计数
            ->  else ，报错                    
        ->  break
    
    -> REPLAN_TRAJ
        -> bool success = planFromCurrentTraj(1)
        -> if success：
            -> 状态切换为EXEC_TRAJ       //如果两次跳转的状态相同，累加计数
            -> publishSwarmTrajs(false)
        -> else unsuccess:
            -> 状态切换为REPLAN_TRAJ     //如果两次跳转的状态相同，累加计数
        -> break
​
​
//执行轨迹
-> EXEC_TRAJ:
    -> 定义局部轨迹指针 *info=&planner_manager->local_data_
    -> 计算当前时间和局部轨迹开始时间之差t_cur，要是t_cur超过局部轨迹的总时间，t_cur不再增加
    -> 计算t_cur时刻局部轨迹期望的位置坐标gps
​
    -> if 预设点模式，且没规划完最后一个点，且当前位置距离end_pt_小于 停止重规划距离no_replan_thresh
        -> 规划下一个路标点
    -> else if局部终点和全局终点距离太近
        -> if 当前时间大于局部轨迹的总时间
            -> 收到目标，收到触发标志位0 //到终点了
            -> if 预设点模式
                -> 返回规划第一个路标点
            -> 状态切换为WAIT_TARGET //如果两次跳转的状态相同，累加计数
            ->goto force_return
        -> else if 当前位置距离end_pt_小于 停止重规划距离no_replan_thresh，且当前时间大于重规划时间replan_thresh_
            -> 切换状态为REPLAN_TRAJ
    -> else if 当前时间t_cur_ > 重规划时间replan_thresh_
        -> 状态切换为REPLAN_TRAJ     //如果两次跳转的状态相同，累加计数
    -> break
​
    -> EMERGENCY_STOP:
        -> if flag_escape_emergency == 1
            -> callEmergecyStop(odmo_pos_)
                -> 根据当前里程计位置生成b样条曲线
                -> bspline_pub_发布生成的b样条曲线
        -> else
            -> if enable_fail_safe_ ==1，且速度为0
                -> 状态切换为GEN_NEW_TRAJ        //如果两次跳转的状态相同，累加计数
            -> flag_escape_emergency = 0
        -> break
//状态机流程部分结束
​
//碰撞检查的回调函数
checkCollisionCallback():
->  定义局部轨迹指针 *info = &planner_manager->local_data_
->  定义地图 map = planner_manager->grid_map_
->  if 当前状态为等待触发或局部轨迹开始时间非常小
    ->  跳出函数
->  if 深度图数据丢失
    ->  报错
    ->  enable_fail_safe_ = 0
    ->  状态切换为EMERGENCY_STOP     //如果两次跳转的状态相同，累加计数
//检查轨迹是否碰撞
->  设置迭代步长，迭代查验当前局部轨迹前2/3是否有碰撞
    ->  如果t时刻位置pos在地图中为占据，occ|=1
    //还要查验和其他轨迹的距离
    ->  如果当前时刻局部轨迹位置和其他轨迹位置的距离dist<swarm_clearance_
        ->  occ = 1
    ->  if occ==1
        ->  bool success = planFromCurrentTraj()    
        ->  if success：
            ->  状态切换为EXEC_TRAJ      //如果两次跳转的状态相同，累加计数
            ->  publishSwarmTrajs(false)
        ->  else unsuccess:
            ->  if t-t_cur < emergency_time_
                ->  报错
                ->  状态切换为EMERGENCY_STOP     //如果两次跳转的状态相同，累加计数
            ->  else
                ->  状态切换为REPLAN_TRAJ        //如果两次跳转的状态相同，累加计数
//碰撞检查的回调函数结束
​
​
​
//状态机部分还是有细节的子函数需要理解，放在下节看吧
//订阅者的回调函数还需要详细阅读，再回头来看状态机会更清晰
planFromGloablTraj()
planFromCurrentTraj()
​

planner/plan_env

参数含义

1. MappingData md_中的参数含义

  //具体占据概率,初始化为-1.99243-0.01=-2.00243(空闲)
  md_.occupancy_buffer_ = 
  vector<double>(buffer_size, mp_.clamp_min_log_ - mp_.unknown_flag_);
  //栅格占据与否,0表示空闲,1表示占据
  md_.occupancy_buffer_inflate_ = vector<char>(buffer_size, 0);
//统计涉及到的体素的次数,占据和空闲均会+1
  md_.count_hit_and_miss_ = vector<short>(buffer_size, 0);
//统计涉及到的占据体素的次数,占据时会+1
  md_.count_hit_ = vector<short>(buffer_size, 0);
  
  md_.flag_rayend_ = vector<char>(buffer_size, -1);
  
  md_.flag_traverse_ = vector<char>(buffer_size, -1);
​
  md_.raycast_num_ = 0;
​
  md_.proj_points_.resize(640 * 480 / mp_.skip_pixel_ / mp_.skip_pixel_);
  
  md_.proj_points_cnt = 0;
  
  md_.cam2body_ << 0.0, 0.0, 1.0, 0.0,
      -1.0, 0.0, 0.0, 0.0,
      0.0, -1.0, 0.0, 0.0,
      0.0, 0.0, 0.0, 1.0;
​

1. MappingParameters mp_中的参数含义

//表示栅格地图的索引（index）范围,mp_.resolution_表示地图分表率，默认值为0.1;ceil向上取整
  for (int i = 0; i < 3; ++i)
  {
    mp_.map_voxel_num_(i) = ceil(mp_.map_size_(i) / mp_.resolution_);
    }
//缓冲区大小（三维索引 ---> 一维向量）
int buffer_size = mp_.map_voxel_num_(0) * mp_.map_voxel_num_(1) * mp_.map_voxel_num_(2);

//表示地图的位置（pos）范围
  mp_.map_min_boundary_ = mp_.map_origin_;
  mp_.map_max_boundary_ = mp_.map_origin_ + mp_.map_size_;

mp_.local_map_margin_//表示局部栅格地图的更新和清除范围,默认值为10(10*0.1=1m)

  mp_.prob_hit_log_ = logit(mp_.p_hit_);//每判定一次占据,自增mp_.prob_hit_log_ 
  mp_.prob_miss_log_ = logit(mp_.p_miss_);//每判定一次空闲,自增mp_.prob_miss_log_ 
  mp_.clamp_min_log_ = logit(mp_.p_min_);//最终能够判定空闲的阈值
  mp_.clamp_max_log_ = logit(mp_.p_max_);//最终能够判定占据的阈值
  mp_.min_occupancy_log_ = logit(mp_.p_occ_);
  mp_.unknown_flag_ = 0.01;
​
  cout << "hit: " << mp_.prob_hit_log_ << endl;
  cout << "miss: " << mp_.prob_miss_log_ << endl;
  cout << "min log: " << mp_.clamp_min_log_ << endl;
  cout << "max: " << mp_.clamp_max_log_ << endl;
  cout << "thresh log: " << mp_.min_occupancy_log_ << endl;
  //终端输出结果
  hit: 0.619039
  miss: -0.619039
  min log: -1.99243
  max: 2.19722
  thresh log: 1.38629

1. grid_map.cpp文件中其他参数的含义

x_size, y_size, z_size//代表设定地图的大小
mp_.map_size_ = Eigen::Vector3d(x_size, y_size, z_size);

//表示地图的原点在地图的左下角，mp_.ground_height_默认值为-0.01
mp_.map_origin_ = Eigen::Vector3d(-x_size / 2.0, -y_size / 2.0, mp_.ground_height_);

函数

depthPoseCallback()

通过message_filter类接受同步后的最新的相机pose与深度图，同时，如果相机的位置处于全局地图Map_size之外，则就会价格md.occ_need_update这一flag置false，反之为true

updateOccupancyCallback()

地图节点通过这一回调函数定时更新地图

其中有两个重要的flag，一个是上文提到的md.occ_need_updata，只有接收到新图像且位于地图范围之内，才会进行接下来的projectDepthImage()以及raycastProcess()这两个流程

同样，另外一个flag是md.local_updated。这一flag只在raycastProcess中判断深度投影点数量不为0时才会设置为true，这时才会进入clearAndInflateLocalMap()这一流程对局部地图进行更新和膨胀

projectDepthImage()

通过相机投影模型，将图像坐标上的点投影到相机坐标系，再通过相机的位姿将相机坐标系上点投影至世界坐标系，最后将所有点存入md.proj_points_这一vector容器中

raycastProcess()

这一函数会对md.proj_points中的每一个点进行raycast流程。首先判断每一个点是否超出地图范围，是否超出ray_length，如果超出，就将此点重新赋值为一个允许的射线上最远的点。

如果重新赋值，则利用setCacheOccupancy（）这一函数将md.count_hit_and_miss这一容器对应序列上的计数+1次。表示这一free空间被经过了一次。如果这一点是第一次被遍历，则应该把它加入到md_.cache_voxel这一容器中区

如果不需要重新赋值，说明当前点是障碍物，则利用setCacheOccupancy将这一点在md.count_hit容器中对应序列的位置计数+1。需要说明的是，不管当前点是不是障碍物，md.count_hit_and_miss_容器对应位置处的计数都会被+1.

当终点被setCache过后，就进入raycast环节，通过raycast.step函数从射线终点开始向相机点步进。并且将每一个中途点都利用setCacheOccupancy函数置一次free。需要注意的是，每一个中途点还利用md.flag_traverse容器进行了判断，如果对应序列处的值不是本轮raycast的num,则将其置为b本轮的racastnum.否则说明这一点及之后的点都已经被raycast过了，因此跳出当前射线终点的raycast循环。

当完成md.proj_points容器中所有点的raycast循环后，开始对md.cache_voxel中的点进行循环判断。首先根据md.count_hit及md.count_hit_and_miss中对应位置的值判断当前voxel为障碍物的概率。并且如果当前点的log_odds_update是prob_hit_log，且md.occupancy_buffer中对应位置的概率值还没有超过最大值或当前点的log_odds_update是prob_miss_log，且md.occupancy_buffer中对应位置的概率值还没有低于最小值。且当前点是在局部地图范围内，则更新md.occupancy_buffer_中的概率值。

clearAndInflateMap()

这一函数首先将局部范围外一圈的点的occupancy_buffer对应值置为：mp.clamp_min_log - mp.unknown_flag。

然后将局部地图范围内的地图上一轮的occupancy_buffer_inflate值全部置为0；

紧接着，对局部地图的occupancy_buffer中所有点的值进行一一判断，判断是否超过为障碍物的最低概率mp.min_occupancy_log，如若判断，就对该点进行膨胀，并将所有膨胀点的occupancy_buffer_inflate值全部置为1；

publishmap() 和 publishInflateMap()

这两函数分别对occupancy_buffer及occupancy_buffer_inflate容器中局部地图范围内的所有点进行判断，若值分别超过障碍物最小概率及为1，且不超过高度范围，则将其从voxel序列还原成三维位置点，推入cloud容器中，最后一并发布。

【代码学习笔记】ego-planner代码学习笔记_egoplanner代码解析-CSDN博客v100pc_search_result_base5&spm=1018.2226.3001.4187

本文标签： 代码EGOPlanner