From 8fbfd1ea0277ea2189750edddd9544c55cd2640f Mon Sep 17 00:00:00 2001 From: Hayrapetyan Grant Sergeevich Date: Wed, 29 Apr 2026 08:31:56 +0300 Subject: [PATCH] feat: replace CSV with SQLite realtime database --- imu_module/imu_reader.py | 88 ++++++---- lidar_module/lidar_reader.py | 103 ++++++----- processing/calculations.cpp | 323 +++++++++++++++-------------------- processing/visualization.py | 142 ++++++++------- run.sh | 34 ++-- 5 files changed, 356 insertions(+), 334 deletions(-) diff --git a/imu_module/imu_reader.py b/imu_module/imu_reader.py index f3ee1d1..aada7ec 100644 --- a/imu_module/imu_reader.py +++ b/imu_module/imu_reader.py @@ -1,68 +1,92 @@ -import smbus2 #библиотека Python для работы с I2C шиной +import smbus2 import time -import csv -import struct +import sqlite3 +import signal +import sys # Константы MPU_ADDRESS = 0x68 -I2C_BUS = 1 +I2C_BUS = 2 +DB_PATH = '../inertial_data.db' # Регистры MPU-9250 -PWR_MGMT_1 = 0x6B # управление питанием -ACCEL_XOUT_H = 0x3B # начало данных акселерометра -GYRO_XOUT_H = 0x43 # начало данных гироскопа +PWR_MGMT_1 = 0x6B +ACCEL_XOUT_H = 0x3B +GYRO_XOUT_H = 0x43 -# Масштабные коэффициенты (диапазон ±2g и ±250°/с) -ACCEL_SCALE = 16384.0 # LSB/g -GYRO_SCALE = 131.0 # LSB/(°/с) +# Масштабные коэффициенты +ACCEL_SCALE = 16384.0 +GYRO_SCALE = 131.0 + +# Глобальные переменные для завершения +running = True +conn = None +bus = None + +def shutdown(signum, frame): + """Обработчик сигнала завершения""" + global running + print("Завершение IMU модуля...") + running = False + +# Перехватываем сигналы завершения +signal.signal(signal.SIGTERM, shutdown) +signal.signal(signal.SIGINT, shutdown) + +def init_db(): + conn = sqlite3.connect(DB_PATH) + conn.execute('''CREATE TABLE IF NOT EXISTS imu_data ( + timestamp REAL, + ax REAL, ay REAL, az REAL, + gx REAL, gy REAL, gz REAL + )''') + conn.execute('DELETE FROM imu_data') + conn.commit() + return conn def init_mpu(bus): - """Разбудить датчик""" bus.write_byte_data(MPU_ADDRESS, PWR_MGMT_1, 0x00) time.sleep(0.1) def read_raw(bus, reg): - """Читать 16-битное значение из двух регистров""" high = bus.read_byte_data(MPU_ADDRESS, reg) low = bus.read_byte_data(MPU_ADDRESS, reg + 1) value = (high << 8) | low - # Перевод в знаковое число if value > 32767: value -= 65536 return value def read_imu(bus): - """Читать и вернуть данные акселерометра и гироскопа""" ax = read_raw(bus, ACCEL_XOUT_H) / ACCEL_SCALE ay = read_raw(bus, ACCEL_XOUT_H + 2) / ACCEL_SCALE az = read_raw(bus, ACCEL_XOUT_H + 4) / ACCEL_SCALE - gx = read_raw(bus, GYRO_XOUT_H) / GYRO_SCALE gy = read_raw(bus, GYRO_XOUT_H + 2) / GYRO_SCALE gz = read_raw(bus, GYRO_XOUT_H + 4) / GYRO_SCALE - return ax, ay, az, gx, gy, gz -def save_to_csv(filename, duration_seconds): - """Записать данные в CSV файл""" +def save_to_db(): + global conn, bus, running + conn = init_db() bus = smbus2.SMBus(I2C_BUS) init_mpu(bus) - with open(filename, 'w', newline='') as f: - writer = csv.writer(f) - writer.writerow(['timestamp', 'ax', 'ay', 'az', 'gx', 'gy', 'gz']) - - t = 0.0 - while t < duration_seconds: + print("Запись данных IMU...") + t = 0.0 + try: + while running: ax, ay, az, gx, gy, gz = read_imu(bus) - writer.writerow([round(t, 3), - round(ax, 4), round(ay, 4), round(az, 4), - round(gx, 4), round(gy, 4), round(gz, 4)]) + conn.execute( + 'INSERT INTO imu_data VALUES (?, ?, ?, ?, ?, ?, ?)', + (round(t, 3), round(ax, 4), round(ay, 4), round(az, 4), + round(gx, 4), round(gy, 4), round(gz, 4)) + ) + conn.commit() t += 0.01 time.sleep(0.01) + finally: + bus.close() + conn.close() + print("IMU модуль остановлен") - bus.close() - -# Запуск — записываем 5 секунд -save_to_csv('imu.csv', 5) -print("Готово!") \ No newline at end of file +save_to_db() \ No newline at end of file diff --git a/lidar_module/lidar_reader.py b/lidar_module/lidar_reader.py index e7c4f81..bfc41e1 100644 --- a/lidar_module/lidar_reader.py +++ b/lidar_module/lidar_reader.py @@ -1,87 +1,104 @@ import serial -import csv import time +import sqlite3 import struct +import signal # Константы -SERIAL_PORT = '/dev/ttyS0' # UART порт на MangoPi -BAUD_RATE = 230400 # скорость передачи LDS02RR -DURATION = 5 # секунд записи +SERIAL_PORT = '/dev/ttyS0' +BAUD_RATE = 230400 +DB_PATH = '../inertial_data.db' # Заголовок пакета LDS02RR HEADER = 0xFA -PACKET_SIZE = 22 # размер пакета в байтах +PACKET_SIZE = 22 + +# Глобальные переменные для завершения +running = True +conn = None +ser = None + +def shutdown(signum, frame): + """Обработчик сигнала завершения""" + global running + print("Завершение лидар модуля...") + running = False + +# Перехватываем сигналы завершения +signal.signal(signal.SIGTERM, shutdown) +signal.signal(signal.SIGINT, shutdown) + +def init_db(): + """Создать таблицу если не существует и очистить""" + conn = sqlite3.connect(DB_PATH) + conn.execute('''CREATE TABLE IF NOT EXISTS lidar_data ( + timestamp REAL, + angle REAL, + distance_mm REAL, + quality INTEGER + )''') + conn.execute('DELETE FROM lidar_data') + conn.commit() + return conn def parse_packet(data): """Парсить один пакет данных лидара""" - if len(data) < PACKET_SIZE: + if len(data) < PACKET_SIZE or data[0] != HEADER: return None - - if data[0] != HEADER: - return None - - # Индекс угла (0xA0 = 0°, 0xF9 = 359°) index = data[1] - 0xA0 if index < 0 or index > 89: return None - - # Скорость вращения - speed = struct.unpack_from('> 8 - - distance = distance_raw / 4.0 # в мм - angle = index * 4 + i # угол в градусах - + quality = struct.unpack_from('> 2 + distance = distance_raw / 4.0 + angle = index * 4 + i points.append((angle, distance, quality)) - return points -def save_to_csv(filename, duration_seconds): - """Читать данные лидара и записывать в CSV""" +def save_to_db(): + """Основной цикл записи данных в БД""" + global conn, ser, running + conn = init_db() + try: ser = serial.Serial(SERIAL_PORT, BAUD_RATE, timeout=1) - print(f"Подключено к {SERIAL_PORT}") + print("Подключено к лидару") except Exception as e: print(f"Ошибка подключения: {e}") + conn.close() return - with open(filename, 'w', newline='') as f: - writer = csv.writer(f) - writer.writerow(['timestamp', 'angle', 'distance_mm', 'quality']) + print("Запись данных лидара...") + buffer = bytearray() + start_time = time.time() - start_time = time.time() - buffer = bytearray() - - while time.time() - start_time < duration_seconds: + try: + while running: data = ser.read(PACKET_SIZE) if not data: continue - buffer.extend(data) - - # Ищем заголовок пакета while len(buffer) >= PACKET_SIZE: if buffer[0] == HEADER: packet = bytes(buffer[:PACKET_SIZE]) points = parse_packet(packet) - if points: t = round(time.time() - start_time, 3) for angle, distance, quality in points: - writer.writerow([t, angle, - round(distance, 1), - quality]) + conn.execute( + 'INSERT INTO lidar_data VALUES (?, ?, ?, ?)', + (t, angle, round(distance, 1), quality) + ) + conn.commit() buffer = buffer[PACKET_SIZE:] else: buffer.pop(0) + finally: + ser.close() + conn.close() + print("Лидар модуль остановлен") - ser.close() - print("Готово!") - -# Запуск -save_to_csv('lidar.csv', DURATION) \ No newline at end of file +save_to_db() \ No newline at end of file diff --git a/processing/calculations.cpp b/processing/calculations.cpp index c4e700a..e60cb2a 100644 --- a/processing/calculations.cpp +++ b/processing/calculations.cpp @@ -1,26 +1,35 @@ #include -#include -#include #include #include #include +#include +#include +#include +#include #include using namespace std; using namespace Eigen; -// СТРУКТУРЫ +// ===== ФЛАГ ЗАВЕРШЕНИЯ ===== +volatile sig_atomic_t running = 1; + +void shutdown(int signum) { + running = 0; +} + +// ===== СТРУКТУРЫ ===== struct IMUData { double t; - Vector3d acc; // g - Vector3d gyro; // deg/s + Vector3d acc; + Vector3d gyro; }; struct LidarData { double t; double angle; - double distance; // mm + double distance; }; struct State { @@ -28,59 +37,91 @@ struct State { MatrixXd P; }; -// ЧТЕНИЕ CSV +// ===== ЧТЕНИЕ ИЗ SQLite ===== -vector readIMU(const string& filename) { +vector readIMU(sqlite3* db) { vector data; - ifstream file(filename); - string line; + sqlite3_stmt* stmt; - getline(file, line); - - while (getline(file, line)) { - stringstream ss(line); - string val; + const char* sql = "SELECT timestamp, ax, ay, az, gx, gy, gz FROM imu_data ORDER BY timestamp"; + sqlite3_prepare_v2(db, sql, -1, &stmt, nullptr); + while (sqlite3_step(stmt) == SQLITE_ROW) { IMUData d; - - getline(ss, val, ','); d.t = stod(val); - getline(ss, val, ','); d.acc.x() = stod(val); - getline(ss, val, ','); d.acc.y() = stod(val); - getline(ss, val, ','); d.acc.z() = stod(val); - getline(ss, val, ','); d.gyro.x() = stod(val); - getline(ss, val, ','); d.gyro.y() = stod(val); - getline(ss, val, ','); d.gyro.z() = stod(val); - + d.t = sqlite3_column_double(stmt, 0); + d.acc.x() = sqlite3_column_double(stmt, 1); + d.acc.y() = sqlite3_column_double(stmt, 2); + d.acc.z() = sqlite3_column_double(stmt, 3); + d.gyro.x() = sqlite3_column_double(stmt, 4); + d.gyro.y() = sqlite3_column_double(stmt, 5); + d.gyro.z() = sqlite3_column_double(stmt, 6); data.push_back(d); } + sqlite3_finalize(stmt); return data; } -vector readLidar(const string& filename) { +vector readLidar(sqlite3* db) { vector data; - ifstream file(filename); - string line; + sqlite3_stmt* stmt; - getline(file, line); - - while (getline(file, line)) { - stringstream ss(line); - string val; + const char* sql = "SELECT timestamp, angle, distance_mm FROM lidar_data ORDER BY timestamp"; + sqlite3_prepare_v2(db, sql, -1, &stmt, nullptr); + while (sqlite3_step(stmt) == SQLITE_ROW) { LidarData d; - - getline(ss, val, ','); d.t = stod(val); - getline(ss, val, ','); d.angle = stod(val); - getline(ss, val, ','); d.distance = stod(val); - + d.t = sqlite3_column_double(stmt, 0); + d.angle = sqlite3_column_double(stmt, 1); + d.distance = sqlite3_column_double(stmt, 2); data.push_back(d); } + sqlite3_finalize(stmt); return data; } -// ПОВОРОТ +// ===== ЗАПИСЬ В SQLite ===== + +void initDB(sqlite3* db) { + const char* sql = + "CREATE TABLE IF NOT EXISTS trajectory (" + "timestamp REAL, x REAL, y REAL, z REAL," + "roll REAL, pitch REAL, yaw REAL);" + "DELETE FROM trajectory;" + "CREATE TABLE IF NOT EXISTS lidar_points (" + "x REAL, y REAL, z REAL);" + "DELETE FROM lidar_points;"; + sqlite3_exec(db, sql, nullptr, nullptr, nullptr); +} + +void writeTraj(sqlite3* db, double t, Vector3d pos, Vector3d ang) { + sqlite3_stmt* stmt; + const char* sql = "INSERT INTO trajectory VALUES (?,?,?,?,?,?,?)"; + sqlite3_prepare_v2(db, sql, -1, &stmt, nullptr); + sqlite3_bind_double(stmt, 1, t); + sqlite3_bind_double(stmt, 2, pos.x()); + sqlite3_bind_double(stmt, 3, pos.y()); + sqlite3_bind_double(stmt, 4, pos.z()); + sqlite3_bind_double(stmt, 5, ang.x()); + sqlite3_bind_double(stmt, 6, ang.y()); + sqlite3_bind_double(stmt, 7, ang.z()); + sqlite3_step(stmt); + sqlite3_finalize(stmt); +} + +void writeLidarPoint(sqlite3* db, Vector3d p) { + sqlite3_stmt* stmt; + const char* sql = "INSERT INTO lidar_points VALUES (?,?,?)"; + sqlite3_prepare_v2(db, sql, -1, &stmt, nullptr); + sqlite3_bind_double(stmt, 1, p.x()); + sqlite3_bind_double(stmt, 2, p.y()); + sqlite3_bind_double(stmt, 3, p.z()); + sqlite3_step(stmt); + sqlite3_finalize(stmt); +} + +// ===== ФИЗИКА (без изменений) ===== Matrix3d rotationMatrix(double roll, double pitch, double yaw) { AngleAxisd rx(roll, Vector3d::UnitX()); @@ -89,11 +130,8 @@ Matrix3d rotationMatrix(double roll, double pitch, double yaw) { return (rz * ry * rx).toRotationMatrix(); } -// IMU PREDICT - void predict(State& s, const IMUData& imu, double dt) { VectorXd& x = s.x; - Vector3d pos = x.segment<3>(0); Vector3d vel = x.segment<3>(3); Vector3d angles = x.segment<3>(6); @@ -102,7 +140,6 @@ void predict(State& s, const IMUData& imu, double dt) { Vector3d gyro = imu.gyro * M_PI / 180.0; Matrix3d R = rotationMatrix(angles[0], angles[1], angles[2]); - Vector3d g(0, 0, 9.81); Vector3d acc_world = R * acc - g; @@ -116,245 +153,167 @@ void predict(State& s, const IMUData& imu, double dt) { MatrixXd F = MatrixXd::Identity(15, 15); F.block<3, 3>(0, 3) = Matrix3d::Identity() * dt; - MatrixXd Q = MatrixXd::Identity(15, 15) * 0.01; - s.P = F * s.P * F.transpose() + Q; } -// ЛИДАР → МИР - Vector3d lidarToWorld(const State& s, const LidarData& l) { double angle = l.angle * M_PI / 180.0; double dist = l.distance / 1000.0; - Vector3d local; - local << dist * cos(angle), dist* sin(angle), 0; - + local << dist * cos(angle), dist * sin(angle), 0; Vector3d pos = s.x.segment<3>(0); Vector3d ang = s.x.segment<3>(6); - Matrix3d R = rotationMatrix(ang[0], ang[1], ang[2]); - return pos + R * local; } -// KD-TREE +// ===== ICP (без изменений) ===== struct KDNode { Vector3d point; KDNode* left; KDNode* right; int axis; - KDNode(Vector3d p, int ax) : point(p), left(nullptr), right(nullptr), axis(ax) {} }; KDNode* buildKDTree(vector& pts, int depth = 0) { if (pts.empty()) return nullptr; - int axis = depth % 3; - sort(pts.begin(), pts.end(), - [axis](const Vector3d& a, const Vector3d& b) { - return a[axis] < b[axis]; - }); - + [axis](const Vector3d& a, const Vector3d& b) { return a[axis] < b[axis]; }); int mid = pts.size() / 2; - KDNode* node = new KDNode(pts[mid], axis); - vector left(pts.begin(), pts.begin() + mid); vector right(pts.begin() + mid + 1, pts.end()); - node->left = buildKDTree(left, depth + 1); node->right = buildKDTree(right, depth + 1); - return node; } void nearestSearch(KDNode* node, const Vector3d& target, Vector3d& best, double& best_dist) { if (!node) return; - double d = (node->point - target).squaredNorm(); - if (d < best_dist) { - best_dist = d; - best = node->point; - } - + if (d < best_dist) { best_dist = d; best = node->point; } int axis = node->axis; - KDNode* near = target[axis] < node->point[axis] ? node->left : node->right; KDNode* far = target[axis] < node->point[axis] ? node->right : node->left; - nearestSearch(near, target, best, best_dist); - double diff = target[axis] - node->point[axis]; - if (diff * diff < best_dist) { - nearestSearch(far, target, best, best_dist); - } + if (diff * diff < best_dist) nearestSearch(far, target, best, best_dist); } -// ICP - -Matrix3d computeRotation(const vector& src, - const vector& dst) { - - Vector3d c1 = Vector3d::Zero(); - Vector3d c2 = Vector3d::Zero(); - - for (size_t i = 0;i < src.size();i++) { - c1 += src[i]; - c2 += dst[i]; - } - - c1 /= src.size(); - c2 /= dst.size(); - +Matrix3d computeRotation(const vector& src, const vector& dst) { + Vector3d c1 = Vector3d::Zero(), c2 = Vector3d::Zero(); + for (size_t i = 0; i < src.size(); i++) { c1 += src[i]; c2 += dst[i]; } + c1 /= src.size(); c2 /= dst.size(); Matrix3d H = Matrix3d::Zero(); - - for (size_t i = 0;i < src.size();i++) { + for (size_t i = 0; i < src.size(); i++) H += (src[i] - c1) * (dst[i] - c2).transpose(); - } - JacobiSVD svd(H, ComputeFullU | ComputeFullV); return svd.matrixV() * svd.matrixU().transpose(); } -Vector3d computeTranslation(const vector& src, - const vector& dst, - const Matrix3d& R) { - - Vector3d c1 = Vector3d::Zero(); - Vector3d c2 = Vector3d::Zero(); - - for (size_t i = 0;i < src.size();i++) { - c1 += src[i]; - c2 += dst[i]; - } - - c1 /= src.size(); - c2 /= dst.size(); - +Vector3d computeTranslation(const vector& src, const vector& dst, const Matrix3d& R) { + Vector3d c1 = Vector3d::Zero(), c2 = Vector3d::Zero(); + for (size_t i = 0; i < src.size(); i++) { c1 += src[i]; c2 += dst[i]; } + c1 /= src.size(); c2 /= dst.size(); return c2 - R * c1; } vector downsample(const vector& pts, int step = 5) { vector out; - for (size_t i = 0;i < pts.size();i += step) + for (size_t i = 0; i < pts.size(); i += step) out.push_back(pts[i]); return out; } -void ICP_fast(vector& src, - vector& dst, - Matrix3d& R, Vector3d& t) { - +void ICP_fast(vector& src, vector& dst, Matrix3d& R, Vector3d& t) { R = Matrix3d::Identity(); t = Vector3d::Zero(); - vector dst_copy = dst; KDNode* tree = buildKDTree(dst_copy); - for (int iter = 0; iter < 10; iter++) { - vector matched; - for (auto& p : src) { - Vector3d best; - double best_dist = 1e9; - + Vector3d best; double best_dist = 1e9; nearestSearch(tree, p, best, best_dist); - - if (best_dist < 0.5) - matched.push_back(best); + if (best_dist < 0.5) matched.push_back(best); } - if (matched.size() < 10) break; - Matrix3d dR = computeRotation(src, matched); Vector3d dt = computeTranslation(src, matched, dR); - - for (auto& p : src) - p = dR * p + dt; - + for (auto& p : src) p = dR * p + dt; R = dR * R; t = dR * t + dt; } } -// MAIN +// ===== MAIN ===== int main() { - auto imu = readIMU("../imu_module/imu.csv"); - auto lidar = readLidar("../lidar_module/lidar.csv"); + signal(SIGTERM, shutdown); + signal(SIGINT, shutdown); + + sqlite3* db; + sqlite3_open("../inertial_data.db", &db); + initDB(db); State state; state.x = VectorXd::Zero(15); state.P = MatrixXd::Identity(15, 15) * 0.1; - ofstream traj("trajectory.csv"); - traj << "timestamp,x,y,z,roll,pitch,yaw\n"; - - ofstream lidar_out("lidar_points.csv"); - lidar_out << "x,y,z\n"; - vector prev_scan, curr_scan; - size_t lidar_idx = 0; + size_t imu_idx = 0; - for (size_t i = 1;i < imu.size();i++) { + cout << "Расчёты запущены..." << endl; - double dt = imu[i].t - imu[i - 1].t; - if (dt <= 0 || dt > 0.1) continue; + while (running) { + auto imu = readIMU(db); + auto lidar = readLidar(db); - predict(state, imu[i], dt); + // Обрабатываем только новые данные + for (size_t i = max((size_t)1, imu_idx); i < imu.size(); i++) { + double dt = imu[i].t - imu[i-1].t; + if (dt <= 0 || dt > 0.1) continue; - while (lidar_idx < lidar.size() && - lidar[lidar_idx].t <= imu[i].t) { + predict(state, imu[i], dt); - curr_scan.push_back(lidarToWorld(state, lidar[lidar_idx])); - lidar_idx++; - } - - if (curr_scan.size() > 200) { - - if (!prev_scan.empty()) { - - vector src = downsample(curr_scan, 5); - vector dst = downsample(prev_scan, 5); - - Matrix3d R; - Vector3d t; - - ICP_fast(src, dst, R, t); - - state.x.segment<3>(0) += t; + while (lidar_idx < lidar.size() && + lidar[lidar_idx].t <= imu[i].t) { + curr_scan.push_back(lidarToWorld(state, lidar[lidar_idx])); + lidar_idx++; } - prev_scan = curr_scan; - curr_scan.clear(); + if (curr_scan.size() > 200) { + if (!prev_scan.empty()) { + vector src = downsample(curr_scan, 5); + vector dst = downsample(prev_scan, 5); + Matrix3d R; Vector3d t; + ICP_fast(src, dst, R, t); + state.x.segment<3>(0) += t; + } + for (auto& p : curr_scan) + writeLidarPoint(db, p); + prev_scan = curr_scan; + curr_scan.clear(); + } + + Vector3d pos = state.x.segment<3>(0); + Vector3d ang = state.x.segment<3>(6); + writeTraj(db, imu[i].t, pos, ang); } - Vector3d pos = state.x.segment<3>(0); - Vector3d ang = state.x.segment<3>(6); + imu_idx = imu.size(); - traj << imu[i].t << "," - << pos.x() << "," - << pos.y() << "," - << pos.z() << "," - << ang.x() << "," - << ang.y() << "," - << ang.z() << "\n"; + // Ждём 200 мс перед следующим циклом + this_thread::sleep_for(chrono::milliseconds(200)); } - for (auto& p : prev_scan) { - lidar_out << p.x() << "," - << p.y() << "," - << p.z() << "\n"; - } - - cout << "Done\n"; + sqlite3_close(db); + cout << "Расчёты остановлены" << endl; return 0; } \ No newline at end of file diff --git a/processing/visualization.py b/processing/visualization.py index 3f24961..e965d12 100644 --- a/processing/visualization.py +++ b/processing/visualization.py @@ -1,55 +1,41 @@ import open3d as o3d import numpy as np -import pandas as pd -import matplotlib.pyplot as plt +import sqlite3 +import signal +import sys import os -from datetime import datetime +import time -# СОЗДАНИЕ УНИКАЛЬНОЙ ПАПКИ +DB_PATH = '../inertial_data.db' -timestamp = datetime.now().strftime("%Y-%m-%d_%H-%M-%S") -output_dir = f"results/run_{timestamp}" -frames_dir = os.path.join(output_dir, "frames") +# ===== ЗАВЕРШЕНИЕ ВСЕГО ПРОЕКТА ===== -os.makedirs(frames_dir, exist_ok=True) +def shutdown_all(): + """Останавливает все процессы проекта""" + os.system("pkill -f imu_reader.py") + os.system("pkill -f lidar_reader.py") + os.system("pkill -f calculations") -# ЗАГРУЗКА ДАННЫХ +# ===== ЧТЕНИЕ ИЗ SQLite ===== -traj = pd.read_csv("trajectory.csv") -lidar = pd.read_csv("lidar_points.csv") +def read_trajectory(conn): + cursor = conn.execute( + "SELECT x, y, z, roll, pitch, yaw FROM trajectory ORDER BY timestamp" + ) + rows = cursor.fetchall() + if not rows: + return np.zeros((0, 3)), np.zeros((0, 3)) + data = np.array(rows) + return data[:, :3], data[:, 3:] -trajectory = traj[['x', 'y', 'z']].values -angles = traj[['roll', 'pitch', 'yaw']].values -lidar_points = lidar[['x', 'y', 'z']].values +def read_lidar_points(conn): + cursor = conn.execute("SELECT x, y, z FROM lidar_points") + rows = cursor.fetchall() + if not rows: + return np.zeros((0, 3)) + return np.array(rows) -# СОХРАНЕНИЕ 2D ГРАФИКОВ - -plt.figure() -plt.plot(trajectory[:,0], trajectory[:,1]) -plt.title("Trajectory (X-Y)") -plt.xlabel("X") -plt.ylabel("Y") -plt.grid() -plt.savefig(os.path.join(output_dir, "trajectory_plot.png")) -plt.close() - -# OPEN3D - -pcd = o3d.geometry.PointCloud() -pcd.points = o3d.utility.Vector3dVector(lidar_points) - -lines = [[i, i+1] for i in range(len(trajectory)-1)] -line_set = o3d.geometry.LineSet() -line_set.points = o3d.utility.Vector3dVector(trajectory) -line_set.lines = o3d.utility.Vector2iVector(lines) - -vis = o3d.visualization.Visualizer() -vis.create_window(visible=False) # важно для сохранения кадров - -vis.add_geometry(pcd) -vis.add_geometry(line_set) - -# ПОВОРОТ +# ===== ПОВОРОТ ===== def get_rotation_matrix(roll, pitch, yaw): Rx = np.array([ @@ -69,36 +55,68 @@ def get_rotation_matrix(roll, pitch, yaw): ]) return Rz @ Ry @ Rx -# СОХРАНЕНИЕ КАДРОВ +# ===== MAIN ===== -for i in range(len(trajectory)): +def main(): + conn = sqlite3.connect(DB_PATH) + # Создаём окно Open3D + vis = o3d.visualization.Visualizer() + vis.create_window(window_name="Инерциальное управление", width=1280, height=720) + + pcd = o3d.geometry.PointCloud() + line_set = o3d.geometry.LineSet() frame = o3d.geometry.TriangleMesh.create_coordinate_frame(size=0.2) - pos = trajectory[i] - roll, pitch, yaw = angles[i] - - R = get_rotation_matrix(roll, pitch, yaw) - - frame.rotate(R, center=(0,0,0)) - frame.translate(pos) - - vis.clear_geometries() vis.add_geometry(pcd) vis.add_geometry(line_set) vis.add_geometry(frame) - vis.poll_events() - vis.update_renderer() + print("Визуализация запущена. Закройте окно для остановки проекта.") - # сохраняем изображение - img_path = os.path.join(frames_dir, f"frame_{i:04d}.png") - vis.capture_screen_image(img_path) + try: + while True: + # Проверяем закрыто ли окно + if not vis.poll_events(): + print("Окно закрыто — останавливаем проект...") + shutdown_all() + break -# СОХРАНЕНИЕ ФИНАЛЬНОГО ВИДА + # Читаем новые данные из БД + trajectory, angles = read_trajectory(conn) + lidar_points = read_lidar_points(conn) -vis.capture_screen_image(os.path.join(output_dir, "3d_view.png")) + # Обновляем облако точек лидара + if len(lidar_points) > 0: + pcd.points = o3d.utility.Vector3dVector(lidar_points) + vis.update_geometry(pcd) -vis.destroy_window() + # Обновляем траекторию + if len(trajectory) > 1: + lines = [[i, i+1] for i in range(len(trajectory)-1)] + line_set.points = o3d.utility.Vector3dVector(trajectory) + line_set.lines = o3d.utility.Vector2iVector(lines) + vis.update_geometry(line_set) -print(f"Результаты сохранены в: {output_dir}") + # Обновляем позицию объекта + if len(trajectory) > 0: + pos = trajectory[-1] + roll, pitch, yaw = angles[-1] + R = get_rotation_matrix(roll, pitch, yaw) + frame_new = o3d.geometry.TriangleMesh.create_coordinate_frame(size=0.2) + frame_new.rotate(R, center=(0, 0, 0)) + frame_new.translate(pos) + frame.vertices = frame_new.vertices + frame.triangles = frame_new.triangles + vis.update_geometry(frame) + + vis.update_renderer() + + # Обновляем каждые 200 мс + time.sleep(0.2) + + finally: + vis.destroy_window() + conn.close() + +main() \ No newline at end of file diff --git a/run.sh b/run.sh index 6cbcc0e..c32ee76 100644 --- a/run.sh +++ b/run.sh @@ -1,22 +1,26 @@ #!/bin/bash -echo "Запуск системы..." -# Запуск модуля акселерометра -echo "Запуск IMU модуля..." +# Переходим в папку проекта +cd "$(dirname "$0")" + +echo "Запуск проекта..." + +# Запускаем модули сбора данных в фоне python3 imu_module/imu_reader.py & +IMU_PID=$! -# Запуск модуля лидара -echo "Запуск лидар модуля..." python3 lidar_module/lidar_reader.py & +LIDAR_PID=$! -# Ждём пока данные запишутся +# Запускаем расчёты в фоне +processing/calculations & +CALC_PID=$! + +# Запускаем визуализацию (она управляет завершением) +python3 processing/visualization.py + +# Когда визуализация закрыта — останавливаем всё +echo "Остановка проекта..." +kill $IMU_PID $LIDAR_PID $CALC_PID 2>/dev/null wait - -# Компиляция и запуск расчётов -echo "Запуск расчётов..." -g++ processing/calculations.cpp -o processing/calculations -./processing/calculations - -# Запуск визуализации -echo "Запуск визуализации..." -python3 processing/visualization.py \ No newline at end of file +echo "Проект остановлен" \ No newline at end of file -- 2.43.0