virtual void onInit() { ros::NodeHandle & nh = getNodeHandle(); ros::NodeHandle & pnh = getPrivateNodeHandle(); int queueSize = 10; bool approxSync = true; pnh.param("approx_sync", approxSync, approxSync); pnh.param("queue_size", queueSize, queueSize); pnh.param("max_depth", maxDepth_, maxDepth_); pnh.param("voxel_size", voxelSize_, voxelSize_); pnh.param("decimation", decimation_, decimation_); pnh.param("noise_filter_radius", noiseFilterRadius_, noiseFilterRadius_); pnh.param("noise_filter_min_neighbors", noiseFilterMinNeighbors_, noiseFilterMinNeighbors_); ROS_INFO("Approximate time sync = %s", approxSync?"true":"false"); if(approxSync) { approxSyncDepth_ = new message_filters::Synchronizer<MyApproxSyncDepthPolicy>(MyApproxSyncDepthPolicy(queueSize), imageDepthSub_, cameraInfoSub_); approxSyncDepth_->registerCallback(boost::bind(&PointCloudXYZ::callback, this, _1, _2)); approxSyncDisparity_ = new message_filters::Synchronizer<MyApproxSyncDisparityDispPolicy>(MyApproxSyncDisparityDispPolicy(queueSize), disparitySub_, disparityCameraInfoSub_); approxSyncDisparity_->registerCallback(boost::bind(&PointCloudXYZ::callbackDisparity, this, _1, _2)); } else { exactSyncDepth_ = new message_filters::Synchronizer<MyExactSyncDepthPolicy>(MyExactSyncDepthPolicy(queueSize), imageDepthSub_, cameraInfoSub_); exactSyncDepth_->registerCallback(boost::bind(&PointCloudXYZ::callback, this, _1, _2)); exactSyncDisparity_ = new message_filters::Synchronizer<MyExactSyncDisparityDispPolicy>(MyExactSyncDisparityDispPolicy(queueSize), disparitySub_, disparityCameraInfoSub_); exactSyncDisparity_->registerCallback(boost::bind(&PointCloudXYZ::callbackDisparity, this, _1, _2)); } ros::NodeHandle depth_nh(nh, "depth"); ros::NodeHandle depth_pnh(pnh, "depth"); image_transport::ImageTransport depth_it(depth_nh); image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), depth_pnh); imageDepthSub_.subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth); cameraInfoSub_.subscribe(depth_nh, "camera_info", 1); disparitySub_.subscribe(nh, "disparity/image", 1); disparityCameraInfoSub_.subscribe(nh, "disparity/camera_info", 1); cloudPub_ = nh.advertise<sensor_msgs::PointCloud2>("cloud", 1); }
virtual void onInit() { ros::NodeHandle & nh = getNodeHandle(); ros::NodeHandle & pnh = getPrivateNodeHandle(); int queueSize = 10; bool approxSync = true; pnh.param("approx_sync", approxSync, approxSync); pnh.param("queue_size", queueSize, queueSize); NODELET_INFO("Approximate time sync = %s", approxSync?"true":"false"); rgbdImagePub_ = nh.advertise<rtabmap_ros::RGBDImage>("rgbd_image", 1); rgbdImageCompressedPub_ = nh.advertise<rtabmap_ros::RGBDImage>("rgbd_image/compressed", 1); if(approxSync) { approxSyncDepth_ = new message_filters::Synchronizer<MyApproxSyncDepthPolicy>(MyApproxSyncDepthPolicy(queueSize), imageSub_, imageDepthSub_, cameraInfoSub_); approxSyncDepth_->registerCallback(boost::bind(&RGBDSync::callback, this, _1, _2, _3)); } else { exactSyncDepth_ = new message_filters::Synchronizer<MyExactSyncDepthPolicy>(MyExactSyncDepthPolicy(queueSize), imageSub_, imageDepthSub_, cameraInfoSub_); exactSyncDepth_->registerCallback(boost::bind(&RGBDSync::callback, this, _1, _2, _3)); } ros::NodeHandle rgb_nh(nh, "rgb"); ros::NodeHandle depth_nh(nh, "depth"); ros::NodeHandle rgb_pnh(pnh, "rgb"); ros::NodeHandle depth_pnh(pnh, "depth"); image_transport::ImageTransport rgb_it(rgb_nh); image_transport::ImageTransport depth_it(depth_nh); image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh); image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), depth_pnh); imageSub_.subscribe(rgb_it, rgb_nh.resolveName("image"), 1, hintsRgb); imageDepthSub_.subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth); cameraInfoSub_.subscribe(rgb_nh, "camera_info", 1); }