Nav2hunter的控制软件结构设计(持续更新中。。。。)

（为免误导，特免责声明如下：本文所有内容，只是基于个人当前理解和实际做法，后面亦会有更正和修订，但任何版本都不免有个人能力有限、理解有误或者工作环境不同的状况，故文中内容仅供参考。任何人都可以借鉴或者直接使用代码片段，但对任何直接引用或者借鉴产生的技术问题等后果，作者不承担任何责任。）

记录工作中使用行为树来控制nav2驱动的小车导航项目的进展，随着工作推进不断更新中，欢迎留言指导和讨论。。。

1. 类架构

• 一个CNavMap类（继承自QWidget）对象，处理地图、位置、坐标和在UI上的显示等
• 一个rclcomm类（继承自QThread）对象，来处理ROS2的通讯，内有一个ROS2节点，在线程的运行（run）里面，spin该node
• 一个BT_Thread类（继承自QThread）对象，来运行行为树处理控制逻辑，在线程的运行（run）里面，tick行为树
• MainWindow（继承自QMainWindow）对象是主界面，处理UI交互,CNavMap类的对象嵌入在主界面里（用提升）

2. 类交互

• 2.1 BT_Thread构建函数会传入一个字符串（xml文件位置名称），一个指向rclcomm类对象的指针，用来与ROS2交互。
• 2.2 在BT_Thread类里面，会把系统状态打包到一个结构体StateVar，便于传入树节点，以及向外发送状态变化。
• 2.3 StateVar结构体内有个互斥锁（QMutex），以确保线程间操作的安全。（好像不用，只有BT_Thread内会操作它，还是要用，因为BT_Thread响应消息要放在主线程，否则BT_Thread的run里面需要调用exec（）并在里面循环处理消息和槽，不出来，这样就没法运行tick）
• 2.4 交互方法
  • 2.4.1 BT_Thread里调用rclcomm向ROS发信息：用指针直接调用
  • 2.4.2 BT_Thread读取rclcomm的变量：用指针直接调用
  • 2.4.3 rclcomm向BT_Thread反馈状态变化：用emit消息 到对方的接收槽
  • 2.4.4 BT_Thread向MainWindow发送显示信息，用emit消息到对方的接收槽
  • 2.4.5 MainWindow给BT_Thread发送命令，直接使用指针调用。
• 2.5 BT_Thread（固定每秒一次）向MainWindow发送Signal 参数；

#define BT_ACTIVECMD_None       0
#define BT_ACTIVECMD_2Goal      1
#define BT_ACTIVECMD_2BASE      2
#define BT_ACTIVECMD_Reset      3
#define BT_ACTIVECMD_Abort      4
enum BT_NAV2_STAGE{
   STAGE_NA=0,
   STAGE_SETGOAL,
   STAGE_GOALACCEPTED,
   STAGE_MOVING,
   STAGE_FAILED,
   STAGE_REACHED,
   STAGE_ABORT
};
struct StateVar{
   uint iCounter_HunterAlive;       /*CAN Bus message alive signal counter*/
   bool IsHunter_Connected;         /*False While iCounter_HunterAlive is 0*/
   bool IsHunter_PowerSufficient;   /*Is Voltage of Hunter not less than 25.5V*/
   bool IsHunter_LineCmdMode;       /*Is Hunter not control by remote controller*/
   bool IsNav2_MapLoaded;           /*Is Nav2 load the correct map*/
   bool IsNav2_Localised;           /*Is Hunter fetched its position correctly*/
   bool IsNav2_NavServiceReady;     /*Is the nav2 nav2pos service active*/
   uint iCounter_IMUAlive;          /*IMU offline counter*/
   bool IsIMU_Connected;            /*False While iCounter_IMUAlive is 0*/
   int _iState;                     /*System state*/
   int _CoutNo;                     /*Record Last print inform to avoid print same msg times */
   int _ActiveCmd;                 /*0:no command | 1: nav 2 _strGoal | 2:nav2 Base | 3:Reset,switch to Initialise*/
   float _fGoalx;                  /*coordinate of Goal*/
   float _fGoaly;
   float _fGoala;
   BT_NAV2_STAGE _navStage;           /*Record the current stage of navigate to pose */
   std::string _strGoal;            /*Next Goal label*/
   std::string _strInfo;            /*introduce of current situation*/  
   QMutex _mutexlocker;             /*Mutex locker for thread safety*/
};
 StateVar _BT_SystemState;
signals:
   void BT_StateChange(StateVar& sysVar);

• 2.6 rclcomm（在有变化的时刻）向BT_Thread反馈状态变化参数：

 void emitRCLCOMM_MSG(QVariantMap qvMsg); /*something need to tell BT_Thread, Key:"MsgCode" Value="IMU_Online","Hunter_Online"*/
 /*e.g at callback of IMU msg*/
    QVariantMap qvMsg;
    qvMsg["MsgCode"]="IMU_Online";
    emit emitRCLCOMM_MSG(qvMsg); /*Tell BT_Thread IMU online*/

3. 获取小车、Nav2、IMU、雷达等各种状态发给BT_Thread（参见2.6）

• 3.1 IMU在线状态：rclcomm订阅IMU的topic，收到的消息的回调里面通过emit signal告诉BT_Thread IMU 在线
• 3.2 hunter状态：rclcomm里面的HunterDriver与小车进行CAN Bus沟通，rclcomm定时读取状态（在线、电压、控制模式等）过emit signal告诉BT_Thread。
• 3.3 Nav2的状态：Navigation 是否active（参见rviz nav2 panel里面的显示）

// The client used to control the nav2 stack
  std::shared_ptr<nav2_lifecycle_manager::LifecycleManagerClient> client_nav_;
  std::shared_ptr<nav2_lifecycle_manager::LifecycleManagerClient> client_loc_;

ros2 service call /lifecycle_manager_navigation/is_active std_srvs/srv/Trigger “{timeout:3}”

• 3.4 Nav2的状态：小车位置是否已定位可获取
  因为原来rclcomm里已有获取位置的功能，直接使用.

• 3.5 Nav2的状态：map的加载情况

• 3.6 Nav2的反馈：导航过程中的反馈和结果

  &有一天另写一个博客，记录如何在C++程序中与nav2交互

https://blog.csdn.net/vmt/article/details/135184832?csdn_share_tail=%7B%22type%22%3A%22blog%22%2C%22rType%22%3A%22article%22%2C%22rId%22%3A%22135184832%22%2C%22source%22%3A%22vmt%22%7D

4. 如何将外部信息传递给行为树节点

参见行为树BehaviorTree学习记录6_传递额外的参数到节点
太烦了，官方示例用的是接口类，这样可以把参数放到类私有变量里面，但我决定还是用全局变量，

//头文件中：
/*Global Point to BT_Thread*/
class BT_Thread;
extern BT_Thread* g_pBT_Thread;
---------

//构造函数中：
BT_Thread* g_pBT_Thread;
BT_Thread::BT_Thread(std::string strXMLfile,rclcomm *prclcomm,CNavMap* pNavMap)
    : _xmlfilename(strXMLfile),
    _pRclComm(prclcomm),
    _pNavMap(pNavMap)
{
...
    g_pBT_Thread=this; /*全敞开，大家开心调用吧*/
    this->start();
}

5. 主行为树结构

系统的有限状态机设计

变成行为树后的结构图：
将OntheWay分裂成两个状态：OntheWayHome和OntheWay2Station，系统行为树的主层为下图：

6 初始化子节点

初始化需要检测小车是否在线、检测车子的电池电压是否足够、检测车子是否可以线控、检测IMU是否在线、加载需要的地图，检测Nav2地图是否加载、向Nav2发布初始位置、检测是否定位成功、检测Navigation服务是否active。

7 AtHome子节点

只是等待系统命令（是否要检测小车、IMU和Nav2的状态再考虑），如果收到命令则切换到OntheWay2Staion状态，行为树表现为返回成功，没收到命令就返回running。

原来使用的simplenode的tick代码不行，提示简单节点不能进行异步操作（即不能返回running），需要使用一个从StatefulActionNode继承的类，为了复用，使用一个端口来输入需要等待的命令

class asycWaitingNode : public BT::StatefulActionNode
{
public:
    asycWaitingNode(const std::string& name, const BT::NodeConfig& config)
        : BT::StatefulActionNode(name, config) {}

    static BT::PortsList providedPorts()
    {
        return{ BT::InputPort<std::int32_t>("WaitingCmdCode")};
    }

    NodeStatus onStart() override
    {

        BT::Expected<std::int32_t> msg =getInput<std::int32_t>("WaitingCmdCode");
        // Check if expected is valid. If not, throw its error
        if (!msg)
        {
            throw BT::RuntimeError("missing required input port:WaitingCmdCode ",
                                   msg.error() );
            return NodeStatus::FAILURE;
        }
        _wCmd=msg.value();
        std::cout << "[Got input WaitingCmdCode:]"<<msg.value()<<std::endl;
        return NodeStatus::RUNNING;
    }

    /// method invoked by an action in the RUNNING state.
    NodeStatus onRunning() override
    {
        if(g_pBT_Thread->_BT_SystemState._ActiveCmd!=_wCmd)
        {
            if(g_pBT_Thread->_BT_SystemState._CoutNo!=21){
                g_pBT_Thread->_BT_SystemState._CoutNo=21;
                std::cout << "[ Keep waiting command:" <<_wCmd<<"]"<< std::endl;
            }
            return NodeStatus::RUNNING;
        }else {
            if(g_pBT_Thread->_BT_SystemState._strGoal==""){
                if(g_pBT_Thread->_BT_SystemState._CoutNo!=22){
                    g_pBT_Thread->_BT_SystemState._CoutNo=22;
                    std::cout << "[Err: Recieved command: "<<_wCmd<<" but Goal is blank]" << std::endl;
                }
                g_pBT_Thread->_BT_SystemState._ActiveCmd=BT_ACTIVECMD_None; //Clear the current command
                return NodeStatus::FAILURE;
            }
            CNavPoint* pt;
            if(g_pBT_Thread->_pNavMap->SearchPointbyLabel(QString::fromStdString(g_pBT_Thread->_BT_SystemState._strGoal),&pt)){
                g_pBT_Thread->_BT_SystemState._fGoalx=pt->m_fx;
                g_pBT_Thread->_BT_SystemState._fGoaly=pt->m_fy;
                g_pBT_Thread->_BT_SystemState._fGoala=pt->m_fAngle;
                if(g_pBT_Thread->_BT_SystemState._CoutNo!=23){
                    g_pBT_Thread->_BT_SystemState._CoutNo=23;
                    std::cout << "[ Recieved Command&Found goal, swtiching to OntheWay2: " <<g_pBT_Thread->_BT_SystemState._strGoal<< "]" << std::endl;
                }
                g_pBT_Thread->_BT_SystemState._ActiveCmd=BT_ACTIVECMD_None; //Clear the current command
                return NodeStatus::SUCCESS;
            } else{
                if(g_pBT_Thread->_BT_SystemState._CoutNo!=24){
                    g_pBT_Thread->_BT_SystemState._CoutNo=24;
                    std::cout << "[Err: Recieved command but no point named:" <<g_pBT_Thread->_BT_SystemState._strGoal <<"in the map]"<<std::endl;
                }
                g_pBT_Thread->_BT_SystemState._ActiveCmd=BT_ACTIVECMD_None; //Clear the current command
                //            g_pBT_Thread->_BT_SystemState._strGoal="";                  //Clear the Goal
                return NodeStatus::FAILURE;
            }
        }

    }
    void onHalted() override
    {
        std::cout << "asycWaitingNode interrupted" << std::endl;
    }

private:
    int _wCmd;
};

命令_ActiveCmd和目标_strGoal都等外部发送，可能是UI发来的（调试手动控制到某点），或者ROS节点发来（集成后）的，由rclcomm收到后emit signal过来。

8 AtChargingPose子节点

与AtHome子节点类似，不复述。

9 OnWay2Station子节点

• 先计算目标前面（约2米）的预备点，把到达精度设置低一点（端口传入），定为目标点 :BT_SA_SetPreGoal
• 导航到目标（预备）点 : asycNav2PoseNode
• 设置真正的goal为目标点，把到达精度调高到满足要求: BT_SA_SetRealGoal
• 导航到目标点: asycNav2PoseNode

NodeStatus BT_HunterInterface::BT_SA_SetPreGoal(BT::TreeNode& btNode)
{
    /*Read goal_checker tolerances from input port*/
        float xy_tol,yaw_tol,preDis;
        Expected<std::float_t> portVal =btNode.getInput<std::float_t>("xy_goal_tolerance");
        if (!portVal)
        {
            throw BT::RuntimeError("missing required input [xy_goal_tolerance]: ",
                                   portVal.error() );
            return NodeStatus::FAILURE;
        }
        xy_tol=portVal.value();
        portVal =btNode.getInput<std::float_t>("yaw_goal_tolerance");
        if (!portVal)
        {
            throw BT::RuntimeError("missing required input [yaw_goal_tolerance]: ",
                                   portVal.error() );
            return NodeStatus::FAILURE;
        }
        yaw_tol=portVal.value();
        portVal =btNode.getInput<std::float_t>("PreAim_dist");
        if (!portVal)
        {
            throw BT::RuntimeError("missing required input [yaw_goal_tolerance]: ",
                                   portVal.error() );
            return NodeStatus::FAILURE;
        }
        preDis=portVal.value();
        std::cout << "BT->[Get input xy_tol:"<<xy_tol<<", yaw_tol:"<<yaw_tol<<", preDistance:"<<preDis<<" ]"<<std::endl;

    /*Set Nav2 Parameter*/
        if(!g_pBT_Thread->_pRclComm->setGoalCheckTol(xy_tol,yaw_tol))
        {
//            return NodeStatus::FAILURE;
        }

    /*calculate PrePosition coordinate*/
        float prePos_x,prePos_y;
        prePos_x=g_pBT_Thread->_BT_SystemState._fGoalx-preDis*cos(g_pBT_Thread->_BT_SystemState._fGoala*M_PI/180.0);
        prePos_y=g_pBT_Thread->_BT_SystemState._fGoaly-preDis*sin(g_pBT_Thread->_BT_SystemState._fGoala*M_PI/180.0);

    /*write the PrePosition to blackboard through output port*/
        btNode.setOutput<std::float_t>("goal_x",prePos_x);
        btNode.setOutput<std::float_t>("goal_y",prePos_y);
        btNode.setOutput<std::float_t>("goal_angle",g_pBT_Thread->_BT_SystemState._fGoala);
        std::cout << "BT->[Set PrePosition as the goal x:"<<prePos_x<<", y:"<<prePos_y<<", yaw:"<<g_pBT_Thread->_BT_SystemState._fGoala<<" ]"<<std::endl;

    return NodeStatus::SUCCESS;
}

异步Nav2Pose节点

class asycNav2PoseNode : public BT::StatefulActionNode
{
public:
    asycNav2PoseNode(const std::string& name, const BT::NodeConfig& config)
        : BT::StatefulActionNode(name, config) {}

    static BT::PortsList providedPorts()
    {
        return{ BT::InputPort<std::float_t>("Pose_x"),
                BT::InputPort<std::float_t>("Pose_y"),
                BT::InputPort<std::float_t>("Pose_yaw")};
    }

    NodeStatus onStart() override
    {
        /*Get Goal from blackboard by input port*/
        BT::Expected<std::float_t> portVal =getInput<std::float_t>("Pose_x");
        // Check if expected is valid. If not, throw its error
        if (!portVal)
        {
            throw BT::RuntimeError("missing required input port:Pose_x ",
                                   portVal.error() );
            return NodeStatus::FAILURE;
        }
        _fGoal_x=portVal.value();
        portVal =getInput<std::float_t>("Pose_y");
        // Check if expected is valid. If not, throw its error
        if (!portVal)
        {
            throw BT::RuntimeError("missing required input port:Pose_y ",
                                   portVal.error() );
            return NodeStatus::FAILURE;
        }
        _fGoal_y=portVal.value();
        portVal =getInput<std::float_t>("Pose_yaw");
        // Check if expected is valid. If not, throw its error
        if (!portVal)
        {
            throw BT::RuntimeError("missing required input port:Pose_yaw ",
                                   portVal.error() );
            return NodeStatus::FAILURE;
        }
        _fGoal_yaw=portVal.value();

        std::cout << "BT->{"<<name()<<"}"<<"[Got input Nav2 Gaol pose_x:"<<_fGoal_x<<", pose_y:"<<_fGoal_y<<", pose_yaw:"<<_fGoal_yaw<<"]"<<std::endl;
        /*Send to Nav2 an asyc command , Then waitting the feedback and result*/
        g_pBT_Thread->_pRclComm->Nav2apt(QString::fromStdString(g_pBT_Thread->_BT_SystemState._strGoal),_fGoal_x,_fGoal_y,_fGoal_yaw);
        g_pBT_Thread->_pNavMap->setGoalPt(_fGoal_x,_fGoal_y,_fGoal_yaw);
        g_pBT_Thread->_BT_SystemState._navStage=BT_NAV2_STAGE::STAGE_SETGOAL;
        return NodeStatus::RUNNING;
    }

    /// method invoked by an action in the RUNNING state.
    NodeStatus onRunning() override
    {
        switch (g_pBT_Thread->_BT_SystemState._navStage)
        {
        case BT_NAV2_STAGE::STAGE_MOVING:
            if(g_pBT_Thread->_BT_SystemState._CoutNo!=81){
                g_pBT_Thread->_BT_SystemState._CoutNo=81;
                std::cout << "BT->{"<<name()<<"}"<<"[ Nav2 Msg: Navigating to goal...]"<< std::endl;
            }
            return NodeStatus::RUNNING;

        case BT_NAV2_STAGE::STAGE_SETGOAL:
            if(g_pBT_Thread->_BT_SystemState._CoutNo!=82){
                g_pBT_Thread->_BT_SystemState._CoutNo=82;
                std::cout << "BT->{"<<name()<<"}"<<"[ Nav2 Msg: Goal Set!]" << std::endl;
            }
            return NodeStatus::RUNNING;

        case BT_NAV2_STAGE::STAGE_GOALACCEPTED:
            if(g_pBT_Thread->_BT_SystemState._CoutNo!=83){
                g_pBT_Thread->_BT_SystemState._CoutNo=83;
                std::cout << "BT->{"<<name()<<"}"<<"[ Nav2 Msg: Goal Accepted by Nav2!]" << std::endl;
            }
            return NodeStatus::RUNNING;

        case BT_NAV2_STAGE::STAGE_REACHED:
            if(g_pBT_Thread->_BT_SystemState._CoutNo!=84){
                g_pBT_Thread->_BT_SystemState._CoutNo=84;
                std::cout << "BT->{"<<name()<<"}"<<"[ Nav2 Msg: Goal Reached!!!]" << std::endl;
            }
            return NodeStatus::SUCCESS;

        default:
            if(g_pBT_Thread->_BT_SystemState._CoutNo!=85){
                g_pBT_Thread->_BT_SystemState._CoutNo=85;
                std::cout << "BT->{"<<name()<<"}"<<"[ Nav2 Error: Mission Failed]"<<std::endl;
            }
            return NodeStatus::FAILURE;
        }

    }
    void onHalted() override
    {
        /*Notify Nav2 to abort the ongoing navigation*/
        g_pBT_Thread->_pRclComm->Nav2_QuitTask();
        std::cout << "asycNav2PoseNode interrupted" << std::endl;
    }

private:
    float _fGoal_x;
    float _fGoal_y;
    float _fGoal_yaw;
};

N. 在Groot里（实时）查看Nav2的BehaviorTree

Okay, forget it, no budget for it at present.

So far so good. Last edit @ 24/12/2023 HK