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

本文介绍了使用行为树控制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();
}

5. 主行为树结构

系统的有限状态机设计
在这里插入图片描述
变成行为树后的结构图:
将OntheWay分裂成两个状态:OntheWayHome和OntheWay2Station,系统行为树的主层为下图:
在这里插入图片描述

6 初始化子节点

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

7 AtHome子节点

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

评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包

打赏作者

code .

你的鼓励将是我创作的最大动力

¥1 ¥2 ¥4 ¥6 ¥10 ¥20
扫码支付:¥1
获取中
扫码支付

您的余额不足,请更换扫码支付或充值

打赏作者

实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值