CloudPlayerThread.hpp 7.91 KB
Newer Older
1
2
3
4
5
6
7
8
9
10
#ifndef CLOUDPLAYERTHREAD_HPP
#define CLOUDPLAYERTHREAD_HPP

#include "ofMain.h"
#include <pcl/compression/octree_pointcloud_compression.h>
#include <sys/stat.h>

namespace ofxCloudLib
{

11
#define NUM_CLOUD_BUFFERS 4
12
13
14
class CloudPlayerThread : public ofThread
{
  public:
15
16
17
18
19
20
21
    CloudPlayerThread()
    {

        octree_coder_ = new pcl::io::OctreePointCloudCompression<pcl::PointXYZ>(
            pcl::io::HIGH_RES_OFFLINE_COMPRESSION_WITH_COLOR,
            false, .01, .01, false, 1, 1,
            static_cast<unsigned char>(4));
22

23
        output_cloud_.reset(new pcl::PointCloud<pcl::PointXYZ>);
24
25
    }

26
27
28
29
30
31
    // ~CloudPlayerThread()
    // {
    //     recorded_.close();
    //     waitForThread(true);
    // }

32
33
34
35
    bool setup(std::string playback_path)
    {
        playback_path_ = playback_path;
        if (ofDirectory::doesDirectoryExist(playback_path_)) {
36
            ofDirectory dir = ofDirectory(playback_path_);
37

38
39
            filename_       = playback_path_;
            is_setup_       = true;
40
            total_frames_   = dir.listDir();
41
            decoded_frames_ = 0;
42

43
            ofLogNotice("CloudPlayerThread") << "will be playing " << total_frames_ << " frames from " << playback_path_;
44
45

        } else {
46
            ofLogError("CloudPlayerThread") << "no such directory: " << playback_path_;
47
48
49
50
        }
        return is_setup_;
    }

51
52
53
54
55
    size_t get_number_of_frames() { return total_frames_; }
    size_t get_current_frame() { return decoded_frames_; }

    void start()
    {
56
        decoded_frames_ = 0;
57
58
59
        startThread();
    }

60
    pcl::PointCloud<pcl::PointXYZ> &get_cloud() { return cloud_[ready_cloud_buffer_]; }
61
62
63
64
65

    void update_cloud()
    {
        new_cloud_ = false;

66
67
68
        while (recorded_.tryReceive(cloud_[(ready_cloud_buffer_ + 1) % NUM_CLOUD_BUFFERS])) {
            new_cloud_          = true;
            ready_cloud_buffer_ = (ready_cloud_buffer_ + 1) % NUM_CLOUD_BUFFERS;
69
        }
70

71
        if (new_cloud_) {
72
            // ofLogNotice("CloudPlayerThread") << "safe cloud data with " << cloud_[ready_cloud_buffer_].size() << " points";
73
74
75
76
77
78
79
        }
    }

    void update_mesh()
    {
        new_mesh_ = false;

80
        while (recorded_mesh_.tryReceive(mesh_[(ready_mesh_buffer_ + 1) % NUM_CLOUD_BUFFERS])) {
81
            new_mesh_          = true;
82
            ready_mesh_buffer_ = (ready_mesh_buffer_ + 1) % NUM_CLOUD_BUFFERS;
83
84
        }
        if (new_mesh_) {
85
            // ofLogNotice("CloudPlayerThread") << "safe mesh data  with " << mesh_[ready_mesh_buffer_].getNumVertices() << " vertices";
86
87
88
89
90
        }
    }

    bool is_cloud_new() { return new_cloud_; }
    bool is_mesh_new() { return new_mesh_; }
91
92
93
94
95
96
97
98
99

    ofMesh &get_mesh() 
    { 
                if (!isThreadRunning() && mesh_[ready_mesh_buffer_].getNumVertices()!=0) {
                    mesh_[ready_mesh_buffer_].clear();
                }

        return mesh_[ready_mesh_buffer_]; 
    }
100

101
102
    float offset_x_ = 0;
    float offset_z_ = 0;
103
104
105

    void stop()
    {
106
        if (isThreadRunning()) {
107
108
            ofLogNotice("CloudPlayerThread") << "stopped_thread is now true";
            stop_thread_ = true;
109

110
111
        } else {
            ofLogNotice("CloudPlayerThread") << "double-stop prevented";
112
        }
113
114
115
116
117
118
119
    }

    std::string get_filename()
    {
        return filename_;
    }

120
121
    bool is_done_ = false;

122
123
124
  private:
    void threadedFunction()
    {
125
        ofLogNotice("CloudPlayerThread") << "thread started";
126
        decodingFenceTimestamp = 0;
127
128
129

        while (1) {
            if (stop_thread_) {
130
131
132
133
134
135
136
137
138
139
140
141
142
143
                output_cloud_->clear();
                ofMesh mesh;

                ofLogNotice("CloudPlayerThread") << "sending empty datasets before stopping";

                recorded_.send(std::move(*output_cloud_));
                recorded_mesh_.send(std::move(mesh));

                // ofLogNotice("CloudPlayerThread") << "closing the channels";

                // recorded_.close();
                // recorded_mesh_.close();

                ofLogNotice("CloudPlayerThread") << "stopping the thread";
144
                stop_thread_ = false;
145
146
                new_cloud_   = false;

147
                stopThread();
148
                ofLogNotice("CloudPlayerThread") << "thread stopped";
149
150
                return;
            }
151

152
153
            auto sysnow   = std::chrono::system_clock::now();
            auto duration = sysnow.time_since_epoch();
154
            uint64_t now  = std::chrono::duration_cast<std::chrono::milliseconds>(duration).count();
155
156
157
158
159
160
161
162
163
164
165
166
167

            if (now > (decodingFenceTimestamp - ((1000. / 60.) / 2.))) {

                // devrait maintenir le modulo de la soustraction pour ne pas accumuler l'erreur (mais pas super grave)
                lastDecodingTimestamp = now;

                char filename_to_decode[100];
                sprintf(filename_to_decode, "%s/cloud_%04i.pcd", playback_path_.c_str(), int(decoded_frames_++));
                filename_ = std::string(filename_to_decode);
                ifstream cloudfile;
                cloudfile.open(filename_to_decode, ios::in | ios::binary);

                if (!cloudfile) {
168
                    ofLogNotice("CloudPlayerThread") << filename_to_decode << " not found";
169
170
                    stop();
                    // stop_thread_ = true;
171
                    // if looping....
172
                    is_done_        = true;
173
174
175
176
177
                    decoded_frames_ = 0;
                } else {

                    cloudfile.seekg(0);
                    cloudfile.unsetf(ios_base::skipws);
178
179

                    octree_coder_->decodePointCloud(cloudfile, output_cloud_);
180
181

                    struct stat st;
182
                    stat(filename_to_decode, &st);
183
184
185
186
187
188
189
190
191
192
193
194
                    uint64_t status_ms = st.st_ctime * 1000 + st.st_ctim.tv_nsec / 1000000;

                    if (previousCloudTimestamp == 0) {
                        delay = 16;
                    } else {
                        delay = status_ms - previousCloudTimestamp;
                    }
                    delay = 50;

                    previousCloudTimestamp = status_ms;
                    decodingFenceTimestamp = now + delay;

195
                    // ofLogNotice("CloudPlayerThread") << decoded_frames_ << " with " << output_cloud_->size() << " points from " << filename_to_decode << " with interval: " << delay;
196
197

                    ofMesh mesh;
198
199
200
201
                    for (unsigned int qq = 0; qq < output_cloud_->size(); qq++) {
                        mesh.addVertex(ofPoint(output_cloud_->points[qq].x + offset_x_,
                                               output_cloud_->points[qq].y,
                                               output_cloud_->points[qq].z + offset_z_));
202
                    }
203
204
205
206
                    recorded_.send(*output_cloud_);
                    recorded_mesh_.send(mesh);
                    // recorded_.send(std::move(*output_cloud_));
                    // recorded_mesh_.send(std::move(mesh));
207
                }
208
209
                cloudfile.close();

210
            } else {
211

212
                //       std::cout << "DECODEUR: sleep" << std::endl;
213
                std::this_thread::sleep_for(std::chrono::milliseconds(2));
214
            }
215
            //   std::this_thread::sleep_for(std::chrono::milliseconds(2));
216
217
218
219
220
        }
    }

    uint64_t lastDecodingTimestamp  = 0;
    uint64_t previousCloudTimestamp = 0;
221
    uint64_t decodingFenceTimestamp = 0;
222
223
    uint64_t delay;

224

225
226
    std::string filename_;
    std::string playback_path_;
227
228
229
230
    bool new_mesh_    = false;
    bool new_cloud_   = false;
    bool is_setup_    = false;
    bool stop_thread_ = false;
231

232
    size_t decoded_frames_ = 0;
233
    size_t total_frames_   = 0;
234

235
236
237
    size_t ready_cloud_buffer_ = 0;
    size_t ready_mesh_buffer_  = 0;

238
239
    pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud_;

240
241
    ofMesh mesh_[NUM_CLOUD_BUFFERS];
    pcl::PointCloud<pcl::PointXYZ> cloud_[NUM_CLOUD_BUFFERS];
242
    pcl::io::OctreePointCloudCompression<pcl::PointXYZ> *octree_coder_;
243
    ofThreadChannel<pcl::PointCloud<pcl::PointXYZ>> recorded_;
244
245
    ofThreadChannel<ofMesh> recorded_mesh_;
};
246
} // namespace ofxCloudLib
247
248

#endif // CLOUDPLAYERTHREAD_HPP