Урок 17

Введение: Данная задача, является первой из группы задач на использование встроенных в ROS алгоритмов автономной навигации. В ней мы разберем как задавать роботу цель, к которой он будет двигаться автономно выбирая путь и избегая препятствий. В данных задачах мы не будем заниматься непосредственно созданием навигационных алгоритмов, мы будем использовать готовые механизмы уже реализованные в ROS. В целом про устройство автономной навигации в ROS вы можете прочитать здесь. Непосредственно перед занятием повторите с учениками теорию изложенную по ссылке, т.к. само практическое задание, по традиции для первых заданий серии - нетяжелое и недлинное.

Подготовка: Данную задачу нагляднее всего решать в комбинации полигона - "Офис". Попросите учащихся непосредственно перед выполнением написанной программы, при помощи управления через веб-интерфейс, поставьте робота в первую комнату офиса - спиной к зарядке. По внешней камере полигона убедитесь, чтобы перед роботом было достаточно свободного пространства для движения по прямой. Если программа написана некорректно, и робот делает что-то несогласованное, попросите учащихся прервать выполнение программы, остановить робота при при помощи управления через веб-интерфейс и вернуть его в первоначальную позицию.

Задача: После запуска программы, основываясь на данных получаемых от лидара, робот должен начать двигаться в точку с координатами (1, -1). Необходимо написать программу так, чтобы робот автоматически начал движение после запуска программы. Таким образом мы пытаемся программно реализовать функционал утилиты rviz по указанию целей роботу для движения по автономной навигации.

Общий алгоритм решения:

1. Запускаем ноду автономной навигации.

2. Создаем задание для встроенного в movebase Экшн-сервера.

3. Убеждаемся, что робот достиг требуемой точки.

Решение: Для начала импортируем все библиотеки и структуры данных, которые нам понадобятся. Это rospy, и actionlib для передачи задания action-серверу.

import rospy
import actionlib
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal

Структуры данных которые мы будем при этом использовать созданны в рамках пакета автономной навигации РОС и называются MoveBaseAction

Инициализируем ноду

rospy.init_node('mover')

Далее создадим функцию, которая в качестве аргумента будет принимать значения X и Y в системе координат одометрии робота и далее собирать структуру запроса и публиковать задание для action-сервера movebase

def move_to_target(x ,y):
    print("Moving to: ", x , " ", y)

    client = actionlib.SimpleActionClient('move_base', MoveBaseAction)
    client.wait_for_server()

Здесь мы просто принтим полученные в качестве аргументов функции координаты и создаем объект класса SimpleActionClient - который и будет общаться с сервером move_base

Далее у созданного объекта мы вызываем функцию wait_for_server(), которая блокирует поток выполнения программы до тех пор пока action-сервер move_base не включится.

Далее создадим и заполним структуру данных MoveBaseGoal, соответствующие поля заполняются значениями текущего времени, фрейма карты, координатами X и Y, переданными как аргументы в функцию, и w-компоненту кватерниона ориентации.

    goal = MoveBaseGoal()
    goal.target_pose.header.frame_id = "map"
    goal.target_pose.header.stamp = rospy.Time.now()
    goal.target_pose.pose.position.x = x
    goal.target_pose.pose.position.y = y
    goal.target_pose.pose.orientation.w = 1.0

Далее после заполнения структуры, мы отправляем ее на сервер при помощи экземпляра клиента, который мы создали в самом начале. И далее ждем ответа от сервера.

    client.send_goal(goal)
    wait = client.wait_for_result()

Затем мы обрабатываем результат работы сервера, и если все ок, завершаем нашу функцию, возвращая результат.

    if not wait:
        rospy.logerr("Action server not available!")
        rospy.signal_shutdown("Action server not available!")
    else:
        return client.get_result()

Мы почти закончили, теперь нам надо просто вызвать нашу функцию и передать в нее координаты (1, -1).

В итоге ваша программа должна иметь вот такой вид:

import rospy
import actionlib
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal

rospy.init_node('mover')

def move_to_target(x ,y):
    print("Moving to: ", x , " ", y)

    client = actionlib.SimpleActionClient('move_base', MoveBaseAction)
    client.wait_for_server()

    goal = MoveBaseGoal()
    goal.target_pose.header.frame_id = "map"
    goal.target_pose.header.stamp = rospy.Time.now()
    goal.target_pose.pose.position.x = x
    goal.target_pose.pose.position.y = y
    goal.target_pose.pose.orientation.w = 1.0

    client.send_goal(goal)
    wait = client.wait_for_result()
    if not wait:
        rospy.logerr("Action server not available!")
        rospy.signal_shutdown("Action server not available!")
    else:
        return client.get_result()

move_to_target(1,-1)

Проверка решения:

Попросите учащихся сохранить написанную программу, перенести ее на робота при помощи утилиты scp, затем запустить ноду автономной SLAM-навигации и затем в новом окне терминала запустить написанную программу.

roslaunch turtlebro_navigation turtlebro_slam_navigation.launch

python3 имя_программы.py

Попросите учащихся включить демонстрацию экрана в Discord и включить камеру робота и внешнюю камеру полигона, чтобы вы могли убедиться видеть как робот самостоятельно начал движение и едет в точку расположенную в метре прямо перед ним и метре направо.

Попросите учащихся чтобы при помощи программы nano, прямо на роботе поменять аргументы передаваемые в функцию на (0,0), снова запустить программу не выключая ноду навигации и убедиться, что робот вернулся в исходное положение.

На этом занятие первое занятие по автономной навигации закончено.