Python как инструмент разработки роботизированных систем

Расширенные примеры программирования роботов на Python

1. Дистанционное управление по Bluetooth с Android

На Raspberry Pi через Bluetooth SPP (серийный порт) можно принимать команды со смартфона. Устанавливается библиотека pybluez.

import bluetooth
import RPi.GPIO as GPIO

server_sock = bluetooth.BluetoothSocket(bluetooth.RFCOMM)
server_sock.bind(("", bluetooth.PORT_ANY))
server_sock.listen(1)

port = server_sock.getsockname()[1]
bluetooth.advertise_service(server_sock, "RobotCtrl", service_id=...,
                            service_classes=[bluetooth.SERIAL_PORT_CLASS])

client_sock, client_info = server_sock.accept()
try:
    while True:
        data = client_sock.recv(1024)
        if data:
            # Парсинг команды (например, 'w','a','s','d')
            command = data.decode().strip()
            print("Received:", command)
            # Далее вызов функции управления моторами
except KeyboardInterrupt:
    client_sock.close()
    server_sock.close()
    GPIO.cleanup()

(При подключении со смартфона отправляется символ 'w' – робот едет вперёд)

Для работы требуется установить pybluez (sudo pip3 install pybluez). На Android используется приложение Serial Bluetooth Terminal. Важно: на Raspberry Pi Bluetooth может конфликтовать с Wi-Fi, рекомендуется использовать внешний USB-адаптер.

2. Обработка видеопотока с камеры для отслеживания объектов

Библиотека OpenCV позволяет анализировать изображение в реальном времени. Пример поиска красного предмета и вычисления его центра.

import cv2
import numpy as np

cap = cv2.VideoCapture(0)
while True:
    ret, frame = cap.read()
    if not ret:
        break
    # Преобразование в HSV
    hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
    # Диапазон красного цвета (два интервала из-за особенностей HSV)
    lower_red1 = np.array([0, 70, 50])
    upper_red1 = np.array([10, 255, 255])
    lower_red2 = np.array([170, 70, 50])
    upper_red2 = np.array([180, 255, 255])
    mask1 = cv2.inRange(hsv, lower_red1, upper_red1)
    mask2 = cv2.inRange(hsv, lower_red2, upper_red2)
    mask = cv2.bitwise_or(mask1, mask2)
    # Нахождение контуров
    contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
    if contours:
        largest = max(contours, key=cv2.contourArea)
        M = cv2.moments(largest)
        if M['m00'] != 0:
            cx = int(M['m10'] / M['m00'])
            cy = int(M['m01'] / M['m00'])
            cv2.circle(frame, (cx, cy), 5, (0,255,0), -1)
            # Координаты можно передать в управление моторами
    cv2.imshow('Frame', frame)
    if cv2.waitKey(1) & 0xFF == ord('q'):
        break
cap.release()
cv2.destroyAllWindows()

(В окне выводится видео с выделенным центром красного объекта)

Для OpenCV под Raspberry Pi требуется ускорение (использовать cv2.CAP_V4L2 или установить opencv-contrib-python). Проблема: высокая загрузка CPU – рекомендуется понижать разрешение или использовать аппаратный H.264 декодер.

3. Планирование пути методом A* для мобильного робота

Алгоритм A* находит кратчайший путь на сетке с учётом препятствий. Реализация на чистом Python без сторонних библиотек.

import heapq

def heuristic(a, b):
    return abs(a[0]-b[0]) + abs(a[1]-b[1])

def astar(grid, start, goal):
    rows, cols = len(grid), len(grid[0])
    open_set = []
    heapq.heappush(open_set, (0, start))
    came_from = {}
    g_score = {start: 0}
    f_score = {start: heuristic(start, goal)}
    
    while open_set:
        _, current = heapq.heappop(open_set)
        if current == goal:
            path = []
            while current in came_from:
                path.append(current)
                current = came_from[current]
            path.reverse()
            return path
        
        for dx, dy in [(0,1),(0,-1),(1,0),(-1,0)]:
            neighbor = (current[0]+dx, current[1]+dy)
            if 0 <= neighbor[0] < rows and 0 <= neighbor[1] < cols and grid[neighbor[0]][neighbor[1]] == 0:
                tentative_g = g_score[current] + 1
                if tentative_g < g_score.get(neighbor, float('inf')):
                    came_from[neighbor] = current
                    g_score[neighbor] = tentative_g
                    f_score[neighbor] = tentative_g + heuristic(neighbor, goal)
                    heapq.heappush(open_set, (f_score[neighbor], neighbor))
    return None

grid = [
 [0,0,0,0,0],
 [0,1,1,1,0],
 [0,0,0,1,0],
 [0,0,0,0,0]
]
path = astar(grid, (0,0), (3,4))
print(path)

[(0,0), (1,0), (2,0), (2,1), (2,2), (3,2), (3,3), (3,4)]

Результат – список координат ячеек. Для интеграции с ROS можно упаковать в сервис или actionlib. Проблема: на больших картах памяти может не хватить – используется бинарная куча и компактное хранение графа.

4. Управление сервоприводами через i2c (PCA9685)

Модуль PCA9685 позволяет управлять 16 сервоприводами по шине I2C. Библиотека Adafruit_CircuitPython_ServoKit упрощает задачу.

from adafruit_servokit import ServoKit
kit = ServoKit(channels=16)
kit.servo[0].angle = 90   # серво на канале 0 поворот на 90 градусов
kit.servo[1].angle = 45

(Сервоприводы плавно перемещаются в заданные позиции)

Для установки: sudo pip3 install adafruit-circuitpython-servokit. Требуется активировать I2C на Raspberry Pi (raspi-config). Типичная ошибка: напряжение питания сервоприводов не должно подаваться от пинов GPIO – используется внешний источник 5В.

5. Интеграция Python с ROS2 (rclpy)

ROS2 (Robot Operating System 2) использует Python-клиент rclpy. Пример простого издателя и подписчика.

import rclpy
from rclpy.node import Node
from std_msgs.msg import String

class Talker(Node):
    def __init__(self):
        super().__init__('talker')
        self.pub = self.create_publisher(String, 'chatter', 10)
        timer_period = 0.5
        self.timer = self.create_timer(timer_period, self.timer_callback)
        self.counter = 0

    def timer_callback(self):
        msg = String()
        msg.data = f'Hello {self.counter}'
        self.pub.publish(msg)
        self.get_logger().info('Publishing: "%s"' % msg.data)
        self.counter += 1

def main(args=None):
    rclpy.init(args=args)
    node = Talker()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

[INFO] [talker]: Publishing: "Hello 0"
[INFO] [talker]: Publishing: "Hello 1" ...

ROS2 обеспечивает лучшую поддержку реального времени и мультиплатформенность по сравнению с ROS1. Необходимо установить ROS2 (Humble или Rolling) и colcon для сборки пакетов.