Skip to content

Commit

Permalink
Merge pull request #4772 from dorodnic/autocalib_fixes
Browse files Browse the repository at this point in the history 
Fixing basic threading issues within on-chip calibration UI
  • Loading branch information
@ev-mp
ev-mp authored Sep 2, 2019
2 parents e48c001 + 1c31c21 commit 69be34c
Show file tree
Hide file tree
Showing 6 changed files with 86 additions and 42 deletions.
4 changes: 3 additions & 1 deletion common/fw-update-helper.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -168,7 +168,9 @@ namespace rs2
return false;
}

void firmware_update_manager::process_flow(std::function<void()> cleanup)
void firmware_update_manager::process_flow(
std::function<void()> cleanup,
invoker invoke)
{
std::string serial = "";
if (_dev.supports(RS2_CAMERA_INFO_ASIC_SERIAL_NUMBER))
Expand Down
3 changes: 2 additions & 1 deletion common/fw-update-helper.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,7 +22,8 @@ namespace rs2
_fw(fw), _is_signed(is_signed), _dev(dev), _ctx(ctx) {}

private:
void process_flow(std::function<void()> cleanup) override;
void process_flow(std::function<void()> cleanup,
invoker invoke) override;
bool check_for(
std::function<bool()> action, std::function<void()> cleanup,
std::chrono::system_clock::duration delta);
Expand Down
30 changes: 28 additions & 2 deletions common/notifications.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -255,6 +255,9 @@ namespace rs2
std::function<void()> notification_model::draw(ux_window& win, int w, int y,
std::shared_ptr<notification_model>& selected, std::string& error_message)
{
std::function<void()> action;
while(dispatch_queue.try_dequeue(&action)) action();

std::function<void()> follow_up = []{};

if (visible)
Expand Down Expand Up @@ -787,24 +790,47 @@ namespace rs2
_failed = true;
}

void notification_model::invoke(std::function<void()> action)
{
single_consumer_queue<bool> q;
dispatch_queue.enqueue([&q, &action](){
try
{
action();
q.enqueue(true);
}
catch(...)
{
q.enqueue(false);
}
});
bool res;
if (!q.dequeue(&res, 100000) || !res)
throw std::runtime_error("Invoke operation failed!");
}

void process_manager::start(std::shared_ptr<notification_model> n)
{
auto cleanup = [n]() {
//n->dismiss(false);
};

auto invoke = [n](std::function<void()> action) {
n->invoke(action);
};

log(to_string() << "Started " << _process_name << " process");

auto me = shared_from_this();
std::weak_ptr<process_manager> ptr(me);

std::thread t([ptr, cleanup]() {
std::thread t([ptr, cleanup, invoke]() {
auto self = ptr.lock();
if (!self) return;

try
{
self->process_flow(cleanup);
self->process_flow(cleanup, invoke);
}
catch (const error& e)
{
Expand Down
10 changes: 9 additions & 1 deletion common/notifications.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,6 +9,7 @@
#include <chrono>

#include "ux-window.h"
#include "../src/concurrency.h"

namespace rs2
{
Expand Down Expand Up @@ -83,10 +84,15 @@ namespace rs2
bool animating = false;
std::chrono::system_clock::time_point last_moved;
std::chrono::system_clock::time_point last_interacted;

single_consumer_queue<std::function<void()>> dispatch_queue;
void invoke(std::function<void()> action);
};

class device_model;

using invoker = std::function<void(std::function<void()>)>;

class process_manager : public std::enable_shared_from_this<process_manager>
{
public:
Expand All @@ -107,7 +113,9 @@ namespace rs2
void fail(std::string error);

protected:
virtual void process_flow(std::function<void()> cleanup) = 0;
virtual void process_flow(
std::function<void()> cleanup,
invoker invoke) = 0;

std::string _log;
bool _started = false;
Expand Down
68 changes: 37 additions & 31 deletions common/on-chip-calib.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -71,13 +71,16 @@ namespace rs2
} DscResultBuffer;
#pragma pack(pop)

void on_chip_calib_manager::stop_viewer()
void on_chip_calib_manager::stop_viewer(invoker invoke)
{
try
{
// Stop viewer UI
auto profiles = _sub->get_selected_profiles();
_sub->stop(_viewer);

invoke([&](){
// Stop viewer UI
_sub->stop(_viewer);
});

// Wait until frames from all active profiles stop arriving
bool frame_arrived = false;
Expand All @@ -101,7 +104,7 @@ namespace rs2
}

// Wait for next depth frame and return it
rs2::depth_frame on_chip_calib_manager::fetch_depth_frame()
rs2::depth_frame on_chip_calib_manager::fetch_depth_frame(invoker invoke)
{
auto profiles = _sub->get_selected_profiles();
bool frame_arrived = false;
Expand Down Expand Up @@ -129,7 +132,7 @@ namespace rs2
return res;
}

void on_chip_calib_manager::start_viewer(int w, int h, int fps)
void on_chip_calib_manager::start_viewer(int w, int h, int fps, invoker invoke)
{
try
{
Expand Down Expand Up @@ -177,15 +180,17 @@ namespace rs2

auto profiles = _sub->get_selected_profiles();

if (!_model.dev_syncer)
_model.dev_syncer = _viewer.syncer->create_syncer();
invoke([&](){
if (!_model.dev_syncer)
_model.dev_syncer = _viewer.syncer->create_syncer();

// Start streaming
_sub->play(profiles, _viewer, _model.dev_syncer);
for (auto&& profile : profiles)
{
_viewer.begin_stream(_sub, profile);
}
// Start streaming
_sub->play(profiles, _viewer, _model.dev_syncer);
for (auto&& profile : profiles)
{
_viewer.begin_stream(_sub, profile);
}
});

// Wait for frames to arrive
bool frame_arrived = false;
Expand Down Expand Up @@ -213,11 +218,11 @@ namespace rs2
return _metrics[use_new ? 1 : 0];
}

std::pair<float, float> on_chip_calib_manager::get_depth_metrics()
std::pair<float, float> on_chip_calib_manager::get_depth_metrics(invoker invoke)
{
using namespace depth_quality;

auto f = fetch_depth_frame();
auto f = fetch_depth_frame(invoke);
auto sensor = _sub->s->as<rs2::depth_stereo_sensor>();
auto intr = f.get_profile().as<rs2::video_stream_profile>().get_intrinsics();
rs2::region_of_interest roi { (int)(f.get_width() * 0.45f), (int)(f.get_height() * 0.45f),
Expand Down Expand Up @@ -301,7 +306,7 @@ namespace rs2

for (int i = 0; i < 31; i++)
{
f = fetch_depth_frame();
f = fetch_depth_frame(invoke);
auto res = depth_quality::analyze_depth_image(f, sensor.get_depth_scale(), sensor.get_stereo_baseline(),
&intr, roi, 0, true, v, false, on_frame);

Expand Down Expand Up @@ -533,7 +538,8 @@ namespace rs2
}
}

void on_chip_calib_manager::process_flow(std::function<void()> cleanup)
void on_chip_calib_manager::process_flow(std::function<void()> cleanup,
invoker invoke)
{
update_last_used();

Expand Down Expand Up @@ -568,19 +574,19 @@ namespace rs2

if (!_was_streaming)
{
start_viewer(0,0,0);
start_viewer(0,0,0, invoke);
}

// Capture metrics before
auto metrics_before = get_depth_metrics();
auto metrics_before = get_depth_metrics(invoke);
_metrics.push_back(metrics_before);

stop_viewer();
stop_viewer(invoke);

_ui = std::make_shared<subdevice_ui_selection>(_sub->ui);

// Switch into special Auto-Calibration mode
start_viewer(256, 144, 90);
start_viewer(256, 144, 90, invoke);

calibrate();

Expand Down Expand Up @@ -614,23 +620,23 @@ namespace rs2

log(to_string() << "Calibration completed, health factor = " << _health);

stop_viewer();
stop_viewer(invoke);

start_viewer(0, 0, 0); // Start with default settings
start_viewer(0, 0, 0, invoke); // Start with default settings

// Make new calibration active
apply_calib(true);

// Capture metrics after
auto metrics_after = get_depth_metrics();
auto metrics_after = get_depth_metrics(invoke);
_metrics.push_back(metrics_after);

_progress = 100;

_done = true;
}

void on_chip_calib_manager::restore_workspace()
void on_chip_calib_manager::restore_workspace(invoker invoke)
{
try
{
Expand All @@ -640,7 +646,7 @@ namespace rs2

_viewer.synchronization_enable = _synchronized;

stop_viewer();
stop_viewer(invoke);

if (_ui.get())
{
Expand All @@ -652,7 +658,7 @@ namespace rs2

std::this_thread::sleep_for(std::chrono::milliseconds(200));

if (_was_streaming) start_viewer(0, 0, 0);
if (_was_streaming) start_viewer(0, 0, 0, invoke);

_restored = true;
}
Expand Down Expand Up @@ -818,7 +824,7 @@ namespace rs2
ImGui::SetCursorScreenPos({ float(x + 5), float(y + height - 25) });
if (ImGui::Button(button_name.c_str(), { float(bar_width), 20.f }))
{
get_manager().restore_workspace();
get_manager().restore_workspace([this](std::function<void()> a){ a(); });
get_manager().reset();
get_manager().tare = true;
get_manager().start(shared_from_this());
Expand Down Expand Up @@ -847,7 +853,7 @@ namespace rs2
ImGui::SetCursorScreenPos({ float(x + 5), float(y + height - 25) });
if (ImGui::Button(button_name.c_str(), { float(bar_width), 20.f }))
{
get_manager().restore_workspace();
get_manager().restore_workspace([this](std::function<void()> a){ a(); });
get_manager().reset();
get_manager().start(shared_from_this());
update_state = RS2_CALIB_STATE_CALIB_IN_PROCESS;
Expand Down Expand Up @@ -997,7 +1003,7 @@ namespace rs2
}
else dismiss(false);

get_manager().restore_workspace();
get_manager().restore_workspace([this](std::function<void()> a){ a(); });
}

if (recommend_keep) ImGui::PopStyleColor(2);
Expand Down Expand Up @@ -1096,7 +1102,7 @@ namespace rs2
if (!use_new_calib && get_manager().done())
get_manager().apply_calib(false);

get_manager().restore_workspace();
get_manager().restore_workspace([this](std::function<void()> a){ a(); });

if (update_state != RS2_CALIB_STATE_TARE_INPUT)
update_state = RS2_CALIB_STATE_INITIAL_PROMPT;
Expand Down
13 changes: 7 additions & 6 deletions common/on-chip-calib.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,6 +4,7 @@
#pragma once

#include "notifications.h"
#include "../src/concurrency.h"

#include <random>

Expand Down Expand Up @@ -38,7 +39,7 @@ namespace rs2
void keep();

// Restore Viewer UI to how it was before auto-calib
void restore_workspace();
void restore_workspace(invoker invoke);

// Ask the firmware to use one of the before/after calibration tables
void apply_calib(bool old);
Expand All @@ -60,11 +61,11 @@ namespace rs2

std::vector<uint8_t> safe_send_command(const std::vector<uint8_t>& cmd, const std::string& name);

rs2::depth_frame fetch_depth_frame();
rs2::depth_frame fetch_depth_frame(invoker invoke);

std::pair<float, float> get_depth_metrics();
std::pair<float, float> get_depth_metrics(invoker invoke);

void process_flow(std::function<void()> cleanup) override;
void process_flow(std::function<void()> cleanup, invoker invoke) override;

float _health = 0.f;
int _speed = 4;
Expand All @@ -84,8 +85,8 @@ namespace rs2

bool _restored = true;

void stop_viewer();
void start_viewer(int w, int h, int fps);
void stop_viewer(invoker invoke);
void start_viewer(int w, int h, int fps, invoker invoke);
};

// Auto-calib notification model is managing the UI state-machine
Expand Down

0 comments on commit 69be34c

Please sign in to comment.