Знакомство с ROS2. Практикум: Управляем двигателем с помощью Raspberry Pi

18.03.2025

Данная программа позволяет управлять одним двигателем с помощью трех кнопок для движения вперед, назад и остановки. Код также интегрирован с ROS 2 для возможного расширения функциональности и интеграции с другими системами.

Схема подключения

Фото

Создание тома Docker и запуск контейнера

Создадим том, где будут сохраняться данные, иначе при выходе из Докера данные (установленные библиотеки, проекты и пр.) пропадут.

Bash

# Создадим том
docker volume create wtf_data

# Запустим контейнер с подключенным томом
docker run -it --privileged \
  --device /dev/gpiomem:/dev/gpiomem \
  -v /dev/mem:/dev/mem \
  -v wtf_data:/data \
  ros:jazzy-perception bash

# Теперь все данные, сохраненные в /data внутри контейнера, 
# будут сохранены даже после остановки контейнера

Создание пакета ROS2

Для использования этого кода в ROS2, нужно создать пакет. Вот как это сделать:

Создадим рабочую директорию ROS2:

Bash

mkdir -p /data/wtf/src
cd /data/wtf/src

Создадим пакет:

Bash

ros2 pkg create --build-type ament_python motor_control

ament_python — это тип системы сборки, используемый в ROS 2 для создания и управления пакетами на языке Python.

Установим редактор nano, если он не установлен:

Bash

sudo apt update
apt install nano

Поместим код в файл motor_controller.py

Bash

sudo nano /data/wtf/src/motor_control/motor_control/motor_controller.py

Код:

Python

#!/usr/bin/env python3
# -*- coding: utf-8 -*-

import rclpy
from rclpy.node import Node
import RPi.GPIO as GPIO
import time

class MotorController(Node):
    def __init__(self):
        super().__init__('motor_controller')
        
        # Настройка GPIO
        GPIO.setmode(GPIO.BCM)
        GPIO.setwarnings(False)
        
        # Пины для драйвера L298N
        self.in1_pin = 17  # Управление направлением 1
        self.in2_pin = 18  # Управление направлением 2
        self.ena_pin = 12  # Включение/выключение двигателя
        
        # Пины для кнопок
        self.forward_button_pin = 23  # Кнопка движения вперед
        self.stop_button_pin = 24     # Кнопка остановки
        self.backward_button_pin = 25  # Кнопка движения назад
        
        # Настройка пинов драйвера как выходов
        GPIO.setup(self.in1_pin, GPIO.OUT)
        GPIO.setup(self.in2_pin, GPIO.OUT)
        GPIO.setup(self.ena_pin, GPIO.OUT)
        
        # Настройка пинов кнопок как входов с подтягивающими резисторами
        GPIO.setup(self.forward_button_pin, GPIO.IN, pull_up_down=GPIO.PUD_UP)
        GPIO.setup(self.stop_button_pin, GPIO.IN, pull_up_down=GPIO.PUD_UP)
        GPIO.setup(self.backward_button_pin, GPIO.IN, pull_up_down=GPIO.PUD_UP)
        
        # Добавление обработчиков событий для кнопок
        GPIO.add_event_detect(self.forward_button_pin, GPIO.FALLING, 
                             callback=self.forward_button_callback, bouncetime=300)
        GPIO.add_event_detect(self.stop_button_pin, GPIO.FALLING, 
                             callback=self.stop_button_callback, bouncetime=300)
        GPIO.add_event_detect(self.backward_button_pin, GPIO.FALLING, 
                             callback=self.backward_button_callback, bouncetime=300)
        
        # Создание таймера для периодической публикации состояния
        self.timer = self.create_timer(1.0, self.timer_callback)
        self.get_logger().info('Контроллер двигателя инициализирован')
        
        # Начальное состояние - остановлен
        self.stop_motor()
    
    def forward_button_callback(self, channel):
        self.get_logger().info('Нажата кнопка ВПЕРЕД')
        self.move_forward()
    
    def stop_button_callback(self, channel):
        self.get_logger().info('Нажата кнопка СТОП')
        self.stop_motor()
    
    def backward_button_callback(self, channel):
        self.get_logger().info('Нажата кнопка НАЗАД')
        self.move_backward()
    
    def move_forward(self):
        self.get_logger().info('Двигатель движется вперед')
        GPIO.output(self.ena_pin, GPIO.HIGH)  # Включить двигатель
        GPIO.output(self.in1_pin, GPIO.HIGH)  # Установить направление вперед
        GPIO.output(self.in2_pin, GPIO.LOW)
    
    def stop_motor(self):
        self.get_logger().info('Двигатель остановлен')
        GPIO.output(self.ena_pin, GPIO.LOW)   # Выключить двигатель
        GPIO.output(self.in1_pin, GPIO.LOW)
        GPIO.output(self.in2_pin, GPIO.LOW)
    
    def move_backward(self):
        self.get_logger().info('Двигатель движется назад')
        GPIO.output(self.ena_pin, GPIO.HIGH)  # Включить двигатель
        GPIO.output(self.in1_pin, GPIO.LOW)   # Установить направление назад
        GPIO.output(self.in2_pin, GPIO.HIGH)
    
    def timer_callback(self):
        # Здесь можно добавить публикацию состояния двигателя, если необходимо
        pass
    
    def __del__(self):
        # Очистка при завершении
        GPIO.cleanup()

def main(args=None):
    rclpy.init(args=args)
    motor_controller = MotorController()
    
    try:
        rclpy.spin(motor_controller)
    except KeyboardInterrupt:
        pass
    finally:
        motor_controller.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()

Установим библитеку python3-rpi-lgpio:

Bash

sudo apt install python3-rpi-lgpio

Откроем setup.py:

Bash

nano motor_control/setup.py

И добавим 'motor_controller = motor_control.motor_controller:main':

Python

entry_points={
    'console_scripts': [
        'motor_controller = motor_control.motor_controller:main',
    ],
},

Откроем package.xml:

Bash

nano motor_control/package.xml

И добавим:

XML

<depend>rclpy</depend>

Соберем пакет:

Bash

cd /data/wtf
colcon build --packages-select motor_control
source install/setup.bash

запустим код:

Bash

ros2 run motor_control motor_controller

Результат работы

Нажатия на кнопки:

***

Содержание

Политика конфиденциальности / ВК