plannnerV1
ego_plannner_node:main函数初始化
init
rebo_replan.init(nh);
ego_replan_fsm(重规划状态机)
将launch文件的参数引入
订阅状态机回调函数+碰撞检测回调函数
判断模式是手动设置目标模式或者waypoint模式
后者进入读取waypoint点函数
在该函数中先进行全局轨迹规划
然后将状态从WAIT_TARGET切换到GEN_NEW_TRAJ(生成新轨迹)模式
循环等待EXEC_TRAJ模式
如果等待到了EXEC_TRAJ模式,状态转换为REPLAN_TRAJ模式
状态机回调函数
在GEN_NEW_TRAJ状态中进行全局轨迹规划
然后将状态转换成EXEC_TRAJ模式
在EXEC_TRAJ模式中检测是否需要replan
如果满足条件则进入REPLAN_TRAJ模式
在REPLAN_TRAJ模式中一直调用planFromCurrentTraj函数进行rebounds
直到成功,将状态转移至EXEC_TRAJ
轨迹数据流向
- (ego-replan-fsm.cpp中)callReboundReplan函数下调用reboundReplan进行轨迹重优化,最终将轨迹发布到planning/bspline话题中
- planning/bspline== 经过traj-server处理过后=/position_cmd
- /position_cmd == cmd == cmd_data == 在get_cmd_des调用cmd_data
- get_cmd_des ==des=经过calculateControl=debug_msg和输出u
最终进入mavros控制
-
debug_msg=通过debug_pub=发布到/debugPx4ctrl上面 ===可能用于调试
-
u通过publish_attitude_ctrl和publish_bodyrate_ctrl函数发布到(“/mavros/setpoint_raw/attitude”,话题上面进行姿态控制,成功控制无人机
px4ctrl控制逻辑
// 1
if (last_mode < API_MODE_THRESHOLD_VALUE && mode > API_MODE_THRESHOLD_VALUE)
enter_hover_mode = true;
else
enter_hover_mode = false;
if (mode > API_MODE_THRESHOLD_VALUE)
is_hover_mode = true;
else
is_hover_mode = false;
// 2
if (is_hover_mode)
{
if (last_gear < GEAR_SHIFT_VALUE && gear > GEAR_SHIFT_VALUE)
enter_command_mode = true;
else if (gear < GEAR_SHIFT_VALUE)
enter_command_mode = false;
if (gear > GEAR_SHIFT_VALUE)
is_command_mode = true;
else
is_command_mode = false;
}
// 3
if (!is_hover_mode && !is_command_mode)
{
if (last_reboot_cmd < REBOOT_THRESHOLD_VALUE && reboot_cmd > REBOOT_THRESHOLD_VALUE)
toggle_reboot = true;
else
toggle_reboot = false;
}
else
toggle_reboot = false;
channel5
mode
mode变大 enter_hover_mode=1
mode>0.75 is_hover_mode=1
channel6
gear
gear 变大 enter_command_mode =1
gear >0.75 is_command_mode =1
channel8
reboot_cmd
reboot_cmd 变大 toggle_reboot
ch5上拉,L1=L2
6上拉 L2=L3