admin管理员组文章数量:1622632
EGO_Planner代码学习(四):轨迹规划器流程——上
- EGO-Planner轨迹规划器流程
之前学习了EGO-Planner启动流程、轨迹服务器流程、以及控制器流程,已经给继续学习EGO-Planner规划器打好了基础,下面就详细的记录一下规划器部分的流程以及细节。由于整个的篇幅较长,本节先写一下规划器总体的流程,详细的轨迹生成的细节后文再进行补充。
EGO-Planner轨迹规划器流程
和前几节一样,在学习轨迹规划器的流程之前,要对规划器的功能有一个大概的了解,轨迹规划器在src/planner/planner_manger
路径下,是规划器的核心代码,src/planner
路径下的其他功能包都是为其服务的。话不多说,直接看伪代码:
ego_planner_node结点的启动流程
//进入函数主接口ego_planner_node.cpp
1-> 初始化ros结点
2-> 创建ego_planner状态机:repo_replan
3-> repo_replan_init(nh) //代码的主要进程
4-> ros::spin()
-> 跳转到repo_replan_init(nh)
1-> 初始化参数,当前的路标点、进程状态、收到目标标志位、收到里程计数据标志位、收到pre_agent标志位
2-> 从参数服务器载入状态机fsm的参数:
----------------------------------------------------------------------------------------------
参数 变量名 默认值 取值(single_run_in_exp\advanced_param_exp)
----------------------------------------------------------------------------------------------
飞行模式 target_type_ -1 1or2(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_->initPlanModoles(&nh,vis)
-> 从参数服务器初始化规划器参数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(&np)
-> 初始化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秒
-> 等待收到里程计信息,若在真实环境下还需等待触发
-> 处理路标点
-> 将路标点存入新的变量wps_里
-> 可视化
-> 规划第一个路标点
-> else 报错
//初始化到这就结束了
//回调函数流程
execFSMCallback():
-> 关闭定时器exec_timer_ //避免堵塞
-> 每秒打印一次当前状态
-> if 没有odmo_数据,报错
-> if 没有收到目标点,报错
//经典状态机(流程),再推一次,痛并快乐着
-> switch(exec_state_)
//初始状态,在状态机初始化的时候就被设置为INIT
-> INIT:
-> if 没有odmo_数据,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
-> GEN_NEW_TRAJ:
-> bool success = planFromGloablTraj(10)
-> if success:
-> 状态切换为EXEC_TRAJ //如果两次跳转的状态相同,累加计数
-> flag_escape_emergency = 1
-> publishSwarmTrajs(false)
-> else unsuccess:
-> 状态切换为GEN_NEW_TRAJ //如果两次跳转的状态相同,累加计数
-> 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_ //数据来自哪呢?planner_manager有一个专门的函数更新local_data_(updateTrajInfo)
-> 计算当前时间和局部轨迹开始时间之差t_cur,要是t_cur超过局部轨迹的总时间,t_cur不再增加
-> 计算t_cur时刻局部轨迹期望的位置坐标 pos
-> if 预设点模式,且没规划完最后一个点,且当前位置距离end_pt_小于 停止重规划距离no_replan_thresh
-> 规划下一个路标点
-> else if 局部终点和全局终点距离太近
-> if 当前时间大于局部轨迹的总时间
-> 收到目标、收到触发标志位置零 //到终点了呗
-> 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
//状态机流程部分结束!!!
-> data_disp_pub_发布可视化数据
//状态机的回调函数流程结束
//碰撞检查的回调函数
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()
本文标签: 轨迹流程代码EGOPlanner
版权声明:本文标题:EGO_Planner代码学习(四):轨迹规划器流程——上 内容由热心网友自发贡献,该文观点仅代表作者本人, 转载请联系作者并注明出处:https://m.elefans.com/dianzi/1728870799a1177253.html, 本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如发现本站有涉嫌抄袭侵权/违法违规的内容,一经查实,本站将立刻删除。
发表评论