Best practices to use subscriber inside a class
I am trying to create a wrapper class. A subscriber inside this wrapper class is supposed to subscribe to a point cloud rostopic and store the received point cloud in a class variable. A method inside this wrapper class named getRecentData()
is supposed to return this class variable, i.e., point cloud data.
See the wrapper class below-
void PointCloudDataProvider::pointCloudCallback(const boost::shared_ptr<const sensor_msgs::PointCloud2>& msg)
{
pcl::PCLPointCloud2 pcl_pc2;
pcl_conversions::toPCL(*msg, pcl_pc2);
pcl::fromPCLPointCloud2(pcl_pc2, *cloud);
}
void PointCloudDataProvider::init(std::string topic_name, int queue_size, PointCloudDataProvider point_cloud_data_provider)
{
ros::NodeHandle node_handle;
node_handle.subscribe(topic_name, queue_size, &PointCloudDataProvider::pointCloudCallback, &point_cloud_data_provider);
}
pcl::PointCloud<pcl::PointXYZRGB>::Ptr PointCloudDataProvider::getRecentData()
{
return cloud;
}
// This doesn't work hence commented
/*
PointCloudDataProvider::PointCloudDataProvider(std::string topic_name, int queue_size)
{
ros::NodeHandle node_handle;
node_handle.subscribe(topic_name, queue_size, &PointCloudDataProvider::point_cloudCallback);
}
*/
// this is defined inside header file
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
Below is the test class-
int main(int argc, char** argv)
{
ros::init(argc, argv, "point_cloud_data_provider_test");
std::string topic = "/point_cloud/points";
int size = 1;
PointCloudDataProvider data_provider;
data_provider.init(topic, size, data_provider);
ros::Rate loop_rate(1);
while(1)
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = data_provider.getRecentData();
// do something with point cloud here
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}
The above code doesn't work and shows following error-
/usr/include/boost/smart_ptr/shared_ptr.hpp:646: typename boost::detail::sp_dereference<T>::type boost::shared_ptr<T>::operator*() const [with T = pcl::PointCloud<pcl::PointXYZRGB>; typename boost::detail::sp_dereference<T>::type = pcl::PointCloud<pcl::PointXYZRGB>&]: Assertion `px != 0' failed.
Below are the reasons for creating the wrapper class-
- The point cloud data is required by another method hence the processing cannot be done inside
pointCloudCallback
function - Use multiple instances of
PointCloudDataProvider
with differentrostopic
. - Access point cloud data from multiple instances of
PointCloudDataProvider
by usinggetRecentData()
in almost same timestamp
Please note that point cloud data is not changing drastically since the scene is mostly static. Hence I didn't think about message synchronization. Also, message synchronization may be overkill for this task. Though I am not sure about it.
Following are my queries-
- How do I make subscriber inside the class in this situation? Any workaround, please.
- What are the best practices to use subscriber inside a class?
Besides your compilation error the
PointCloudDataProvider::init(..)
method creates aNodeHandle
instance with function-scope which will be destructed as soon asinit(..)
finishes. That is probably not what you want.Yes. You are right. Based on the reasons/requirements which I mentioned in the post, can you please suggest me the proper solution for this? Thank you very much for your help.