Skip to content

Commit 7bdea18

Browse files
Kontra2BKontra2B
committed
testing
1 parent fbb4909 commit 7bdea18

5 files changed

Lines changed: 76 additions & 152 deletions

File tree

‎examples/capture/rs-capture.cpp‎

Lines changed: 34 additions & 22 deletions
Original file line numberDiff line numberDiff line change
@@ -11,56 +11,68 @@
1111
// capture depth and color video streams and render them to the screen
1212
int main(int argc, char * argv[]) try
1313
{
14-
std::map<rs2_stream, std::pair<rs2_metadata_type, rs2_metadata_type>> old;
14+
std::map<int, std::pair<rs2_metadata_type, rs2_metadata_type>> last;
1515
rs2::log_to_file(RS2_LOG_SEVERITY_DEBUG, "rs-capture.log");
1616
// Create a simple OpenGL window for rendering:
1717
// window app(1280, 720, "RealSense Capture Example");
1818

19+
// Declare depth colorizer for pretty visualization of depth data
20+
rs2::colorizer color_map;
21+
// Declare rates printer for showing streaming rates of the enabled streams.
22+
rs2::rates_printer printer;
23+
1924
// Declare RealSense pipeline, encapsulating the actual device and sensors
2025
rs2::pipeline pipe;
26+
rs2::config cfg;
2127

2228
// Start streaming with default recommended configuration
2329
// The default video configuration contains Depth and Color streams
2430
// If a device is capable to stream IMU data, both Gyro and Accelerometer are enabled by default
25-
rs2::config cfg;
2631
cfg.enable_stream(RS2_STREAM_DEPTH);
2732
cfg.enable_stream(RS2_STREAM_COLOR);
2833
cfg.enable_stream(RS2_STREAM_INFRARED);
34+
// Or enable all camera streams
35+
// cfg.enable_all_streams();
2936

3037
if (argc > 1) {
3138
cfg.enable_device(argv[1]);
3239
}
3340
pipe.start(cfg);
3441

35-
int idx = 1;
42+
int idx = 0;
3643
while (true) // Application still alive?
3744
{
38-
//rs2::frameset data = pipe.wait_for_frames(). // Wait for next set of frames from the camera
39-
const rs2::frameset& fset = pipe.wait_for_frames();
45+
rs2::frameset data = pipe.wait_for_frames(4000000000).
46+
apply_filter(printer). // Print each enabled stream frame rate
47+
apply_filter(color_map); // Find and colorize the depth data
4048
// std::cout << std::endl << "----- CLOCK:" << std::chrono::steady_clock::now().time_since_epoch().count() << "ns" << std::endl;
41-
const size_t nf = fset.size();
42-
int i = 1;
49+
const size_t nf = data.size();
4350

44-
fset.foreach_rs([&idx, &old, &i, nf] (const rs2::frame& f) {
51+
std::cout << std::endl << idx << '/' << nf << '.';
52+
data.foreach_rs([&last, &idx ] (const rs2::frame& f) {
4553
static int ne = 0;
46-
auto fs = f.get_profile().stream_type();
47-
auto fc = f.get_frame_metadata(RS2_FRAME_METADATA_FRAME_COUNTER);
48-
auto ft = f.get_frame_metadata(RS2_FRAME_METADATA_FRAME_TIMESTAMP);
49-
// auto fr = f.get_frame_metadata(RS2_FRAME_METADATA_CRC);
50-
//char path[64];
51-
//sprintf(path, "S%d-%d.bin", (int)fs, (int)idx);
52-
if (i == 1) std::cout << std::endl << idx << '.';
53-
std::cout << '\t' << i++ << '/' << nf << '\t' << fs << ':' << fc << '/' << ft;
54+
auto fn = f.get_profile().stream_name().substr(0, 1);
55+
auto fi = f.get_profile().stream_index();
56+
auto fu = f.get_profile().unique_id();
57+
rs2_metadata_type fc, ft;
58+
try {
59+
fc = f.get_frame_metadata(RS2_FRAME_METADATA_FRAME_COUNTER);
60+
ft = f.get_frame_metadata(RS2_FRAME_METADATA_FRAME_TIMESTAMP);
61+
} catch(...) { return; }
62+
// auto fr = f.get_frame_metadata(RS2_FRAME_METADATA_CRC);
63+
//char path[64];
64+
//sprintf(path, "S%d-%d.bin", (int)fs, (int)idx);
65+
std::cout << '\t' << fn << fu << ':' << fc << '/' << ft;
5466
try {
55-
const auto& ofs = old.at(fs);
67+
const auto& ofs = last.at(fu);
5668
if (!(fc > ofs.first && ft > ofs.second)) {
57-
std::cout << std::endl << "!!!/" << ++ne << ":\t\t" << fs << ':' << ofs.first << '/' << ofs.second << std::endl;
58-
// for (const auto& rec:old) std::cout << "\t" << rec.first << ":" << rec.second.first << '/' << rec.second.second;
59-
if (ne > 10) exit(EXIT_FAILURE);
69+
std::cout << " !!!/" << ++ne << ' ' << fn << ':' << ofs.first << '/' << ofs.second;
70+
if (ne > 10) exit(EXIT_FAILURE);
6071
}
72+
else if (fc > ofs.first + 1)
73+
std::cout << " +";
6174
} catch(...) {}
62-
old[fs].first = fc;
63-
old[fs].second = ft;
75+
last[fu] = {fc, ft};
6476
//auto fo = fopen(path, "wb+");
6577
//fwrite(fd, f.get_data_size(), 1, fo);
6678
//fclose(fo);

‎src/frame.cpp‎

Lines changed: 16 additions & 25 deletions
Original file line numberDiff line numberDiff line change
@@ -16,31 +16,22 @@ namespace librealsense {
1616

1717
std::ostream & operator<<( std::ostream & s, const frame_interface & f )
1818
{
19-
if( !&f )
20-
{
21-
s << "[null]";
22-
}
23-
else
24-
{
25-
auto composite = dynamic_cast<const composite_frame *>(&f);
26-
if( composite )
27-
{
28-
s << "[";
29-
for( int i = 0; i < composite->get_embedded_frames_count(); i++ )
30-
{
31-
s << *composite->get_frame( i );
32-
}
33-
s << "]";
34-
}
35-
else
36-
{
37-
s << "[" << get_abbr_string( f.get_stream()->get_stream_type() );
38-
s << f.get_stream()->get_unique_id();
39-
s << " " << f.get_header();
40-
s << "]";
41-
}
42-
}
43-
return s;
19+
auto composite = dynamic_cast<const composite_frame *>(&f);
20+
if( composite )
21+
{
22+
s << "[C/" << composite->get_embedded_frames_count();
23+
for( int i = 0; i < composite->get_embedded_frames_count(); i++ )
24+
s << *composite->get_frame( i );
25+
s << ']';
26+
}
27+
else
28+
{
29+
s << "[" << get_abbr_string( f.get_stream()->get_stream_type() );
30+
s << f.get_stream()->get_unique_id();
31+
s << " " << f.get_header();
32+
s << "]";
33+
}
34+
return s;
4435
}
4536

4637

‎src/sensor.cpp‎

Lines changed: 2 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -314,10 +314,10 @@ void log_callback_end( uint32_t fps,
314314
int bpp = get_image_bpp(profile->get_format());
315315
auto frame_size = compute_frame_expected_size(width, height, bpp);
316316

317-
frame_additional_data additional_data(0,
317+
frame_additional_data additional_data(0.0,
318318
0,
319319
system_time,
320-
static_cast<uint8_t>(fo.metadata_size),
320+
fo.metadata_size,
321321
(const uint8_t*)fo.metadata,
322322
fo.backend_time,
323323
last_timestamp,
@@ -328,7 +328,6 @@ void log_callback_end( uint32_t fps,
328328

329329
if (_metadata_modifier)
330330
_metadata_modifier(additional_data);
331-
fr->additional_data = additional_data;
332331

333332
// update additional data
334333
additional_data.timestamp = timestamp_reader->get_frame_timestamp(fr);

‎src/stream.h‎

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -96,14 +96,14 @@ namespace librealsense
9696
rs2_metadata_type getLastFrame() { return _last_frame; }
9797
void setLastFrame(rs2_metadata_type last_frame ) { _last_frame = last_frame; }
9898

99-
rs2_metadata_type getLastTimestamp() { return _last_timestamp; }
100-
void setLastTimestamp(rs2_metadata_type last_timestamp ) { _last_timestamp = last_timestamp; }
99+
rs2_time_t getLastTimestamp() { return _last_timestamp; }
100+
void setLastTimestamp(rs2_time_t last_timestamp ) { _last_timestamp = last_timestamp; }
101101
private:
102102
int _index = 1;
103103
int _uid = 0;
104104
rs2_stream _type = RS2_STREAM_ANY;
105105
rs2_metadata_type _last_frame = 0;
106-
rs2_metadata_type _last_timestamp = 0;
106+
rs2_time_t _last_timestamp = 0;
107107
rs2_format _format = RS2_FORMAT_ANY;
108108
uint32_t _framerate = 0;
109109
int _tag = profile_tag::PROFILE_TAG_ANY;

‎src/sync.cpp‎

Lines changed: 21 additions & 99 deletions
Original file line numberDiff line numberDiff line change
@@ -332,10 +332,6 @@ namespace librealsense
332332
}
333333
update_next_expected( matcher, f );
334334

335-
// We want to keep track of a "last-arrived" frame which is our current equivalent of "now" -- it contains the
336-
// latest timestamp/frame-number/etc. that we can compare to.
337-
auto const last_arrived = f->get_header();
338-
339335
if( ! _frames_queue[matcher.get()].q.enqueue( std::move( f ) ) )
340336
// If we get stopped, nothing to do!
341337
return;
@@ -345,131 +341,58 @@ namespace librealsense
345341
// If we have a Color frame but not Depth, then Depth is "missing" and needs to be
346342
// waited-for...
347343

348-
std::vector< frame_holder * > frames_arrived;
349-
std::vector< librealsense::matcher * > frames_arrived_matchers;
344+
std::map<librealsense::matcher *, frame_holder> frames_arrived;
350345
std::vector< int > synced_frames;
351346
std::vector< int > unsynced_frames;
352347
std::vector< librealsense::matcher * > missing_streams;
348+
std::vector< frame_holder > match;
353349

350+
LOG_DEBUG("[WOJTEK] " << __func__ << ':' << *f.frame);
354351
while( true )
355352
{
356-
missing_streams.clear();
357-
frames_arrived_matchers.clear();
358-
frames_arrived.clear();
359-
360-
std::vector< frame_holder > match;
361353
{
362354
// We don't want to stop while syncing!
363355
std::lock_guard< std::mutex > lock( _mutex );
364356

365357
// We want to release one frame from each matcher. If a matcher has nothing queued, it is "missing" and
366358
// we need to consider waiting for it:
359+
for( auto s = _frames_queue.begin(); s != _frames_queue.end(); s++ )
360+
if (s->second.q.empty()) return;
361+
367362
for( auto s = _frames_queue.begin(); s != _frames_queue.end(); s++ )
368363
{
369364
librealsense::matcher * const m = s->first;
370-
if( ! s->second.q.peek( [&]( frame_holder & fh ) {
371-
LOG_IF_ENABLE( "... have " << *fh.frame, env );
372-
frames_arrived.push_back( &fh );
373-
frames_arrived_matchers.push_back( m );
374-
} ) )
375-
{
376-
missing_streams.push_back( m );
377-
}
378-
}
379-
if( frames_arrived.empty() )
380-
{
381-
// LOG_IF_ENABLE( "... nothing more to do", env );
382-
break;
365+
while(s->second.q.try_dequeue(&frames_arrived[m]))
366+
;
383367
}
384368

385369
// From what we collected, we want to release only the frames that are synchronized (based on timestamp,
386370
// number, etc.) -- anything else we'll leave to the next iteration. The synced frames should be the
387371
// earliest possible!
388372

389-
frame_holder * curr_sync = frames_arrived[0];
390-
synced_frames.clear();
391-
synced_frames.push_back( 0 );
392-
393-
// Sometimes we have to release newly-arrived frames even before frames we already had previously
394-
// queued. If we have something like this, 'have_unsynced_frames' will be true:
395-
unsynced_frames.clear();
396-
for( auto i = 1; i < frames_arrived.size(); i++ )
397-
{
398-
if( are_equivalent( *curr_sync, *frames_arrived[i] ) )
399-
{
400-
synced_frames.push_back( i );
401-
}
402-
else if( is_smaller_than( *frames_arrived[i], *curr_sync ) )
403-
{
404-
unsynced_frames.insert( unsynced_frames.end(), synced_frames.begin(), synced_frames.end() );
405-
synced_frames.clear();
406-
synced_frames.push_back( i );
407-
curr_sync = frames_arrived[i];
408-
}
409-
else
410-
{
411-
unsynced_frames.push_back( i );
412-
}
413-
}
414-
bool release_synced_frames = ( synced_frames.size() != 0 );
415-
if( unsynced_frames.empty() )
416-
{
417-
// Everything (could be only one!) matches together... but if we also have
418-
// something missing, we can't release anything yet...
419-
for( auto i : missing_streams )
420-
{
421-
LOG_IF_ENABLE( "... missing " << i->get_name() << ", next expected @"
422-
<< rsutils::string::from( _next_expected[i].value ) << " (from "
423-
<< rsutils::string::from( _next_expected[i].fps ) << " fps)",
424-
env );
425-
if( skip_missing_stream( *curr_sync, i, last_arrived, env ) )
426-
{
427-
LOG_IF_ENABLE( "... cannot be synced; not waiting for it", env );
428-
continue;
429-
}
430-
431-
LOG_IF_ENABLE( "... waiting for it", env );
432-
release_synced_frames = false;
433-
}
434-
}
435-
else
436-
{
437-
for( auto i : unsynced_frames )
438-
{
439-
LOG_IF_ENABLE( " - " << *frames_arrived[i]->frame << " is not in sync; won't be released", env );
440-
}
441-
}
442-
if( ! release_synced_frames )
443-
break;
444-
445-
match.reserve( synced_frames.size() );
446-
447-
for( auto index : synced_frames )
448-
{
449-
frame_holder frame;
450-
int const timeout_ms = 5000;
451-
librealsense::matcher * m = frames_arrived_matchers[index];
452-
_frames_queue[m].q.dequeue( &frame, timeout_ms );
453-
match.push_back( std::move( frame ) );
454-
}
373+
for(auto& matches: frames_arrived)
374+
match.push_back(std::move(matches.second));
455375
}
456376

457377
// The frameset should always be with the same order of streams (the first stream carries extra
458378
// meaning because it decides the frameset properties) -- so we sort them...
379+
if (match.size() > 1 ) {
459380
std::sort( match.begin(),
460381
match.end(),
461382
[]( const frame_holder & f1, const frame_holder & f2 ) {
462383
return f1.frame->get_stream()->get_unique_id()
463384
> f2.frame->get_stream()->get_unique_id();
464385
} );
465-
386+
}
466387

467388
frame_holder composite = env.source->allocate_composite_frame(std::move(match));
468389
if (composite.frame)
469390
{
470-
auto cb = begin_callback();
391+
begin_callback();
392+
LOG_DEBUG("[WOJTEK] " << __func__ << ':' << *composite.frame);
471393
_callback(std::move(composite), env);
472394
}
395+
break;
473396
}
474397
}
475398

@@ -490,6 +413,7 @@ namespace librealsense
490413
}
491414
bool frame_number_composite_matcher::is_smaller_than(frame_holder & a, frame_holder & b)
492415
{
416+
if (!a || !b) return false;
493417
return a->get_frame_number() < b->get_frame_number();
494418
}
495419
void frame_number_composite_matcher::clean_inactive_streams(frame_holder& f)
@@ -565,16 +489,14 @@ namespace librealsense
565489
auto min_fps = std::min(a_fps, b_fps);
566490

567491
auto ts = extract_timestamps(a, b);
492+
bool equal = are_equivalent(ts.first, ts.second, min_fps);
568493

569-
return are_equivalent(ts.first, ts.second, min_fps);
494+
return equal;
570495
}
571496

572497
bool timestamp_composite_matcher::is_smaller_than(frame_holder & a, frame_holder & b)
573498
{
574-
if (!a || !b)
575-
{
576-
return false;
577-
}
499+
if (!a || !b) return false;
578500

579501
auto ts = extract_timestamps(a, b);
580502

@@ -737,8 +659,8 @@ namespace librealsense
737659
auto gap = 1000. / fps;
738660
if( std::abs( a - b ) < ( gap / 2 ) )
739661
{
740-
//LOG_DEBUG( "... " << rsutils::string::from( a ) << " == " << rsutils::string::from( b ) << " {diff}"
741-
// << std::abs( a - b ) << " < " << rsutils::string::from( gap / 2 ) << "{gap/2}" );
662+
LOG_DEBUG( "... " << rsutils::string::from( a ) << " == " << rsutils::string::from( b ) << " {diff}"
663+
<< std::abs( a - b ) << " < " << rsutils::string::from( gap / 2 ) << "{gap/2}" );
742664
return true;
743665
}
744666

0 commit comments

Comments
 (0)