| 143 | } |
| 144 | |
| 145 | void |
| 146 | run() |
| 147 | { |
| 148 | pcl::OpenNIGrabber grabber(device_id_); |
| 149 | std::function<void(const CloudConstPtr&)> handler_function = |
| 150 | [this](const CloudConstPtr& cloud) { handleIncomingCloud(cloud); }; |
| 151 | grabber.registerCallback(handler_function); |
| 152 | grabber.start(); |
| 153 | |
| 154 | // wait for first cloud |
| 155 | while (!getLatestPointCloud()) |
| 156 | std::this_thread::sleep_for(10ms); |
| 157 | |
| 158 | viewer_.showCloud(getLatestPointCloud()); |
| 159 | |
| 160 | boost::asio::io_context io_service; |
| 161 | tcp::endpoint endpoint(tcp::v4(), static_cast<unsigned short>(port_)); |
| 162 | tcp::acceptor acceptor(io_service, endpoint); |
| 163 | tcp::socket socket(io_service); |
| 164 | |
| 165 | std::cout << "Listening on port " << port_ << "..." << std::endl; |
| 166 | acceptor.accept(socket); |
| 167 | |
| 168 | std::cout << "Client connected." << std::endl; |
| 169 | |
| 170 | double start_time = pcl::getTime(); |
| 171 | int counter = 0; |
| 172 | |
| 173 | while (!viewer_.wasStopped()) { |
| 174 | |
| 175 | // wait for client |
| 176 | unsigned int nr_points = 0; |
| 177 | boost::asio::read(socket, boost::asio::buffer(&nr_points, sizeof(nr_points))); |
| 178 | |
| 179 | PointCloudBuffers::Ptr buffers_to_send = getLatestBuffers(); |
| 180 | |
| 181 | nr_points = static_cast<unsigned int>(buffers_to_send->points.size() / 3); |
| 182 | boost::asio::write(socket, boost::asio::buffer(&nr_points, sizeof(nr_points))); |
| 183 | |
| 184 | if (nr_points) { |
| 185 | boost::asio::write(socket, |
| 186 | boost::asio::buffer(&buffers_to_send->points.front(), |
| 187 | nr_points * 3 * sizeof(short))); |
| 188 | boost::asio::write(socket, |
| 189 | boost::asio::buffer(&buffers_to_send->rgb.front(), |
| 190 | nr_points * 3 * sizeof(unsigned char))); |
| 191 | } |
| 192 | |
| 193 | counter++; |
| 194 | |
| 195 | double new_time = pcl::getTime(); |
| 196 | double elapsed_time = new_time - start_time; |
| 197 | if (elapsed_time > 1.0) { |
| 198 | double frames_per_second = counter / elapsed_time; |
| 199 | start_time = new_time; |
| 200 | counter = 0; |
| 201 | std::cout << "fps: " << frames_per_second << std::endl; |
| 202 | } |