Паттерны проектирования в робототехнике — Архитектура кибернетического разума

class HierarchicalFSM {
    struct State {
        virtual void enter() = 0;
        virtual void execute() = 0;
        virtual void exit() = 0;
        virtual State* check_transitions() = 0;
    };
    
    struct RobotState : State {
        // Состояния верхнего уровня
        State* current_state;
        map<string, State*> substates;
        
        State* check_transitions() override {
            // Проверка переходов внутри иерархии
            if (emergency_stop) return &emergency_state;
            if (battery_low) return &charging_state;
            return current_state->check_transitions();
        }
    };
};

class CyclicScheduler {
    struct Task {
        void (*function)();
        uint32_t period_ms;
        uint32_t deadline_ms;
        uint32_t last_run;
    };
    
    vector<Task> tasks;
    
    void run() {
        while (true) {
            uint32_t now = millis();
            for (auto& task : tasks) {
                if (now - task.last_run >= task.period_ms) {
                    if (check_deadline(now, task)) {
                        task.function();
                        task.last_run = now;
                    }
                }
            }
            idle_task(); // Фоновая задача
        }
    }
};

// Прерывание от энкодера (реактивное)
volatile int32_t encoder_count = 0;
void ISR_EncoderA() {
    if (digitalRead(ENC_B)) encoder_count++;
    else encoder_count--;
}

// Фоновая обработка (стратегическое)
void calculate_velocity() {
    static int32_t last_count = 0;
    int32_t delta = encoder_count - last_count;
    float velocity = delta / (ENCODER_RESOLUTION * SAMPLE_TIME);
    last_count = encoder_count;
    publish_velocity(velocity);
}

// Издатель
class LaserPublisher : public rclcpp::Node {
public:
    LaserPublisher() : Node("laser_publisher") {
        publisher_ = create_publisher<sensor_msgs::msg::LaserScan>("scan", 10);
        timer_ = create_wall_timer(100ms, [this]() {
            auto message = sensor_msgs::msg::LaserScan();
            // ... заполнение данных
            publisher_->publish(message);
        });
    }
};

// Подписчик
class NavigationSubscriber : public rclcpp::Node {
public:
    NavigationSubscriber() : Node("navigation_node") {
        subscription_ = create_subscription<sensor_msgs::msg::LaserScan>(
            "scan", 10, [this](const sensor_msgs::msg::LaserScan::SharedPtr msg) {
                process_scan(msg);
            });
    }
};

auto qos = rclcpp::QoS(rclcpp::KeepLast(10))
    .reliability(RMW_QOS_POLICY_RELIABILITY_RELIABLE)  // Гарантированная доставка
    .durability(RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL)  // Сохранение для поздних подписчиков
    .deadline(std::chrono::milliseconds(100))  // Максимальная задержка
    .liveliness(RMW_QOS_POLICY_LIVELINESS_AUTOMATIC);  // Проверка "живости"

[Сенсоры] → [Чёрная доска] ← [Планировщики]
    ↓           ↓               ↓
[Фильтры] → [Факты] → [Решатели]

class Blackboard:
    def __init__(self):
        self.data = {}
        self.lock = threading.RLock()
        self.subscribers = defaultdict(list)
    
    def write(self, key, value, source):
        with self.lock:
            old_value = self.data.get(key)
            self.data[key] = {
                'value': value,
                'timestamp': time.time(),
                'source': source,
                'confidence': self.calculate_confidence(source, value)
            }
            
            # Уведомление подписчиков
            if old_value != value:
                for callback in self.subscribers[key]:
                    callback(key, value)
    
    def read(self, key, max_age=5.0):
        with self.lock:
            if key not in self.data:
                return None
            entry = self.data[key]
            if time.time() - entry['timestamp'] > max_age:
                return None
            return entry['value'], entry['confidence']

class ProcessingPipeline:
    def __init__(self):
        self.stages = []
        self.queues = []
        
    def add_stage(self, processor, max_queue=10):
        self.stages.append(processor)
        self.queues.append(queue.Queue(max_queue))
    
    def run(self):
        # Запуск стадий в отдельных потоках
        threads = []
        for i in range(len(self.stages)):
            t = threading.Thread(target=self._stage_worker, args=(i,))
            t.start()
            threads.append(t)
    
    def _stage_worker(self, stage_idx):
        input_queue = self.queues[stage_idx]
        output_queue = self.queues[stage_idx + 1] if stage_idx + 1 < len(self.queues) else None
        processor = self.stages[stage_idx]
        
        while True:
            data = input_queue.get()  # Блокирующее ожидание
            result = processor(data)
            if output_queue:
                output_queue.put(result, block=True, timeout=1.0)

services:
  perception-service:
    image: perception:2026.1
    resources:
      gpu: 1
    env:
      - MODEL_PATH=/models/yolo-v9
    
  planning-service:
    image: planner:2026.1
    depends_on:
      - perception-service
    env:
      - PLANNER_TYPE=graph_nav
    
  control-service:
    image: controller:2026.1
    depends_on:
      - planning-service
    devices:
      - /dev/ttyMotor:/dev/ttyMotor
    
  monitoring-service:
    image: monitor:2026.1
    ports:
      - "9090:9090"  # Prometheus метрики

class TaskOrchestrator:
    def __init__(self):
        self.workflows = {}
        self.execution_engine = ExecutionEngine()
        
    def define_workflow(self, name, definition):
        """Определение workflow в формате YAML/JSON"""
        # Пример определения доставки
        workflow = {
            'states': [
                {'name': 'PickItem', 'type': 'task', 'resource': 'arm_controller'},
                {'name': 'NavigateTo', 'type': 'task', 'resource': 'nav_system'},
                {'name': 'PlaceItem', 'type': 'task', 'resource': 'arm_controller'},
                {'name': 'ReturnHome', 'type': 'task', 'resource': 'nav_system'},
            ],
            'transitions': [
                {'from': 'PickItem', 'to': 'NavigateTo', 'condition': 'item_grabbed'},
                {'from': 'NavigateTo', 'to': 'PlaceItem', 'condition': 'at_destination'},
                # ...
            ]
        }
        self.workflows[name] = workflow
    
    def execute_workflow(self, workflow_name, parameters):
        """Выполнение workflow с отслеживанием состояния"""
        execution_id = str(uuid.uuid4())
        
        # Состояние выполнения
        state = {
            'execution_id': execution_id,
            'current_state': 'PickItem',
            'history': [],
            'parameters': parameters,
            'status': 'running'
        }
        
        while state['status'] == 'running':
            current = state['current_state']
            task_def = self.get_task_definition(workflow_name, current)
            
            # Выполнение задачи
            result = self.execution_engine.execute(task_def, state['parameters'])
            
            # Обновление состояния
            state['history'].append({
                'state': current,
                'result': result,
                'timestamp': time.time()
            })
            
            # Переход к следующему состоянию
            next_state = self.evaluate_transitions(workflow_name, current, result)
            if next_state:
                state['current_state'] = next_state
            else:
                state['status'] = 'completed'
        
        return state

import tensorflow as tf
import tf_robotics as tfr

# Декларативное определение конвейера восприятия
class PerceptionGraph(tf.Module):
    def __init__(self):
        self.detector = tfr.vision.YOLOv7()
        self.tracker = tfr.tracking.SORT()
        self.localizer = tfr.localization.VisualOdometry()
    
    @tf.function
    def __call__(self, image_sequence):
        # Граф автоматически оптимизируется TF
        detections = self.detector(image_sequence)
        tracks = self.tracker(detections)
        pose_updates = self.localizer(image_sequence)
        
        return {
            'detections': detections,
            'tracks': tracks,
            'odometry': pose_updates
        }

# JIT-компиляция для оптимального исполнения
graph = PerceptionGraph()
compiled_graph = tf.function(graph, jit_compile=True)

class AdaptiveSensorFusion:
    def __init__(self, sensors):
        self.sensors = sensors
        self.weights = {s.name: 1.0/len(sensors) for s in sensors}
        self.confidence_model = ConfidenceModel()
        
    def update(self):
        readings = {}
        confidences = {}
        
        # Получение данных от всех сенсоров
        for sensor in self.sensors:
            reading, metadata = sensor.read()
            readings[sensor.name] = reading
            confidences[sensor.name] = self.confidence_model.evaluate(
                reading, metadata
            )
        
        # Адаптивное взвешивание
        total_confidence = sum(confidences.values())
        if total_confidence > 0:
            self.weights = {
                name: confidences[name]/total_confidence 
                for name in confidences
            }
        
        # Взвешенное усреднение
        fused_value = 0.0
        for name, reading in readings.items():
            fused_value += reading * self.weights[name]
            
        return fused_value, self.weights

class RedundantController {
    enum ControllerState { PRIMARY, BACKUP, FAILED };
    
    struct Controller {
        PID pid;
        ControllerState state;
        uint32_t error_count;
        float last_output;
    };
    
    Controller primary;
    Controller backup;
    
    float compute(float error, float dt) {
        // Основной контроллер
        float primary_output = primary.pid.compute(error, dt);
        
        // Мониторинг состояния
        if (detect_anomaly(primary_output, error)) {
            primary.error_count++;
            if (primary.error_count > MAX_ERRORS) {
                primary.state = FAILED;
                switch_to_backup();
            }
        }
        
        // Резервный контроллер (всегда вычисляет)
        float backup_output = backup.pid.compute(error, dt);
        
        return (primary.state == PRIMARY) ? primary_output : backup_output;
    }
};

class EnergyAwarePlanner:
    def __init__(self, power_model, battery_capacity):
        self.power_model = power_model
        self.battery = battery_capacity
        self.cost_functions = {
            'distance': self.distance_cost,
            'energy': self.energy_cost,
            'time': self.time_cost
        }
    
    def plan_path(self, start, goal, constraints):
        # Многокритериальная оптимизация
        paths = self.generate_candidate_paths(start, goal)
        
        scored_paths = []
        for path in paths:
            score = 0
            for constraint, weight in constraints.items():
                cost_func = self.cost_functions[constraint]
                score += weight * cost_func(path)
            scored_paths.append((score, path))
        
        # Выбор пути с учётом остатка энергии
        best_score, best_path = min(scored_paths, key=lambda x: x[0])
        
        if self.estimate_energy(best_path) > self.battery * 0.8:
            return self.find_charging_path(best_path)
        
        return best_path

class ExplorationRobotArchitecture:
    def __init__(self):
        # Уровень 1: Реактивные компоненты (100 Гц)
        self.reactive_layer = ReactiveLayer(
            sensors=['lidar', 'imu', 'bumper'],
            actuators=['motors', 'emergency_stop'],
            fsm_states=['normal', 'collision', 'emergency']
        )
        
        # Уровень 2: Координационные компоненты (10 Гц)
        self.coordination_layer = CoordinationLayer(
            blackboard=Blackboard(),
            publishers=['/map', '/pose', '/goals'],
            subscribers=['/sensor_data', '/system_status']
        )
        
        # Уровень 3: Стратегические компоненты (1 Гц)
        self.strategic_layer = StrategicLayer(
            services=['exploration', 'mapping', 'diagnostics'],
            orchestrator=WorkflowOrchestrator(),
            planner=GraphBasedPlanner()
        )
    
    def run(self):
        # Запуск на разных частотах
        reactive_thread = threading.Thread(
            target=self.reactive_layer.run, 
            args=(100,)
        )
        coordination_thread = threading.Thread(
            target=self.coordination_layer.run,
            args=(10,)
        )
        strategic_thread = threading.Thread(
            target=self.strategic_layer.run,
            args=(1,)
        )
        
        # Связь между уровнями через очереди
        self.setup_interlayer_communication()
        
        reactive_thread.start()
        coordination_thread.start()
        strategic_thread.start()