Pointcloud2 synchronization with sensor_msg

asked 2019-11-08 12:36:46 -0600

zlg9 gravatar image

updated 2019-11-08 12:41:32 -0600

My node subscribes to three sensors (Lidar,GPS, IMU). I need to synchronize three messages as they are generated at different frequency. I first tried synchronizing GPS with IMU and it was fine, however, when I add Lidar point cloud , I get an error with regard to boost.

My Code:

// --------callback function---------
void callbackFunction(const sensor_msgs::PointCloud2Ptr &msg1, const gps_common::GPSFixConstPtr &msg2, const sensor_msgs::ImuConstPtr &msg3)
{
// do stuff with msg1, msg2, msg3
}


// --------main function---------
int main(int argc, char** argv) {
  ..
  ros::NodeHandle nh;
  message_filters::Subscriber<sensor_msgs::PointCloud2> sensor1_sub(nh, "/points_raw", 1000);
  message_filters::Subscriber<gps_common::GPSFix> sensor2_sub(nh, "/gps", 5);
  message_filters::Subscriber<sensor_msgs::Imu> sensor3_sub(nh, "/imu", 5);
  typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::PointCloud2,gps_common::GPSFix,sensor_msgs::Imu> 
  MySyncPolicy;
  message_filters::Synchronizer<MySyncPolicy> sync(MySyncPolicy(10),sensor1_sub, sensor2_sub, sensor3_sub);
  sync.registerCallback(boost::bind(&callbackFunction, _1, _2,_3));
  ros::spin();
}

When I pass sensor1_sub to synchronizer (like what is in code above), it returns error:

error: invalid initialization of reference of type ‘const boost::shared_ptr<sensor_msgs::PointCloud2_<std::allocator<void> > >&’ from expression of type ‘const boost::shared_ptr<const sensor_msgs::PointCloud2_<std::allocator<void> > >’
     unwrapper<F>::unwrap(f, 0)(a[base_type::a1_], a[base_type::a2_], a[base_type::a3_]);

I checked this link as well. Couldn't solve the issue in my case.

edit retag flag offensive close merge delete

Comments

Try changing the signature of your callbackFunction to take in const sensor_msgs::PointCloud2ConstPtr as the first arg

jarvisschultz gravatar imagejarvisschultz ( 2019-11-08 13:41:37 -0600 )edit

Thanks, that was a good catch, solved the issue. I was initially using pcl::PCLPointCloud2 instead of sensor_msgs::PointCloud2. But that returns another error:

could not convert "m.pcl::PCLPointCloud2::header.pcl::PCLHeader::stamp" from "const uint64_t {aka const long unsigned int}" to "ros::Time" static ros::Time value(const M& m) { return m.header.stamp; }

Is it like I can't use PCLPointcloud2 together with boost::bind ?

zlg9 gravatar imagezlg9 ( 2019-11-08 14:18:20 -0600 )edit