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

本文链接：https://blog.youkuaiyun.com/vmt/article/details/134988859

本文介绍了使用行为树控制nav2驱动的小车导航项目，包括C++类架构设计、类交互方式、状态获取与传递、行为树结构及初始化子节点等。主要涉及自动驾驶、ROS2和Nav2的集成应用。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

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

记录工作中使用行为树来控制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交互
C++程序与nav2交互

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();
}