概述
-
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
参数含义
-
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;
-
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
-
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
扫一扫
1153
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
