ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

Best practices to use subscriber inside a class

asked 2018-02-14 05:56:26 -0500

ravijoshi gravatar image

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-

  1. The point cloud data is required by another method hence the processing cannot be done inside pointCloudCallback function
  2. Use multiple instances of PointCloudDataProvider with different rostopic.
  3. Access point cloud data from multiple instances of PointCloudDataProvider by using getRecentData() 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-

  1. How do I make subscriber inside the class in this situation? Any workaround, please.
  2. What are the best practices to use subscriber inside a class?
edit retag flag offensive close merge delete

Comments

1

Besides your compilation error the PointCloudDataProvider::init(..) method creates a NodeHandle instance with function-scope which will be destructed as soon as init(..) finishes. That is probably not what you want.

gvdhoorn gravatar image gvdhoorn  ( 2018-02-14 06:53:22 -0500 )edit

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.

ravijoshi gravatar image ravijoshi  ( 2018-02-14 07:07:33 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
1

answered 2018-02-15 02:35:32 -0500

Hamid Didari gravatar image

updated 2018-02-15 02:37:09 -0500

you must to create one node handle in your class such as:

class High_control
{
private:
       ros::NodeHandle n;
....
}

and in your init you should do something like this

rcInSubscriber     = n.subscribe<mavros_msgs::RCIn>("mavros/rc/in", 10, &High_control::RcIn,this);
edit flag offensive delete link more

Comments

Yeah! thanks a lot.

ravijoshi gravatar image ravijoshi  ( 2018-02-17 03:17:17 -0500 )edit

Question Tools

1 follower

Stats

Asked: 2018-02-14 05:56:26 -0500

Seen: 427 times

Last updated: Feb 15 '18