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