| 88 | }; |
| 89 | |
| 90 | int |
| 91 | main (int argc, char **argv) |
| 92 | { |
| 93 | ros::init (argc, argv, "disparity_to_cloud"); |
| 94 | ros::NodeHandle nh; |
| 95 | |
| 96 | // Prepare output |
| 97 | pub = nh.advertise<PCLPointCloud2>("output", 1); |
| 98 | |
| 99 | // Subscribe to topics |
| 100 | Synchronizer<sync_policies::ApproximateTime<PCLImage, PCLImage, CameraInfo> > sync_rgb (30); |
| 101 | Synchronizer<sync_policies::ApproximateTime<PCLImage, CameraInfo> > sync (30); |
| 102 | Subscriber<PCLImage> sub_depth, sub_rgb; |
| 103 | Subscriber<CameraInfo> sub_info; |
| 104 | sub_depth.subscribe (nh, "/camera/depth/image", 30); |
| 105 | sub_rgb.subscribe (nh, "/camera/rgb/image_color", 30); |
| 106 | sub_info.subscribe (nh, "/camera/rgb/camera_info", 120); |
| 107 | //sub_info.subscribe (nh, "/camera/depth/camera_info", 120); |
| 108 | |
| 109 | EventHelper h; |
| 110 | |
| 111 | if (argc > 1 && atoi (argv[1]) == 1) |
| 112 | { |
| 113 | ROS_INFO ("[disparity_to_cloud] Using RGB to color the points."); |
| 114 | sync_rgb.connectInput (sub_depth, sub_rgb, sub_info); |
| 115 | //sync_rgb.registerCallback (bind (&pcl_cuda::DisparityToCloud::callback, k, _1, _2, _3)); |
| 116 | sync_rgb.registerCallback (std::bind (&EventHelper::callback, &h, _1, _2, _3)); |
| 117 | } |
| 118 | else |
| 119 | { |
| 120 | sync.connectInput (sub_depth, sub_info); |
| 121 | //sync.registerCallback (bind (&pcl_cuda::DisparityToCloud::callback, k, _1, PCLImageConstPtr (), _2)); |
| 122 | sync.registerCallback (std::bind (&EventHelper::callback, &h, _1, PCLImageConstPtr (), _2)); |
| 123 | } |
| 124 | |
| 125 | // Do this indefinitely |
| 126 | ros::spin (); |
| 127 | |
| 128 | return (0); |
| 129 | } |
nothing calls this directly
no test coverage detected