imu_module/imu_reader.py

@@ -1,92 +1,68 @@
-import smbus2
+import smbus2 #библиотека Python для работы с I2C шиной
 import time
-import sqlite3
+import csv
-import signal
+import struct
-import sys
 
 # Константы
 MPU_ADDRESS = 0x68
-I2C_BUS = 2
+I2C_BUS = 1
-DB_PATH = '../inertial_data.db'
 
 # Регистры MPU-9250
-PWR_MGMT_1 = 0x6B
+PWR_MGMT_1 = 0x6B      # управление питанием
-ACCEL_XOUT_H = 0x3B
+ACCEL_XOUT_H = 0x3B    # начало данных акселерометра
-GYRO_XOUT_H = 0x43
+GYRO_XOUT_H = 0x43     # начало данных гироскопа
 
-# Масштабные коэффициенты
+# Масштабные коэффициенты (диапазон ±2g и ±250°/с)
-ACCEL_SCALE = 16384.0
+ACCEL_SCALE = 16384.0  # LSB/g
-GYRO_SCALE = 131.0
+GYRO_SCALE = 131.0     # LSB/(°/с)
-
-# Глобальные переменные для завершения
-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_db():
+def save_to_csv(filename, duration_seconds):
-    global conn, bus, running
+    """Записать данные в CSV файл"""
-    conn = init_db()
     bus = smbus2.SMBus(I2C_BUS)
     init_mpu(bus)
 
-    print("Запись данных IMU...")
+    with open(filename, 'w', newline='') as f:
-    t = 0.0
+        writer = csv.writer(f)
-    try:
+        writer.writerow(['timestamp', 'ax', 'ay', 'az', 'gx', 'gy', 'gz'])
-        while running:
+
+        t = 0.0
+        while t < duration_seconds:
             ax, ay, az, gx, gy, gz = read_imu(bus)
-            conn.execute(
+            writer.writerow([round(t, 3), 
-                'INSERT INTO imu_data VALUES (?, ?, ?, ?, ?, ?, ?)',
+                           round(ax, 4), round(ay, 4), round(az, 4),
-                (round(t, 3), round(ax, 4), round(ay, 4), round(az, 4),
+                           round(gx, 4), round(gy, 4), round(gz, 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 модуль остановлен")
 
-save_to_db()
+    bus.close()
+
+# Запуск — записываем 5 секунд
+save_to_csv('imu.csv', 5)
+print("Готово!")

lidar_module/lidar_reader.py

@@ -1,104 +1,87 @@
 import serial
+import csv
 import time
-import sqlite3
 import struct
-import signal
 
 # Константы
-SERIAL_PORT = '/dev/ttyS0'
+SERIAL_PORT = '/dev/ttyS0'  # UART порт на MangoPi
-BAUD_RATE = 230400
+BAUD_RATE = 230400          # скорость передачи LDS02RR
-DB_PATH = '../inertial_data.db'
+DURATION = 5                # секунд записи
 
 # Заголовок пакета 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 or data[0] != HEADER:
+    if len(data) < PACKET_SIZE:
         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] >> 2
+        quality = struct.unpack_from('<H', data, offset + 2)[0] >> 8
-        distance = distance_raw / 4.0
+
-        angle = index * 4 + i
+        distance = distance_raw / 4.0  # в мм
+        angle = index * 4 + i          # угол в градусах
+
         points.append((angle, distance, quality))
+
     return points
 
-def save_to_db():
+def save_to_csv(filename, duration_seconds):
-    """Основной цикл записи данных в БД"""
+    """Читать данные лидара и записывать в CSV"""
-    global conn, ser, running
-    conn = init_db()
-
     try:
         ser = serial.Serial(SERIAL_PORT, BAUD_RATE, timeout=1)
-        print("Подключено к лидару")
+        print(f"Подключено к {SERIAL_PORT}")
     except Exception as e:
         print(f"Ошибка подключения: {e}")
-        conn.close()
         return
 
-    print("Запись данных лидара...")
+    with open(filename, 'w', newline='') as f:
-    buffer = bytearray()
+        writer = csv.writer(f)
-    start_time = time.time()
+        writer.writerow(['timestamp', 'angle', 'distance_mm', 'quality'])
 
-    try:
+        start_time = time.time()
-        while running:
+        buffer = bytearray()
+
+        while time.time() - start_time < duration_seconds:
             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:
-                            conn.execute(
+                            writer.writerow([t, angle, 
-                                'INSERT INTO lidar_data VALUES (?, ?, ?, ?)',
+                                           round(distance, 1), 
-                                (t, angle, round(distance, 1), quality)
+                                           quality])
-                            )
-                        conn.commit()
                     buffer = buffer[PACKET_SIZE:]
                 else:
                     buffer.pop(0)
-    finally:
-        ser.close()
-        conn.close()
-        print("Лидар модуль остановлен")
 
-save_to_db()
+    ser.close()
+    print("Готово!")
+
+# Запуск
+save_to_csv('lidar.csv', DURATION)

processing/calculations.cpp

@@ -1,319 +0,0 @@
-#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

@@ -1,122 +0,0 @@
-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,26 +1,22 @@
 #!/bin/bash
+echo "Запуск системы..."
 
-# Переходим в папку проекта
+# Запуск модуля акселерометра
-cd "$(dirname "$0")"
+echo "Запуск IMU модуля..."
-
-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 "Проект остановлен"
+
+# Компиляция и запуск расчётов
+echo "Запуск расчётов..."
+g++ processing/calculations.cpp -o processing/calculations
+./processing/calculations
+
+# Запуск визуализации
+echo "Запуск визуализации..."
+python3 processing/visualization.py