Robotics StackExchange | Archived questions

Pointcloud2 synchronization with sensor_msg

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.

Asked by zlg9 on 2019-11-08 13:36:46 UTC

Comments

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

Asked by jarvisschultz on 2019-11-08 14:41:37 UTC

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 ?

Asked by zlg9 on 2019-11-08 15:18:20 UTC

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.

Asked by jarvisschultz on 2019-11-12 11:21:50 UTC

Answers