Начало весны, пора приниматься за недоделанные проекты.
Пока жду детали и вспоминаю ROS.
В качестве разминки (надеюсь после разминки будут более интересные проекты) — небольшая игра в ROS с использованием пакета turtlesim.
ROS — (Robot Operating System) Операционная система для роботов — это фреймворк для программирования роботов, предоставляющий функционал для распределённой работы. Хорошие мануалы (с которых я начинал изучение ROS) на русском языке на этом сайте — здесь
Последняя (восьмая) версия ROS — indigo.
Сначала установил ее на компьютер с операционной системой Ubuntu 14.04. (Инструкция по установке и настройке)
Пакет Turtlesim используют в примерах по изучению ROS. Запуск пакета
rosrun turtlesim turtlesim_node
Появляется окно, в котором расположена черепашка turtle, управлять которой можно отправкой сообщений в темы или сервисы. Есть также возможность управления движением черепашки с помощью клавиатуры.
На самом деле возможности пакета шире, можно создавать любое количество экземпляров черепашки, управлять их перемещением, устанавливать цвет и размер следа черепашки, цвет поля, отслеживать текущие значения координат и текущих скоростей черепашки.
Посмотреть описание пакета turtlesim можно здесь
Итак, игра на ROS c использованием пакета Turtlesim. Движением черепашки будем управлять с помощью геймпада Defender Game Racer X7.
Defender Game Racer X7 имеет 12 кнопок (включая D-Pad и 2 аналоговых джойстика), а также кнопки Turbo, Clear и Home. Устройство поддерживает вибрационную обратную связь, работающую при помощи 2-х вибромоторов. Подключение к компьютеру производится через интерфейс USB. Джойстик может работать в двух режимах, один из которых HID-устройство, другой – контроллер XBOX360 . Переключение производится с помощью кнопки Mode.
Проверяем, опознается ли данный геймпад системой. Набираем в терминале команду
ls /dev/input
и смотрим результат выполнения команды
Джойстик устройства называются по jsX, у меня было js0. Удостоверимся, что джойстик работает. Выполним в терминале команду
sudo jstest /dev/input/js0
И смотрим изменение показаний кнопок и джойстиков
4.1. ROS пакет joy.
ROS пакет joy – драйвер джойстика уже установлен в системе. По умолчанию пакет joy использует устройство /dev/input/js0. Если ваш джойстик имеет другое название (например /dev/input/js1), необходимо перед запуском пакета установить параметр joy_node/dev на сервере параметров:
rosparam set joy_node/dev "/dev/input/js1"
Для запуска
rosrun joy joy_node
При запуске пакет выдает сообщение о подключении джойстика
Пакет создает узел ros node_joy, который публикует в тему joy сообщения типа sensor_msgs/Joy
Посмотреть выдачу сообщений узлом joy_node в тему joy можно по команде
rostopic echo joy
И смотрим сообщения в тему joy при нажатии кнопок и передвижении джойстиков нашего геймпада
Наша задача создать узел, который будет слушателем (subscriber) сообщений из темы joy. Этот узел будет обрабатывать эти сообщения и посылать сообщения в темы и сервисы пакета turtlesim для управления черепахами.
СОЗДАНИЕ ИГРЫ
Суть игры следующая – на поле через некоторое время в случайных позициях появляются черепахи. Задача игрока – управляя с помощью джойстика своей черепахой turtle1 наезжать на этих черепах (при наезде черепаха исчезает).
Создаем в рабочем пространстве catkin новый пакет game_turtlesim.
cd ~/catkin_ws/src catkin_create_pkg game_turtlesim std_msgs rospy joy turtlesim
При этом сразу указываем зависимости пакета. После создания пакета В директории ~/catkin_ws/src появится новая директория с файлами нового, только что созданного пакета game_turtlesim.
Теперь необходимо отредактировать файлы package.xml и CmakeLists.txt.
В файле package.xml мы заполняем номер версии, данные об авторе, сведения о лицензии. Зависимости пакета указаны в блоках
В данном проекте мы не собираемся использовать сообщения пользовательского типа, поэтому других изменений в файле package.xml и CmakeLists.txt мы делать не будем.
Необходимо реализовать
— управление с помощью джойстика движением черепахи turtle1;
— определение текущего местонахождения нашей движущейся черепахи;
— создание на поле черепах через определенное время;
— проверку столкновения нашей движущейся черепахи с черепахами на поле (в случае наезда – удаление черепахи).
Управление с помощью джойстика движением черепахи turtle1
Скрипт на языке python, управляющий движением черепахи turtle1 с помощью джойстиков геймпада Defender Game Racer X7. Узел должен являться слушателем (subscriber) темы joy и преобразовывать получаемые данные и формировать запросы в сервис /turtle1/teleport_relative для управления движением черепахи turtle1. Т.к. узел joy_node при перемещениях джойстиков публикует сообщения в тему joy очень часто, сервис /turtle1/teleport_relative не будет успевать обрабатывать эти сообщения и они будут ставиться в очередь, что приведет к сильному замедлению реакции черепахи turtle1 на перемещения джойстиков геймпада. Поэтому я поступил следующим образом. Subscriber будет только обновлять последние значения показаний джойстиков и сохранять их в параметре example_1/joy_tek на сервере параметров, а другой скрипт будет раз в 0.1 сек получать значения параметра example_1/joy_tek и отправлять запросы в сервис /turtle1/teleport_relative. Это приведет к потере реакции на сообщения, публикуемые в тему joy, но задержки не будет.
Скрипт turtlesim_joy.py
Создаем файл
roscd game_turtlesim mkdir nodes cd nodes touch turtlesim_joy.py chmod +x turtlesim_joy.py
И сам скрипт
#!/usr/bin/env python
#-*-coding:utf-8 -*-
import roslib; roslib.load_manifest('game_turtlesim')
import rospy
import time
from sensor_msgs.msg import Joy
import std_srvs.srv
from turtlesim.srv import SetPen
from turtlesim.srv import TeleportAbsolute
def controller(data):
# запись данных с джойстиков
rospy.loginfo(str(data.axes[0])+" "+str(data.axes[3]))
joy_tek=rospy.get_param("game_turtlesim/joy_tek")
joy_tek[0]=data.axes[0]
joy_tek[1]=data.axes[3]
rospy.set_param("game_turtlesim/joy_tek",joy_tek)
def listener():
rospy.init_node('sub_turtle1_joy')
# установка параметров
rospy.set_param("game_turtlesim/start",1)
rospy.set_param("game_turtlesim/joy_tek",[0.0, 0.0])
# установить цвет фона
rospy.set_param("background_r",255)
rospy.set_param("background_g",255)
rospy.set_param("background_b",0)
serv1=rospy.ServiceProxy('/clear',std_srvs.srv.Empty)
res1=serv1();
# установить цвет карандаша для turtle1
serv1=rospy.ServiceProxy('/turtle1/set_pen',SetPen)
res1=serv1(255,255,0,10,0);
# черепаху в начальную позицию
serv1=rospy.ServiceProxy('turtle1/teleport_absolute',TeleportAbsolute)
res1=serv1(0.0,0.0,0.89);
# пауза - отдышаться перед игрой
rospy.sleep(1.0)
sub = rospy.Subscriber("joy",Joy,controller)
rospy.spin()
if __name__ == '__main__':
listener()
Отклонения влево/вправо левого джойстика (поворот влево/вправо черепахи) и отклонения вперед/назад правого джойстика (движение черепахи вперед/назад). Одним джойстиком осуществлять управление неудобно.
Движение черепахи реализуется в основном скрипте turtle1_go.py
Вот код, реализующий движение turtle1
if rospy.get_param("game_turtlesim/start")==1:
joy_tek=rospy.get_param("game_turtlesim/joy_tek")
speed=joy_tek[1]/K_SPEED
k=1
if joy_tek[1]<0:
k=-1
angle=math.asin(joy_tek[0]*k)/K_ANGLE
res1=serv1(speed,angle);
rospy.sleep(0.1)
Определение текущего местонахождения нашей движущейся черепахи
Узел turtlesim_node публикует в тему turtleX/pose сообщения типа turtlesim/Pose о текущей позиции и скоростях передвижения черепахи. Дополнительный скрипт turtle1_pose.py, создающий узел sub_turtle1_pose, который будет являться слушателем (subscriber) сообщений из темы turtle1/pose. Значения x и y мы будем сохранять в параметре game_turtlesim/turtle1_pose на сервере параметров. Значения этого параметра мы будет использовать в основном скрипте turtle1_go.py. Содержимое скрипта turtle1_pose.py
#!/usr/bin/env python
#-*-coding:utf-8 -*-
import roslib; roslib.load_manifest('game_turtlesim')
import rospy
import time
from turtlesim.msg import Pose
def controller(data):
# запись данных положения turtle1
rospy.set_param("game_turtlesim/turtle1_pose",[data.x,data.y])
rospy.loginfo("x="+str(data.x)+" y="+str(data.y))
def listener():
rospy.init_node('sub_turtle1_pose')
# начальная установка параметров
rospy.set_param("game_turtlesim/turtle1_pose",[0.0, 0.0])
# объявление subcriber
sub = rospy.Subscriber("turtle1/pose",Pose,controller)
rospy.spin()
if __name__ == '__main__':
listener()
Cоздание на поле черепах через определенное время
Код создания на поле черепах через определенное время. Добавление черепах на поле в происходит при отправке запроса к сервису spawn. Формат сообщения сервису spawn – turtlesim/Spawn:
float32 x
float32 y
float32 theta
string name # Optional. A unique name will be created and returned if this is empty
---
string name
Т.е. нам необходимо отправить значения координат на поле, угол поворота (в радианах) и имя. Список черепах (1 – есть, 0 – нет) и их координаты на поле, а также текущее значение количества черепах будем сохранять в параметрах game_turtlesim/list_turtles, game_turtlesim/coord_turtles, game_turtlesim/count_turtles на сервере параметров rosparam. Параметр game_turtlesim/time считает время выполнения скрипта, чтобы мы могли отслеживать задержку перед появлением следующей черепахи. Код данного фрагмента
# создание черепах turtle00-turtle010
count_turtles=rospy.get_param("game_turtlesim/count_turtles")
count=rospy.get_param("game_turtlesim/count")
if rospy.get_param("game_turtlesim/start")==1 and count_turtles<10 and
time.time()-rospy.get_param("game_turtlesim/time")>(T_SPEED-max(2,count*0.03)):
count_turtles=count_turtles+1
list_turtles=rospy.get_param("game_turtlesim/list_turtles")
coord_turtles=rospy.get_param("game_turtlesim/coord_turtles")
ind=list_turtles.index(0,0,10)
prv1=False
while prv1==False:
x=random.randrange(5,110,5)
y=random.randrange(5,110,5)
theta=0.1*random.randint(1,10)
tek_turtle="turtle0"+str(ind)
res2=serv2(0.1*x,0.1*y,theta,tek_turtle);
rospy.loginfo("create turtle="+tek_turtle)
list_turtles[ind]=1
coord_turtles[ind]=[x,y]
rospy.loginfo(list_turtles)
rospy.set_param("game_turtlesim/list_turtles",list_turtles)
rospy.set_param("game_turtlesim/coord_turtles",coord_turtles)
rospy.set_param("game_turtlesim/time",time.time())
rospy.set_param("game_turtlesim/count_turtles",count_turtles)
prv1=True
Проверка столкновения нашей движущейся черепахи с черепахами на поле
Проверку столкновения нашей движущейся черепахи с черепахами на поле с помощью перебора координат всех черепах на поле (параметр game_turtlesim/coord_turtles) с текущим положением нашей движущейся черепахи turtle1 (параметр game_turtlesim/turtle1_pose) с погрешностью 3 пиксела. В случае наезда удаление черепахи (запрос сервису kill) и внесение значения нулевых координат [0.0, 0.0] для удаленной черепахи в массиве game_turtlesim/coord_turtles и 0 в массиве game_turtlesim/list_turtles. При уничтожении черепахи инкрементируем счетчик игры (параметр game_turtlesim/count на сервере параметров rosparam). Код данного фрагмента
# проверка столкновения turtle1 с turtle00-turtle010
count=rospy.get_param("game_turtlesim/count")
list_turtles=rospy.get_param("game_turtlesim/list_turtles")
coord_turtles=rospy.get_param("game_turtlesim/coord_turtles")
xy=rospy.get_param("game_turtlesim/turtle1_pose")
count_turtles=rospy.get_param("game_turtlesim/count_turtles")
x1=int(xy[0]*10)
y1=int(xy[1]*10)
i=0
for xy in coord_turtles:
if abs(xy[0]-x1)<4 and abs(xy[1]-(110-y1))<4 and list_turtles[i]==1:
turtle_del="turtle0"+str(i)
rospy.loginfo(str(xy[0])+" "+str(xy[1])+" "+str(x1)+" "+str(y1)+" "+turtle_del)
res3=serv3(turtle_del);
list_turtles[i]=0
coord_turtles[i]=[0,0]
count_turtles=count_turtles-1
rospy.set_param("game_turtlesim/count",count+1)
rospy.set_param("game_turtlesim/count_turtles",count_turtles)
rospy.set_param("game_turtlesim/list_turtles",list_turtles)
rospy.set_param("game_turtlesim/coord_turtles",coord_turtles)
i=i+1
И запуск
Командный файл game_turtlesim.launch
В первом терминале
roscore
Во втором терминале
roslaunch game_turtlesim game_turtlesim.launch
Проект данного примера можно скачать с github (https://github.com/victoruni/game_turtlesim.git)