Merge pull request 'feat: replace CSV with SQLite realtime database' (#2) from feature/sqlite-realtime into develop

Reviewed-on: #2

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("Готово!")
+save_to_db()

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('<H', data, 2)[0] / 64.0
-
     points = []
     for i in range(4):
         offset = 4 + i * 4
         distance_raw = struct.unpack_from('<H', data, offset)[0]
-        quality = struct.unpack_from('<H', data, offset + 2)[0] >> 8
-
-        distance = distance_raw / 4.0  # в мм
-        angle = index * 4 + i          # угол в градусах
-
+        quality = struct.unpack_from('<H', data, offset + 2)[0] >> 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)
+save_to_db()

processing/calculations.cpp

@@ -0,0 +1,319 @@
+#include <iostream>
+#include <vector>
+#include <cmath>
+#include <algorithm>
+#include <chrono>
+#include <thread>
+#include <csignal>
+#include <sqlite3.h>
+#include <Eigen/Dense>
+
+using namespace std;
+using namespace Eigen;
+
+// ===== ФЛАГ ЗАВЕРШЕНИЯ =====
+volatile sig_atomic_t running = 1;
+
+void shutdown(int signum) {
+    running = 0;
+}
+
+// ===== СТРУКТУРЫ =====
+
+struct IMUData {
+    double t;
+    Vector3d acc;
+    Vector3d gyro;
+};
+
+struct LidarData {
+    double t;
+    double angle;
+    double distance;
+};
+
+struct State {
+    VectorXd x;
+    MatrixXd P;
+};
+
+// ===== ЧТЕНИЕ ИЗ SQLite =====
+
+vector<IMUData> readIMU(sqlite3* db) {
+    vector<IMUData> data;
+    sqlite3_stmt* stmt;
+
+    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;
+        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<LidarData> readLidar(sqlite3* db) {
+    vector<LidarData> data;
+    sqlite3_stmt* stmt;
+
+    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;
+        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());
+    AngleAxisd ry(pitch, Vector3d::UnitY());
+    AngleAxisd rz(yaw, Vector3d::UnitZ());
+    return (rz * ry * rx).toRotationMatrix();
+}
+
+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);
+
+    Vector3d acc = imu.acc * 9.81;
+    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;
+
+    vel += acc_world * dt;
+    pos += vel * dt + 0.5 * acc_world * dt * dt;
+    angles += gyro * dt;
+
+    x.segment<3>(0) = pos;
+    x.segment<3>(3) = vel;
+    x.segment<3>(6) = angles;
+
+    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;
+    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;
+}
+
+// ===== 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<Vector3d>& 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]; });
+    int mid = pts.size() / 2;
+    KDNode* node = new KDNode(pts[mid], axis);
+    vector<Vector3d> left(pts.begin(), pts.begin() + mid);
+    vector<Vector3d> 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; }
+    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);
+}
+
+Matrix3d computeRotation(const vector<Vector3d>& src, const vector<Vector3d>& 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++)
+        H += (src[i] - c1) * (dst[i] - c2).transpose();
+    JacobiSVD<Matrix3d> svd(H, ComputeFullU | ComputeFullV);
+    return svd.matrixV() * svd.matrixU().transpose();
+}
+
+Vector3d computeTranslation(const vector<Vector3d>& src, const vector<Vector3d>& 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<Vector3d> downsample(const vector<Vector3d>& pts, int step = 5) {
+    vector<Vector3d> out;
+    for (size_t i = 0; i < pts.size(); i += step)
+        out.push_back(pts[i]);
+    return out;
+}
+
+void ICP_fast(vector<Vector3d>& src, vector<Vector3d>& dst, Matrix3d& R, Vector3d& t) {
+    R = Matrix3d::Identity();
+    t = Vector3d::Zero();
+    vector<Vector3d> dst_copy = dst;
+    KDNode* tree = buildKDTree(dst_copy);
+    for (int iter = 0; iter < 10; iter++) {
+        vector<Vector3d> matched;
+        for (auto& p : src) {
+            Vector3d best; double best_dist = 1e9;
+            nearestSearch(tree, p, best, best_dist);
+            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;
+        R = dR * R;
+        t = dR * t + dt;
+    }
+}
+
+// ===== MAIN =====
+
+int main() {
+    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;
+
+    vector<Vector3d> prev_scan, curr_scan;
+    size_t lidar_idx = 0;
+    size_t imu_idx = 0;
+
+    cout << "Расчёты запущены..." << endl;
+
+    while (running) {
+        auto imu = readIMU(db);
+        auto lidar = readLidar(db);
+
+        // Обрабатываем только новые данные
+        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;
+
+            predict(state, imu[i], dt);
+
+            while (lidar_idx < lidar.size() &&
+                   lidar[lidar_idx].t <= imu[i].t) {
+                curr_scan.push_back(lidarToWorld(state, lidar[lidar_idx]));
+                lidar_idx++;
+            }
+
+            if (curr_scan.size() > 200) {
+                if (!prev_scan.empty()) {
+                    vector<Vector3d> src = downsample(curr_scan, 5);
+                    vector<Vector3d> 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);
+        }
+
+        imu_idx = imu.size();
+
+        // Ждём 200 мс перед следующим циклом
+        this_thread::sleep_for(chrono::milliseconds(200));
+    }
+
+    sqlite3_close(db);
+    cout << "Расчёты остановлены" << endl;
+    return 0;
+}

processing/visualization.py

@@ -0,0 +1,122 @@
+import open3d as o3d
+import numpy as np
+import sqlite3
+import signal
+import sys
+import os
+import time
+
+DB_PATH = '../inertial_data.db'
+
+# ===== ЗАВЕРШЕНИЕ ВСЕГО ПРОЕКТА =====
+
+def shutdown_all():
+    """Останавливает все процессы проекта"""
+    os.system("pkill -f imu_reader.py")
+    os.system("pkill -f lidar_reader.py")
+    os.system("pkill -f calculations")
+
+# ===== ЧТЕНИЕ ИЗ SQLite =====
+
+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:]
+
+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)
+
+# ===== ПОВОРОТ =====
+
+def get_rotation_matrix(roll, pitch, yaw):
+    Rx = np.array([
+        [1, 0, 0],
+        [0, np.cos(roll), -np.sin(roll)],
+        [0, np.sin(roll), np.cos(roll)]
+    ])
+    Ry = np.array([
+        [np.cos(pitch), 0, np.sin(pitch)],
+        [0, 1, 0],
+        [-np.sin(pitch), 0, np.cos(pitch)]
+    ])
+    Rz = np.array([
+        [np.cos(yaw), -np.sin(yaw), 0],
+        [np.sin(yaw), np.cos(yaw), 0],
+        [0, 0, 1]
+    ])
+    return Rz @ Ry @ Rx
+
+# ===== MAIN =====
+
+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)
+
+    vis.add_geometry(pcd)
+    vis.add_geometry(line_set)
+    vis.add_geometry(frame)
+
+    print("Визуализация запущена. Закройте окно для остановки проекта.")
+
+    try:
+        while True:
+            # Проверяем закрыто ли окно
+            if not vis.poll_events():
+                print("Окно закрыто — останавливаем проект...")
+                shutdown_all()
+                break
+
+            # Читаем новые данные из БД
+            trajectory, angles = read_trajectory(conn)
+            lidar_points = read_lidar_points(conn)
+
+            # Обновляем облако точек лидара
+            if len(lidar_points) > 0:
+                pcd.points = o3d.utility.Vector3dVector(lidar_points)
+                vis.update_geometry(pcd)
+
+            # Обновляем траекторию
+            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)
+
+            # Обновляем позицию объекта
+            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()

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=$!
 
-# Ждём пока данные запишутся
-wait
+# Запускаем расчёты в фоне
+processing/calculations &
+CALC_PID=$!
 
-# Компиляция и запуск расчётов
-echo "Запуск расчётов..."
-g++ processing/calculations.cpp -o processing/calculations
-./processing/calculations
-
-# Запуск визуализации
-echo "Запуск визуализации..."
+# Запускаем визуализацию (она управляет завершением)
 python3 processing/visualization.py
+
+# Когда визуализация закрыта — останавливаем всё
+echo "Остановка проекта..."
+kill $IMU_PID $LIDAR_PID $CALC_PID 2>/dev/null
+wait
+echo "Проект остановлен"