Pointcloud2 synchronization with sensor_msg

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

zlg9 gravatar image

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

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 image jarvisschultz  ( 2019-11-08 13:41:37 -0500 )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 image zlg9  ( 2019-11-08 14:18:20 -0500 )edit

I doubt boost::bind is the problem. I'd guess the issue is more related to the message_filters. Never tried this myself, but a quick google showed a few people having trouble with time sync and PCL message types. It seems the issue is that the header in the pcl::PCLPointCloud2 uses a uint64 that can't automatically be converted to a ros::Time instance. The sensor_msgs/PointCloud2 option is an okay workaround, but it does likely incur an extra expense of a PCL conversion or two.

jarvisschultz gravatar image jarvisschultz  ( 2019-11-12 10:21:50 -0500 )edit
add a comment