(为免误导,特免责声明如下:本文所有内容,只是基于个人当前理解和实际做法,后面亦会有更正和修订,但任何版本都不免有个人能力有限、理解有误或者工作环境不同的状况,故文中内容仅供参考。任何人都可以借鉴或者直接使用代码片段,但对任何直接引用或者借鉴产生的技术问题等后果,作者不承担任何责任。)
记录工作中使用行为树来控制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。