ego_swarm代码框架

Xuan-ZY

已于 2023-12-25 22:16:06 修改

阅读量526

点赞数 1

分类专栏： ego 文章标签：笔记数学建模学习

于 2023-10-25 10:21:09 首次发布

本文链接：https://blog.csdn.net/yunddun/article/details/134027404

版权

ego 专栏收录该内容

2 篇文章

订阅专栏

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

轨迹数据流向

Bspline解析

（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;