mirror of
https://github.com/legop3/MultiRoombaRover.git
synced 2026-09-16 09:31:20 -04:00
kinect stuff v1
This commit is contained in:
@@ -0,0 +1,251 @@
|
||||
// Kinect Hardware Bridge
|
||||
// Purpose: Owns the persistent native Kinect worker process and converts worker output into Socket.IO-ready payloads.
|
||||
// Scope: Keeps libfreenect process management isolated from auth, cooldown, and browser delivery.
|
||||
const { spawn } = require('child_process');
|
||||
const path = require('path');
|
||||
const sharp = require('sharp');
|
||||
|
||||
const WORKER_PATH = process.env.KINECT_WORKER || path.join(__dirname, 'native', 'kinect_worker');
|
||||
const CAPTURE_TIMEOUT_MS = 12000;
|
||||
const WORKER_STDERR_LOG_INTERVAL_MS = 5000;
|
||||
|
||||
let worker = null;
|
||||
let stdoutBuffer = Buffer.alloc(0);
|
||||
let pending = null;
|
||||
let commandChain = Promise.resolve();
|
||||
let nextCommandId = 1;
|
||||
let lastWorkerStderr = '';
|
||||
let lastWorkerStderrLogAt = 0;
|
||||
let suppressedWorkerStderr = 0;
|
||||
|
||||
function resetWorker(child = worker) {
|
||||
// Child process events can arrive after a replacement worker has already
|
||||
// started. Only clear module state when the event belongs to the current
|
||||
// process so a stale close event cannot tear down the live stream.
|
||||
if (child && worker && child !== worker) return;
|
||||
worker = null;
|
||||
stdoutBuffer = Buffer.alloc(0);
|
||||
pending = null;
|
||||
}
|
||||
|
||||
function rejectPending(err) {
|
||||
if (!pending) return;
|
||||
const current = pending;
|
||||
pending = null;
|
||||
clearTimeout(current.timeout);
|
||||
current.reject(err);
|
||||
}
|
||||
|
||||
function stopWorker() {
|
||||
if (!worker) return;
|
||||
const child = worker;
|
||||
resetWorker();
|
||||
child.kill('SIGTERM');
|
||||
}
|
||||
|
||||
function parseWorkerStdout() {
|
||||
if (!pending) return;
|
||||
|
||||
while (pending) {
|
||||
if (!pending.meta) {
|
||||
const newline = stdoutBuffer.indexOf(0x0a);
|
||||
if (newline === -1) return;
|
||||
try {
|
||||
pending.meta = JSON.parse(stdoutBuffer.slice(0, newline).toString('utf8'));
|
||||
} catch (err) {
|
||||
rejectPending(new Error(`kinect worker returned invalid metadata: ${err.message}`));
|
||||
return;
|
||||
}
|
||||
stdoutBuffer = stdoutBuffer.slice(newline + 1);
|
||||
}
|
||||
|
||||
const payloadBytes = Number(pending.meta.payloadBytes) || 0;
|
||||
if (stdoutBuffer.length < payloadBytes) return;
|
||||
const payload = stdoutBuffer.slice(0, payloadBytes);
|
||||
stdoutBuffer = stdoutBuffer.slice(payloadBytes);
|
||||
|
||||
const current = pending;
|
||||
pending = null;
|
||||
clearTimeout(current.timeout);
|
||||
if (current.meta.id !== current.id) {
|
||||
current.reject(new Error('kinect worker response id mismatch'));
|
||||
} else if (!current.meta.ok) {
|
||||
current.reject(new Error(current.meta.error || 'kinect worker failed'));
|
||||
} else {
|
||||
current.resolve({ meta: current.meta, payload });
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
function ensureWorker() {
|
||||
if (worker && !worker.killed) {
|
||||
return worker;
|
||||
}
|
||||
|
||||
lastWorkerStderr = '';
|
||||
const child = spawn(WORKER_PATH, [], {
|
||||
stdio: ['pipe', 'pipe', 'pipe'],
|
||||
});
|
||||
worker = child;
|
||||
stdoutBuffer = Buffer.alloc(0);
|
||||
|
||||
child.stdout.on('data', (chunk) => {
|
||||
stdoutBuffer = Buffer.concat([stdoutBuffer, chunk]);
|
||||
parseWorkerStdout();
|
||||
});
|
||||
|
||||
child.stderr.on('data', (chunk) => {
|
||||
// libfreenect logs diagnostic USB details to stderr. Keep those details
|
||||
// available for the eventual close error, but throttle console output
|
||||
// because packet-loss messages can become repetitive on Kinect v1 hardware.
|
||||
const text = chunk.toString('utf8').trim();
|
||||
if (!text) return;
|
||||
lastWorkerStderr = text.split('\n').filter(Boolean).slice(-1)[0] || text;
|
||||
const now = Date.now();
|
||||
if (now - lastWorkerStderrLogAt >= WORKER_STDERR_LOG_INTERVAL_MS) {
|
||||
const suffix = suppressedWorkerStderr
|
||||
? ` (${suppressedWorkerStderr} similar worker stderr messages suppressed)`
|
||||
: '';
|
||||
console.warn('[kinect worker]', `${text}${suffix}`);
|
||||
lastWorkerStderrLogAt = now;
|
||||
suppressedWorkerStderr = 0;
|
||||
} else {
|
||||
suppressedWorkerStderr += 1;
|
||||
}
|
||||
});
|
||||
|
||||
child.on('error', (err) => {
|
||||
rejectPending(err);
|
||||
resetWorker(child);
|
||||
});
|
||||
|
||||
child.on('close', (code, signal) => {
|
||||
const reason = signal || code;
|
||||
const detail = lastWorkerStderr ? `: ${lastWorkerStderr}` : '';
|
||||
rejectPending(new Error(`kinect worker exited (${reason})${detail}`));
|
||||
resetWorker(child);
|
||||
});
|
||||
|
||||
return child;
|
||||
}
|
||||
|
||||
function startWorker() {
|
||||
ensureWorker();
|
||||
}
|
||||
|
||||
function sendWorkerCommand(command, timeoutMs) {
|
||||
return new Promise((resolve, reject) => {
|
||||
const child = ensureWorker();
|
||||
const id = nextCommandId;
|
||||
nextCommandId += 1;
|
||||
|
||||
// The native worker frames stdout as "one response for one command", so
|
||||
// keeping only one in-flight command prevents interleaved binary payloads
|
||||
// and also avoids overlapping expensive point-cloud serialization.
|
||||
if (pending) {
|
||||
reject(new Error('kinect worker command already running'));
|
||||
return;
|
||||
}
|
||||
|
||||
const timeout = setTimeout(() => {
|
||||
rejectPending(new Error('kinect worker timed out'));
|
||||
stopWorker();
|
||||
}, timeoutMs);
|
||||
|
||||
pending = {
|
||||
id,
|
||||
meta: null,
|
||||
timeout,
|
||||
resolve,
|
||||
reject,
|
||||
};
|
||||
|
||||
try {
|
||||
child.stdin.write(`${JSON.stringify({ id, ...command })}\n`);
|
||||
} catch (err) {
|
||||
rejectPending(err);
|
||||
stopWorker();
|
||||
}
|
||||
});
|
||||
}
|
||||
|
||||
function queueWorkerCommand(command, timeoutMs) {
|
||||
commandChain = commandChain
|
||||
.catch(() => {})
|
||||
.then(() => sendWorkerCommand(command, timeoutMs));
|
||||
return commandChain;
|
||||
}
|
||||
|
||||
async function captureColorImage() {
|
||||
const { meta, payload } = await queueWorkerCommand({ mode: 'color' }, CAPTURE_TIMEOUT_MS);
|
||||
const jpeg = await sharp(payload, {
|
||||
raw: {
|
||||
width: meta.width,
|
||||
height: meta.height,
|
||||
channels: 3,
|
||||
},
|
||||
})
|
||||
.jpeg({ quality: 90 })
|
||||
.toBuffer();
|
||||
return {
|
||||
meta: {
|
||||
width: meta.width,
|
||||
height: meta.height,
|
||||
format: 'jpeg',
|
||||
frameAgeMs: meta.frameAgeMs,
|
||||
},
|
||||
buffer: jpeg,
|
||||
};
|
||||
}
|
||||
|
||||
async function capturePointCloud() {
|
||||
const { meta, payload } = await queueWorkerCommand({ mode: 'pointcloud' }, CAPTURE_TIMEOUT_MS);
|
||||
return {
|
||||
meta: {
|
||||
width: meta.width,
|
||||
height: meta.height,
|
||||
pointCount: meta.pointCount,
|
||||
format: meta.format,
|
||||
strideBytes: 16,
|
||||
rgbFrameAgeMs: meta.rgbFrameAgeMs,
|
||||
depthFrameAgeMs: meta.depthFrameAgeMs,
|
||||
},
|
||||
buffer: payload,
|
||||
};
|
||||
}
|
||||
|
||||
async function getWorkerStatus() {
|
||||
const { meta } = await queueWorkerCommand({ mode: 'status' }, CAPTURE_TIMEOUT_MS);
|
||||
return {
|
||||
hasRgb: Boolean(meta.hasRgb),
|
||||
hasDepth: Boolean(meta.hasDepth),
|
||||
rgbFrames: Number(meta.rgbFrames) || 0,
|
||||
depthFrames: Number(meta.depthFrames) || 0,
|
||||
validDepthPixels: Number(meta.validDepthPixels) || 0,
|
||||
rgbFrameAgeMs: meta.rgbFrameAgeMs ?? null,
|
||||
depthFrameAgeMs: meta.depthFrameAgeMs ?? null,
|
||||
};
|
||||
}
|
||||
|
||||
function installShutdownHooks() {
|
||||
const shutdown = () => stopWorker();
|
||||
process.once('exit', shutdown);
|
||||
process.once('SIGINT', () => {
|
||||
shutdown();
|
||||
process.exit(130);
|
||||
});
|
||||
process.once('SIGTERM', () => {
|
||||
shutdown();
|
||||
process.exit(143);
|
||||
});
|
||||
}
|
||||
|
||||
installShutdownHooks();
|
||||
|
||||
module.exports = {
|
||||
startWorker,
|
||||
stopWorker,
|
||||
captureColorImage,
|
||||
capturePointCloud,
|
||||
getWorkerStatus,
|
||||
};
|
||||
@@ -0,0 +1,17 @@
|
||||
// Kinect Service
|
||||
// Purpose: Composes Kinect hardware capture and browser socket delivery.
|
||||
// Scope: Exposes session-readable state while keeping startup side effects in this service folder.
|
||||
const { loadConfig } = require('../../helpers/configLoader');
|
||||
const hardware = require('./hardware');
|
||||
const { registerKinectSocketGateway, kinectEvents } = require('./socketGateway');
|
||||
|
||||
const config = loadConfig();
|
||||
const gateway = registerKinectSocketGateway({
|
||||
config,
|
||||
hardware,
|
||||
});
|
||||
|
||||
module.exports = {
|
||||
getState: gateway.getState,
|
||||
kinectEvents,
|
||||
};
|
||||
@@ -0,0 +1,3 @@
|
||||
# The native Kinect worker is built on the target Fedora server because it links
|
||||
# against the locally installed libfreenect/libusb packages.
|
||||
/kinect_worker
|
||||
@@ -0,0 +1,30 @@
|
||||
CXX ?= g++
|
||||
PKG_CONFIG ?= pkg-config
|
||||
|
||||
# Build the production Kinect worker from native C++ so it uses the same
|
||||
# libfreenect callback/event path that the standalone probe proved reliable.
|
||||
CXXFLAGS ?= -O2 -std=c++17 -Wall -Wextra -pedantic
|
||||
CPPFLAGS += $(shell $(PKG_CONFIG) --cflags libfreenect 2>/dev/null)
|
||||
LDLIBS += $(shell $(PKG_CONFIG) --libs libfreenect 2>/dev/null) -pthread
|
||||
|
||||
# Fedora installations do not always provide libfreenect.pc, so keep explicit
|
||||
# fallback paths for the package layout installed by install_server.sh.
|
||||
ifeq ($(strip $(CPPFLAGS)),)
|
||||
CPPFLAGS += -I/usr/include/libfreenect -I/usr/include/libusb-1.0
|
||||
endif
|
||||
ifeq ($(filter -lfreenect,$(LDLIBS)),)
|
||||
LDLIBS += -lfreenect
|
||||
endif
|
||||
|
||||
TARGET := kinect_worker
|
||||
SRC := kinect_worker.cpp
|
||||
|
||||
.PHONY: all clean
|
||||
|
||||
all: $(TARGET)
|
||||
|
||||
$(TARGET): $(SRC)
|
||||
$(CXX) $(CXXFLAGS) $(CPPFLAGS) -o $@ $< $(LDLIBS)
|
||||
|
||||
clean:
|
||||
rm -f $(TARGET)
|
||||
@@ -0,0 +1,441 @@
|
||||
// Kinect native worker.
|
||||
//
|
||||
// Purpose:
|
||||
// Keep the proven libfreenect camera/depth callback path running in one native
|
||||
// process and answer snapshot commands from Node. Node owns auth, cooldowns,
|
||||
// JPEG encoding, and Socket.IO fan-out; this worker owns only USB streaming and
|
||||
// binary frame extraction.
|
||||
//
|
||||
// Protocol:
|
||||
// stdin receives one JSON command per line, for example {"id":1,"mode":"color"}.
|
||||
// stdout returns one JSON metadata line followed by payloadBytes raw bytes. All
|
||||
// diagnostics go to stderr so libfreenect logs can never corrupt binary frames.
|
||||
|
||||
#include <libfreenect.h>
|
||||
|
||||
#include <atomic>
|
||||
#include <chrono>
|
||||
#include <condition_variable>
|
||||
#include <cstdint>
|
||||
#include <cstdio>
|
||||
#include <cstdlib>
|
||||
#include <cstring>
|
||||
#include <iomanip>
|
||||
#include <iostream>
|
||||
#include <mutex>
|
||||
#include <sstream>
|
||||
#include <string>
|
||||
#include <sys/time.h>
|
||||
#include <thread>
|
||||
#include <vector>
|
||||
|
||||
namespace {
|
||||
|
||||
constexpr int kWidth = 640;
|
||||
constexpr int kHeight = 480;
|
||||
constexpr int kRgbBytes = kWidth * kHeight * 3;
|
||||
constexpr int kDepthPixels = kWidth * kHeight;
|
||||
constexpr int kFrameStaleMs = 5000;
|
||||
constexpr int kCommandFrameWaitMs = 3000;
|
||||
|
||||
struct FrameCache {
|
||||
std::mutex mutex;
|
||||
std::condition_variable cv;
|
||||
std::vector<uint8_t> rgb = std::vector<uint8_t>(kRgbBytes);
|
||||
std::vector<uint16_t> depth = std::vector<uint16_t>(kDepthPixels);
|
||||
bool has_rgb = false;
|
||||
bool has_depth = false;
|
||||
uint32_t valid_depth_pixels = 0;
|
||||
uint64_t rgb_at_ms = 0;
|
||||
uint64_t depth_at_ms = 0;
|
||||
uint64_t rgb_frames = 0;
|
||||
uint64_t depth_frames = 0;
|
||||
};
|
||||
|
||||
FrameCache cache;
|
||||
std::atomic<bool> running{true};
|
||||
freenect_context* freenect_ctx = nullptr;
|
||||
freenect_device* freenect_dev = nullptr;
|
||||
|
||||
// libfreenect's video callback expects us to hand back a replacement buffer.
|
||||
// The cache receives its own copy, so this buffer can be reused solely for USB
|
||||
// streaming without Node ever reading from memory libfreenect still owns.
|
||||
std::vector<uint8_t> video_back_buffer(kRgbBytes);
|
||||
|
||||
uint64_t now_ms() {
|
||||
using namespace std::chrono;
|
||||
return duration_cast<milliseconds>(steady_clock::now().time_since_epoch()).count();
|
||||
}
|
||||
|
||||
void log_step(const std::string& message) {
|
||||
std::cerr << "[kinect-worker] " << message << "\n";
|
||||
}
|
||||
|
||||
std::string json_escape(const std::string& value) {
|
||||
std::ostringstream out;
|
||||
for (char ch : value) {
|
||||
switch (ch) {
|
||||
case '\\':
|
||||
out << "\\\\";
|
||||
break;
|
||||
case '"':
|
||||
out << "\\\"";
|
||||
break;
|
||||
case '\n':
|
||||
out << "\\n";
|
||||
break;
|
||||
case '\r':
|
||||
out << "\\r";
|
||||
break;
|
||||
case '\t':
|
||||
out << "\\t";
|
||||
break;
|
||||
default:
|
||||
if (static_cast<unsigned char>(ch) < 0x20) {
|
||||
out << "\\u" << std::hex << std::setw(4) << std::setfill('0')
|
||||
<< static_cast<int>(static_cast<unsigned char>(ch));
|
||||
} else {
|
||||
out << ch;
|
||||
}
|
||||
break;
|
||||
}
|
||||
}
|
||||
return out.str();
|
||||
}
|
||||
|
||||
int parse_id(const std::string& line) {
|
||||
const std::string key = "\"id\"";
|
||||
const auto key_pos = line.find(key);
|
||||
if (key_pos == std::string::npos) return 0;
|
||||
const auto colon = line.find(':', key_pos + key.size());
|
||||
if (colon == std::string::npos) return 0;
|
||||
std::size_t pos = colon + 1;
|
||||
while (pos < line.size() && (line[pos] == ' ' || line[pos] == '\t')) pos += 1;
|
||||
return std::atoi(line.c_str() + pos);
|
||||
}
|
||||
|
||||
std::string parse_mode(const std::string& line) {
|
||||
const std::string key = "\"mode\"";
|
||||
const auto key_pos = line.find(key);
|
||||
if (key_pos == std::string::npos) return "";
|
||||
const auto colon = line.find(':', key_pos + key.size());
|
||||
if (colon == std::string::npos) return "";
|
||||
const auto first_quote = line.find('"', colon + 1);
|
||||
if (first_quote == std::string::npos) return "";
|
||||
const auto second_quote = line.find('"', first_quote + 1);
|
||||
if (second_quote == std::string::npos) return "";
|
||||
return line.substr(first_quote + 1, second_quote - first_quote - 1);
|
||||
}
|
||||
|
||||
void write_packet(int id, const std::string& meta_fields, const std::vector<uint8_t>& payload) {
|
||||
std::cout << "{\"id\":" << id << ",\"ok\":true" << meta_fields
|
||||
<< ",\"payloadBytes\":" << payload.size() << "}\n";
|
||||
std::cout.flush();
|
||||
if (!payload.empty()) {
|
||||
std::cout.write(reinterpret_cast<const char*>(payload.data()), static_cast<std::streamsize>(payload.size()));
|
||||
std::cout.flush();
|
||||
}
|
||||
}
|
||||
|
||||
void write_error(int id, const std::string& message) {
|
||||
std::cout << "{\"id\":" << id << ",\"ok\":false,\"error\":\""
|
||||
<< json_escape(message) << "\",\"payloadBytes\":0}\n";
|
||||
std::cout.flush();
|
||||
}
|
||||
|
||||
void depth_callback(freenect_device*, void* depth_data, uint32_t) {
|
||||
const auto* depth = static_cast<const uint16_t*>(depth_data);
|
||||
std::lock_guard<std::mutex> lock(cache.mutex);
|
||||
std::memcpy(cache.depth.data(), depth, kDepthPixels * sizeof(uint16_t));
|
||||
uint32_t valid_depth_pixels = 0;
|
||||
for (int index = 0; index < kDepthPixels; index += 1) {
|
||||
if (depth[index] != 0) valid_depth_pixels += 1;
|
||||
}
|
||||
cache.has_depth = true;
|
||||
cache.valid_depth_pixels = valid_depth_pixels;
|
||||
cache.depth_at_ms = now_ms();
|
||||
cache.depth_frames += 1;
|
||||
cache.cv.notify_all();
|
||||
}
|
||||
|
||||
void video_callback(freenect_device* device, void* rgb_data, uint32_t) {
|
||||
const auto* rgb = static_cast<const uint8_t*>(rgb_data);
|
||||
std::lock_guard<std::mutex> lock(cache.mutex);
|
||||
std::memcpy(cache.rgb.data(), rgb, kRgbBytes);
|
||||
cache.has_rgb = true;
|
||||
cache.rgb_at_ms = now_ms();
|
||||
cache.rgb_frames += 1;
|
||||
|
||||
// Hand libfreenect a replacement immediately. Node reads only from the
|
||||
// independent cache copy, which avoids a use-after-callback race.
|
||||
freenect_set_video_buffer(device, video_back_buffer.data());
|
||||
cache.cv.notify_all();
|
||||
}
|
||||
|
||||
bool wait_for_frames(bool need_rgb, bool need_depth, std::string* error) {
|
||||
std::unique_lock<std::mutex> lock(cache.mutex);
|
||||
const auto deadline = std::chrono::steady_clock::now() + std::chrono::milliseconds(kCommandFrameWaitMs);
|
||||
const auto ready = [&]() {
|
||||
const uint64_t now = now_ms();
|
||||
const bool rgb_ok = !need_rgb || (cache.has_rgb && now - cache.rgb_at_ms <= kFrameStaleMs);
|
||||
const bool depth_ok =
|
||||
!need_depth ||
|
||||
(cache.has_depth && cache.valid_depth_pixels > 0 && now - cache.depth_at_ms <= kFrameStaleMs);
|
||||
return rgb_ok && depth_ok;
|
||||
};
|
||||
|
||||
while (!ready()) {
|
||||
if (cache.cv.wait_until(lock, deadline) == std::cv_status::timeout) break;
|
||||
}
|
||||
if (ready()) return true;
|
||||
|
||||
const uint64_t now = now_ms();
|
||||
std::ostringstream msg;
|
||||
msg << "kinect frames unavailable";
|
||||
if (need_rgb) {
|
||||
msg << " rgb=" << (cache.has_rgb ? std::to_string(now - cache.rgb_at_ms) + "ms old" : "missing");
|
||||
}
|
||||
if (need_depth) {
|
||||
msg << " depth="
|
||||
<< (cache.has_depth
|
||||
? std::to_string(now - cache.depth_at_ms) + "ms old valid=" +
|
||||
std::to_string(cache.valid_depth_pixels)
|
||||
: "missing");
|
||||
}
|
||||
*error = msg.str();
|
||||
return false;
|
||||
}
|
||||
|
||||
void handle_color(int id) {
|
||||
std::string error;
|
||||
if (!wait_for_frames(true, false, &error)) {
|
||||
write_error(id, error);
|
||||
return;
|
||||
}
|
||||
|
||||
std::vector<uint8_t> payload;
|
||||
uint64_t age = 0;
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(cache.mutex);
|
||||
payload = cache.rgb;
|
||||
age = now_ms() - cache.rgb_at_ms;
|
||||
}
|
||||
|
||||
std::ostringstream meta;
|
||||
meta << ",\"kind\":\"color\",\"format\":\"rgb24\",\"width\":" << kWidth
|
||||
<< ",\"height\":" << kHeight << ",\"frameAgeMs\":" << age;
|
||||
write_packet(id, meta.str(), payload);
|
||||
}
|
||||
|
||||
void handle_pointcloud(int id) {
|
||||
std::string error;
|
||||
if (!wait_for_frames(true, true, &error)) {
|
||||
write_error(id, error);
|
||||
return;
|
||||
}
|
||||
|
||||
std::vector<uint8_t> rgb;
|
||||
std::vector<uint16_t> depth;
|
||||
uint64_t rgb_age = 0;
|
||||
uint64_t depth_age = 0;
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(cache.mutex);
|
||||
rgb = cache.rgb;
|
||||
depth = cache.depth;
|
||||
const uint64_t now = now_ms();
|
||||
rgb_age = now - cache.rgb_at_ms;
|
||||
depth_age = now - cache.depth_at_ms;
|
||||
}
|
||||
|
||||
std::vector<uint8_t> payload;
|
||||
payload.reserve(kDepthPixels * 16);
|
||||
uint32_t point_count = 0;
|
||||
const float focal_x = 525.0f;
|
||||
const float focal_y = 525.0f;
|
||||
const float center_x = static_cast<float>(kWidth - 1) / 2.0f;
|
||||
const float center_y = static_cast<float>(kHeight - 1) / 2.0f;
|
||||
|
||||
auto append_float = [&](float value) {
|
||||
uint8_t bytes[sizeof(float)];
|
||||
std::memcpy(bytes, &value, sizeof(float));
|
||||
payload.insert(payload.end(), bytes, bytes + sizeof(float));
|
||||
};
|
||||
|
||||
// Registered depth aligns with RGB, so each valid depth pixel can become a
|
||||
// colored point without an additional calibration lookup. Invalid zero-depth
|
||||
// pixels are skipped to keep the payload and browser point count smaller.
|
||||
for (int y = 0; y < kHeight; y += 1) {
|
||||
for (int x = 0; x < kWidth; x += 1) {
|
||||
const int idx = y * kWidth + x;
|
||||
const uint16_t z_mm = depth[idx];
|
||||
if (z_mm == 0) continue;
|
||||
const float z = static_cast<float>(z_mm) / 1000.0f;
|
||||
const float world_x = (static_cast<float>(x) - center_x) * z / focal_x;
|
||||
const float world_y = -(static_cast<float>(y) - center_y) * z / focal_y;
|
||||
append_float(world_x);
|
||||
append_float(world_y);
|
||||
append_float(z);
|
||||
payload.push_back(rgb[idx * 3 + 0]);
|
||||
payload.push_back(rgb[idx * 3 + 1]);
|
||||
payload.push_back(rgb[idx * 3 + 2]);
|
||||
payload.push_back(255);
|
||||
point_count += 1;
|
||||
}
|
||||
}
|
||||
|
||||
std::ostringstream meta;
|
||||
meta << ",\"kind\":\"pointCloud\",\"format\":\"xyzrgb-f32-u8\",\"width\":" << kWidth
|
||||
<< ",\"height\":" << kHeight << ",\"pointCount\":" << point_count
|
||||
<< ",\"rgbFrameAgeMs\":" << rgb_age << ",\"depthFrameAgeMs\":" << depth_age;
|
||||
write_packet(id, meta.str(), payload);
|
||||
}
|
||||
|
||||
void handle_status(int id) {
|
||||
bool has_rgb = false;
|
||||
bool has_depth = false;
|
||||
uint64_t rgb_age = 0;
|
||||
uint64_t depth_age = 0;
|
||||
uint64_t rgb_frames = 0;
|
||||
uint64_t depth_frames = 0;
|
||||
uint32_t valid_depth_pixels = 0;
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(cache.mutex);
|
||||
const uint64_t now = now_ms();
|
||||
has_rgb = cache.has_rgb;
|
||||
has_depth = cache.has_depth;
|
||||
rgb_age = has_rgb ? now - cache.rgb_at_ms : 0;
|
||||
depth_age = has_depth ? now - cache.depth_at_ms : 0;
|
||||
rgb_frames = cache.rgb_frames;
|
||||
depth_frames = cache.depth_frames;
|
||||
valid_depth_pixels = cache.valid_depth_pixels;
|
||||
}
|
||||
|
||||
std::ostringstream meta;
|
||||
meta << ",\"kind\":\"status\",\"hasRgb\":" << (has_rgb ? "true" : "false")
|
||||
<< ",\"hasDepth\":" << (has_depth ? "true" : "false")
|
||||
<< ",\"rgbFrameAgeMs\":" << (has_rgb ? std::to_string(rgb_age) : "null")
|
||||
<< ",\"depthFrameAgeMs\":" << (has_depth ? std::to_string(depth_age) : "null")
|
||||
<< ",\"rgbFrames\":" << rgb_frames << ",\"depthFrames\":" << depth_frames
|
||||
<< ",\"validDepthPixels\":" << valid_depth_pixels;
|
||||
write_packet(id, meta.str(), {});
|
||||
}
|
||||
|
||||
bool init_freenect() {
|
||||
log_step("initializing libfreenect");
|
||||
const int init_result = freenect_init(&freenect_ctx, nullptr);
|
||||
if (init_result < 0) {
|
||||
log_step("freenect_init failed with result " + std::to_string(init_result));
|
||||
return false;
|
||||
}
|
||||
freenect_set_log_level(freenect_ctx, FREENECT_LOG_WARNING);
|
||||
|
||||
// Use only the camera subdevice for this first app integration. The probe
|
||||
// showed the LED/motor sibling can error while camera/depth still works, so
|
||||
// startup should not depend on motor access.
|
||||
freenect_select_subdevices(
|
||||
freenect_ctx,
|
||||
static_cast<freenect_device_flags>(FREENECT_DEVICE_CAMERA));
|
||||
|
||||
const int device_count = freenect_num_devices(freenect_ctx);
|
||||
log_step("device count: " + std::to_string(device_count));
|
||||
if (device_count < 1) {
|
||||
log_step("no kinect devices found");
|
||||
return false;
|
||||
}
|
||||
|
||||
const int open_result = freenect_open_device(freenect_ctx, &freenect_dev, 0);
|
||||
if (open_result < 0) {
|
||||
log_step("freenect_open_device failed with result " + std::to_string(open_result));
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
bool start_streams() {
|
||||
log_step("starting camera/depth streams");
|
||||
freenect_set_depth_callback(freenect_dev, depth_callback);
|
||||
freenect_set_video_callback(freenect_dev, video_callback);
|
||||
freenect_set_video_buffer(freenect_dev, video_back_buffer.data());
|
||||
|
||||
if (freenect_set_video_mode(
|
||||
freenect_dev,
|
||||
freenect_find_video_mode(FREENECT_RESOLUTION_MEDIUM, FREENECT_VIDEO_RGB)) < 0) {
|
||||
log_step("freenect_set_video_mode failed");
|
||||
return false;
|
||||
}
|
||||
if (freenect_set_depth_mode(
|
||||
freenect_dev,
|
||||
freenect_find_depth_mode(FREENECT_RESOLUTION_MEDIUM, FREENECT_DEPTH_REGISTERED)) < 0) {
|
||||
log_step("freenect_set_depth_mode failed");
|
||||
return false;
|
||||
}
|
||||
if (freenect_start_depth(freenect_dev) < 0) {
|
||||
log_step("freenect_start_depth failed");
|
||||
return false;
|
||||
}
|
||||
if (freenect_start_video(freenect_dev) < 0) {
|
||||
log_step("freenect_start_video failed");
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
void event_loop() {
|
||||
while (running) {
|
||||
timeval timeout;
|
||||
timeout.tv_sec = 0;
|
||||
timeout.tv_usec = 100000;
|
||||
const int result = freenect_process_events_timeout(freenect_ctx, &timeout);
|
||||
if (result < 0) {
|
||||
log_step("libfreenect event loop failed with result " + std::to_string(result));
|
||||
running = false;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void shutdown_freenect() {
|
||||
running = false;
|
||||
if (freenect_dev) {
|
||||
freenect_stop_depth(freenect_dev);
|
||||
freenect_stop_video(freenect_dev);
|
||||
freenect_close_device(freenect_dev);
|
||||
freenect_dev = nullptr;
|
||||
}
|
||||
if (freenect_ctx) {
|
||||
freenect_shutdown(freenect_ctx);
|
||||
freenect_ctx = nullptr;
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
int main() {
|
||||
if (!init_freenect() || !start_streams()) {
|
||||
shutdown_freenect();
|
||||
return 1;
|
||||
}
|
||||
|
||||
std::thread worker(event_loop);
|
||||
std::string line;
|
||||
while (running && std::getline(std::cin, line)) {
|
||||
const int id = parse_id(line);
|
||||
const std::string mode = parse_mode(line);
|
||||
if (mode == "color") {
|
||||
handle_color(id);
|
||||
} else if (mode == "pointcloud") {
|
||||
handle_pointcloud(id);
|
||||
} else if (mode == "status") {
|
||||
handle_status(id);
|
||||
} else {
|
||||
write_error(id, "unknown kinect command");
|
||||
}
|
||||
}
|
||||
|
||||
running = false;
|
||||
if (worker.joinable()) {
|
||||
worker.join();
|
||||
}
|
||||
shutdown_freenect();
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,201 @@
|
||||
// Kinect Socket Gateway
|
||||
// Purpose: Registers browser-facing Kinect snapshot controls and broadcasts shared Kinect frames over Socket.IO.
|
||||
// Scope: Owns authorization, global capture cooldown, request serialization, cached-frame replay, and session-status events.
|
||||
const EventEmitter = require('events');
|
||||
const io = require('../../globals/io');
|
||||
const logger = require('../../globals/logger').child('kinectService');
|
||||
const { getMode, MODES } = require('../modeManager');
|
||||
const { isAdmin, isLockdownAdmin, getRole } = require('../roleService');
|
||||
|
||||
const DEFAULT_CAPTURE_COOLDOWN_MS = 10000;
|
||||
|
||||
const kinectEvents = new EventEmitter();
|
||||
|
||||
function passesMode(socket) {
|
||||
const mode = getMode();
|
||||
if (mode === MODES.LOCKDOWN) return isLockdownAdmin(socket);
|
||||
if (mode === MODES.ADMIN) {
|
||||
const role = getRole(socket);
|
||||
return role === 'spectator' || isAdmin(socket);
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
function normalizeKinectConfig(config = {}) {
|
||||
const raw = config.kinect || {};
|
||||
return {
|
||||
enabled: Boolean(raw.enabled),
|
||||
captureCooldownMs:
|
||||
Number.isFinite(Number(raw.captureCooldownMs)) && Number(raw.captureCooldownMs) >= 0
|
||||
? Number(raw.captureCooldownMs)
|
||||
: DEFAULT_CAPTURE_COOLDOWN_MS,
|
||||
};
|
||||
}
|
||||
|
||||
function registerKinectSocketGateway({ config, hardware }) {
|
||||
const settings = normalizeKinectConfig(config);
|
||||
let captureCooldownUntil = 0;
|
||||
let busy = false;
|
||||
let lastAction = null;
|
||||
let lastError = null;
|
||||
let lastPointCloud = null;
|
||||
let lastColorImage = null;
|
||||
|
||||
function buildStatus(extra = {}) {
|
||||
return {
|
||||
enabled: settings.enabled,
|
||||
// Availability starts optimistic when enabled. A real worker/capture
|
||||
// failure changes lastError, and session sync then makes the UI show that
|
||||
// the camera path needs attention.
|
||||
available: settings.enabled && !lastError,
|
||||
busy,
|
||||
captureCooldownUntil,
|
||||
lastAction,
|
||||
lastError,
|
||||
hasPointCloud: Boolean(lastPointCloud?.buffer),
|
||||
hasColorImage: Boolean(lastColorImage?.buffer),
|
||||
lastPointCloudTs: lastPointCloud?.meta?.ts || null,
|
||||
lastColorImageTs: lastColorImage?.meta?.ts || null,
|
||||
...extra,
|
||||
};
|
||||
}
|
||||
|
||||
function emitStatusChange(extra = {}) {
|
||||
// The browser already receives session-wide status through sessionService.
|
||||
// Emitting a local service event keeps Kinect-specific code from importing
|
||||
// sessionService directly and creating a require cycle.
|
||||
kinectEvents.emit('change', buildStatus(extra));
|
||||
}
|
||||
|
||||
function sendCachedFrames(socket) {
|
||||
// Cached-frame replay gives newly opened tabs the latest room snapshot
|
||||
// without starting a new Kinect capture or spending upload continuously.
|
||||
if (lastPointCloud?.buffer) {
|
||||
socket.emit('kinect:pointCloudFrame', lastPointCloud.meta, lastPointCloud.buffer);
|
||||
}
|
||||
if (lastColorImage?.buffer) {
|
||||
socket.emit('kinect:colorFrame', lastColorImage.meta, lastColorImage.buffer);
|
||||
}
|
||||
}
|
||||
|
||||
function rejectDisabled() {
|
||||
if (!settings.enabled) {
|
||||
return { error: 'kinect service is disabled' };
|
||||
}
|
||||
return null;
|
||||
}
|
||||
|
||||
function rejectUnauthorized(socket) {
|
||||
if (!passesMode(socket)) {
|
||||
return { error: 'not authorized for kinect controls' };
|
||||
}
|
||||
return null;
|
||||
}
|
||||
|
||||
function rejectCaptureCooldown() {
|
||||
const now = Date.now();
|
||||
if (captureCooldownUntil > now) {
|
||||
return {
|
||||
error: 'kinect capture cooldown active',
|
||||
retryAfterMs: captureCooldownUntil - now,
|
||||
captureCooldownUntil,
|
||||
};
|
||||
}
|
||||
return null;
|
||||
}
|
||||
|
||||
async function handleCapture(socket, kind, cb) {
|
||||
const disabled = rejectDisabled();
|
||||
const unauthorized = rejectUnauthorized(socket);
|
||||
const cooldown = rejectCaptureCooldown();
|
||||
if (disabled || unauthorized || cooldown) {
|
||||
const response = disabled || unauthorized || cooldown;
|
||||
cb(response);
|
||||
emitStatusChange();
|
||||
return;
|
||||
}
|
||||
|
||||
if (busy) {
|
||||
cb({ error: 'kinect capture already running' });
|
||||
emitStatusChange();
|
||||
return;
|
||||
}
|
||||
|
||||
busy = true;
|
||||
lastAction = kind;
|
||||
lastError = null;
|
||||
// The cooldown starts when the server accepts the request. That makes every
|
||||
// connected browser disable capture controls immediately, instead of waiting
|
||||
// for the worker to finish serializing a multi-megabyte point cloud.
|
||||
captureCooldownUntil = Date.now() + settings.captureCooldownMs;
|
||||
cb({ ok: true, captureCooldownUntil });
|
||||
emitStatusChange();
|
||||
|
||||
try {
|
||||
const capture =
|
||||
kind === 'pointCloud'
|
||||
? await hardware.capturePointCloud()
|
||||
: await hardware.captureColorImage();
|
||||
const meta = {
|
||||
...capture.meta,
|
||||
ts: Date.now(),
|
||||
requestedBy: socket.id,
|
||||
};
|
||||
if (kind === 'pointCloud') {
|
||||
lastPointCloud = { meta, buffer: capture.buffer };
|
||||
io.emit('kinect:pointCloudFrame', meta, capture.buffer);
|
||||
} else {
|
||||
lastColorImage = { meta, buffer: capture.buffer };
|
||||
io.emit('kinect:colorFrame', meta, capture.buffer);
|
||||
}
|
||||
logger.info('Kinect capture broadcast', {
|
||||
kind,
|
||||
bytes: capture.buffer?.length || capture.buffer?.byteLength || 0,
|
||||
socketId: socket.id,
|
||||
});
|
||||
} catch (err) {
|
||||
lastError = err.message || 'kinect capture failed';
|
||||
logger.warn('Kinect capture failed', { kind, socketId: socket.id, err: lastError });
|
||||
} finally {
|
||||
busy = false;
|
||||
emitStatusChange();
|
||||
}
|
||||
}
|
||||
|
||||
io.on('connection', (socket) => {
|
||||
sendCachedFrames(socket);
|
||||
|
||||
socket.on('kinect:requestCachedFrames', (_payload = {}, cb = () => {}) => {
|
||||
sendCachedFrames(socket);
|
||||
cb({ ok: true });
|
||||
});
|
||||
|
||||
socket.on('kinect:requestPointCloud', (_payload = {}, cb = () => {}) => {
|
||||
handleCapture(socket, 'pointCloud', cb);
|
||||
});
|
||||
|
||||
socket.on('kinect:requestColorImage', (_payload = {}, cb = () => {}) => {
|
||||
handleCapture(socket, 'colorImage', cb);
|
||||
});
|
||||
});
|
||||
|
||||
if (settings.enabled) {
|
||||
try {
|
||||
// Starting the worker at service startup gives the callback path time to
|
||||
// warm up, while clients still control when bytes are uploaded to them.
|
||||
hardware.startWorker();
|
||||
} catch (err) {
|
||||
lastError = err.message || 'kinect worker failed to start';
|
||||
logger.warn('Kinect worker startup failed', { err: lastError });
|
||||
}
|
||||
}
|
||||
|
||||
return {
|
||||
getState: buildStatus,
|
||||
};
|
||||
}
|
||||
|
||||
module.exports = {
|
||||
registerKinectSocketGateway,
|
||||
kinectEvents,
|
||||
};
|
||||
Reference in New Issue
Block a user