Передвижение робота(Python)

Что вы узнаете в этом разделе:

• Как создать программу для управления движением робота на Python
• Как правильно остановить робота при завершении программы
• Как работать со временем в ROS2 программах
• Как заставить робота двигаться в течение заданного времени
• Как обрабатывать прерывания программы (Ctrl+C)

Пример программы движения робота

В прошлой главе, мы создали Издателя, который публиковал данные температуры. Также мы знаем, что для управления роботом, необходимо отправить сообщение Twist в топик cmd_vel

Рассмотрим простой пример программы, которая заставляет робота передвигать прямо.

#!/usr/bin/env python3

import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist

import time

class MoveStraightNode(Node):
    def __init__(self):
        super().__init__('move_straight_node')
        
        # Создаем publisher для топика /cmd_vel
        self.publisher = self.create_publisher(Twist, '/cmd_vel', 10)
        
        # Создаем таймер для периодической отправки команд
        timer_period = 0.5  # секунды (2 Гц)
        self.timer = self.create_timer(timer_period, self.timer_callback)
        
        self.get_logger().info('Move straight node started')
    
    def timer_callback(self):
        # Создаем сообщение Twist для движения прямо
        msg = Twist()
        
        # Линейная скорость по оси X (вперед)
        msg.linear.x = 0.2  # м/с - скорость движения вперед
        msg.linear.y = 0.0
        msg.linear.z = 0.0
        
        # Угловая скорость (вращение) - 0 для движения прямо
        msg.angular.x = 0.0
        msg.angular.y = 0.0
        msg.angular.z = 0.0
        
        # Публикуем сообщение
        self.publisher.publish(msg)
        
        # Логируем отправленную команду (опционально)
        self.get_logger().info(f'Moving straight with speed: {msg.linear.x} m/s')

def main(args=None):
    rclpy.init(args=args)
    
    move_straight_node = MoveStraightNode()
    
    try:
        rclpy.spin(move_straight_node)
    except (KeyboardInterrupt):
        pass

if __name__ == '__main__':
    main()

Сохраним программу в файл /home/pi/ros2-base/chapter3/publisher_cmd_vel.py и запустим его.

python3 publisher_cmd_vel.py
[INFO] [1757341419.088200400] [move_straight_node]: Move straight node started
[INFO] [1757341419.572211300] [move_straight_node]: Moving straight with speed: 0.2 m/s
[INFO] [1757341420.072226781] [move_straight_node]: Moving straight with speed: 0.2 m/s
[INFO] [1757341420.572120985] [move_straight_node]: Moving straight with speed: 0.2 m/s
[INFO] [1757341421.072242984] [move_straight_node]: Moving straight with speed: 0.2 m/s
[INFO] [1757341421.572182373] [move_straight_node]: Moving straight with speed: 0.2 m/s
[INFO] [1757341422.072247891] [move_straight_node]: Moving straight with speed: 0.2 m/s

Мы видим что робот начал движение прямо.

Остановка робота

Если мы прервем работу программы, робот продолжит движение. Это не очень удобно.

Мы ожидаем, что если мы остановили программу движения робота, то робот тоже должен остановиться.

Для начала, нам необходима функция остановки робота, которая установит все скорости робота в нулевые значения.

Например, функция может выглядеть так

    def stop_robot(self):

        self.get_logger().info('Stopping robot')

        #Остановить робота, опубликовать пустое сообщение
        stop_msg = Twist()
        self.publisher.publish(stop_msg)

И второе, нам необходимо "поймать" выход из программы и перед ее завершением вызвать функцию stop_robot

Для этого нем необходимо немного переписать блок инициализации ноды, отключив "стандартный" обработчик выхода, указав signal_handler_options для метода инициализации rclpy.init()

def main(args=None):
    
    rclpy.init(args=args, signal_handler_options=SignalHandlerOptions.NO)
    move_straight_node = MoveStraightNode()
    
    try:
        rclpy.spin(move_straight_node)
    except (KeyboardInterrupt):
        move_straight_node.get_logger().info('Program interruption by user')
        # Остановим робота
        move_straight_node.stop_robot()

    finally:
        # Уничтожаем узел
        move_straight_node.destroy_node()
        # Завершаем работу ROS2
        rclpy.try_shutdown()

Работа со временем

Часто нам необходимо, чтобы робот выполнял какую-либо работу по времени. Например, мы хотим чтобы робот ехал прямо 5 секунд и после останавливался. Останавливаться мы уже умеем, значит нам необходимо "правильно" посчитать время.

Для начала, создадим две переменных. В одной будем хранить время старта программы (self.start_time), а в другой необходимое время движения (self.duration).

import time

    def __init__(self):
        ####
        self.start_time = time.time()
        self.duration = 5.0  

Также изменим функцию timer_callback, добавив в нее логику проверки времени.

    def timer_callback(self):
        # Создаем сообщение Twist для движения прямо
        msg = Twist()

        #Проверяем время, если условие выполняется, продолжаем движение
        #Если нет, то выполняем остановку робота и выход из программы 
        if time.time() - self.start_time < self.duration:
        
            msg.linear.x = 0.2  # м/с - скорость движения вперед
            self.publisher.publish(msg)
            
            self.get_logger().info(f'Moving straight with speed: {msg.linear.x} m/s')
        else:
            self.get_logger().info(f'Stoping robot after {self.duration} sec mooving')

            self.stop_robot()
            rclpy.try_shutdown()

Финальный примеры программы, доступны в разделе примеры на GitHub turtlebro2-examples