今天测试提了一个BUG, ROS节点切换masterUri时,无法订阅到数据,经过排查发现是因为在释放ros::NodeHandle包装类qnode对象时,使用的是deleteLater而不是delete,deleteLater延迟析构,会导致新的qnode初始化成功后,之前的deleteLater触发之前的qnode对象的析构函数,而析构函数中调用了ros::shutdown(),又把ros节点给关闭了,ros订阅没有抛异常也没报错
QNode::~QNode() {
m_check_master = false;
if(ros::isStarted()) {
ros::shutdown(); // explicitly needed since we use ros::start();
ros::waitForShutdown();
}
wait();
qInfo() <<"Destroy ros node successfully.";
}
bool QNode::init(const std::string &master_url, const std::string &host_url) {
std::map<std::string,std::string> remappings;
remappings["__master"] = master_url;
remappings["__hostname"] = host_url;
qInfo() << "QNode::init, masterUrl=" << master_url.c_str() << ",host_url=" << host_url.c_str()
<<"topic info=" << SystemConfigImpl->topics();
ros::init(remappings,"demokits_tool",ros::init_options::AnonymousName);
//ros::init方法在一个程序运行期间只初始化依次,如果想改变master的地址通过ros::master::init方法
ros::master::init(remappings);
ros::master::setRetryTimeout(ros::WallDuration(3));// loop connection time out interval
if ( ! ros::master::check() ) {
qWarning() << "Ros master check failed, ros init failed.";
return false;
}
m_master_ok = true;
QtConcurrent::run(this,&QNode::startMasterCheck);
qDebug() << "QNode::init, ros::isStarted()=" << ros::isStarted() << ",ros::isOk()=" << ros::ok()
<<",ros::isShuttingDown()=" << ros::isShuttingDown();
ros::start(); // explicitly needed since our nodehandle is going out of scope.
ros::NodeHandle n;
start();
qInfo() << "Ros node initialized succesfully.";
return true;
}
void QNode::run() {
ros::Rate loop_rate(30);
int count = 0;
if(ros::ok())
{
//ros::spinOnce();
//loop_rate.sleep();
ros::MultiThreadedSpinner s(2);
ros::spin(s);
}
qInfo() << "Ros shutdown.";
Q_EMIT rosShutdown(); // used to signal the gui for a shutdown (useful to roslaunch)
}
另外当ros初始化之后,如何切换masterUri的节点可以通过调用ros::master::init方法来实现,ros::master::init没有在头文件中定义,需要手动添加函数声明,如下
namespace ros
{
namespace master
{
void init(const M_string& remappings);
}
}