Продолжение. Начало — Часть1 и Часть2
Приступим к написанию скриптов для ROS
1. Запуск узла преобразования речи в текст ardrone1_julius_talker и выдача результатов в тему ardrone1_textspeech
Создадим файл ros_ardrone1_julius_init.py
Скрипт запускает в цикле программу julius с выдачей результата преобразования скрипту ros_ardrone1_julius_to_text.py
Т.к. система должна понимать не только полные команды, но и укороченные, создадим несколько параметров (данные, хранящиеся на сервере ROS), где будем хранить текущие значения для текущего робота, текущего действия, текущего значения действия — ardrone1_name, ardrone1_do, ardrone1_digit.
Вот содержимое файла ros_ardrone1_julius_init.py
#!/usr/bin/env python
# -*- coding: utf-8 -*-
# Запуск программы julius (распознование речи)
# входящие данные - микрофон
# перенаправление вывода результатов в скрипт ros_ardrone1_julius_to_text.py
#
import roslib; roslib.load_manifest('vp_ardrone1')
import rospy
from std_msgs.msg import String
import os
import subprocess
def run():
rospy.set_param("ardrone1_name",1);
rospy.set_param("ardrone1_do",[0,0]);
rospy.set_param("ardrone1_digit",[[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0],[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]]);
while not rospy.is_shutdown():
a = os.system('padsp julius -quiet -input mic -C julian.jconf 2>/dev/null | ./ros_ardrone1_julius_to_text.py')
rospy.sleep(1.0)
if __name__ == '__main__':
try:
run()
except rospy.ROSInterruptException: pass
except KeyboardInterrupt:
sys.exit(1)
Результаты передаются скрипту ros_ardrone1_julius_to_text.py, который только создает узел и публикует значение sentence1 в тему ardrone1_textspeech
Вот вывод результата
Содержимое файла ros_ardrone1_julius_to_text.py
#!/usr/bin/env python
# -*- coding: utf-8 -*-
# Узел ardrone1_julius_talker, получающий результат программы julius(распознование речи) из stdin
# (скрипт ros_ardrone1_julius_init.py) и публикующий строку в тему ardrone1_textspeech
#
import roslib; roslib.load_manifest('vp_ardrone1')
import rospy
from std_msgs.msg import String
import os
import subprocess
import sys
def Control(file_object):
pub = rospy.Publisher('ardrone1_textspeech', String)
rospy.init_node('ardrone1_julius_talker')
startstring = 'sentence1: '
endstring = ' '
while not rospy.is_shutdown():
line = file_object.readline()
if not line:
break
end = len(line)-6
if line[0:9] == 'sentence1':
word = line[15:end]
print word
str = word
rospy.loginfo(str)
pub.publish(String(str))
rospy.sleep(0.001)
if __name__ == '__main__':
try:
Control(sys.stdin)
except rospy.ROSInterruptException: pass
except KeyboardInterrupt:
sys.exit(1)
2. Запуск слушателя (subscriber) ardrone1_textspeech, преобразование текста в цифровую команду и выдача результатов в тему ardrone1_command
Скрипт ros_ardrone1_text_to_command_julius.py создает промежуточный узел ros_ardrone1_sub_voice, который является слушателем (subscriber) темы ardrone_textspeech. Его задача:
получив текст, дополнить его до полной команды (из данных rosparam), например forward slow ->ardrone forward slow, seven -> ardrone dance seven
сформировать цифровую форму кманды, отсечь неверные команды, например ardrone hi slow — неверно, ardrone off seven — неверно
выдать цифровую форму команды в тему ardrone1_command
Все слова из словаря разбиты на 6 групп, каждая из которых имеет свой коэффициент k
«DRONE»,»ROBERT» — k=100 000
«HI»,»BUY»,»THANKS» — k=10 000
«ON»,»OFF»,»SLEEP»,»BASE» — k=1000
«FORWARD»,»BACK»,»LEFT»,»RIGHT»,»TURN»,»UP»,»DOWN»,»HANG»,»DANCE» — k=100
«ONE»,»TWO»,»THREE»,»FOUR»,»FIVE»,»SIX»,»SEVEN»,»EIGHT»,»NINE» — k=10
«FAST»,»SLOW» — k=1
Цифровая форма команды формируется так
count2=слово1*k*(индекс слова в группе+1)+слово2*k*(индекс слова в группе+1)+…..
Одновременно формируется и число
count1=слово1*k*1+слово2*k*1+…..
Например
DRONE THANKS RIGHT FAST
count2=100000*(0+1)+10000*(2+1)+100*(3+1)+1*(0+1)=130401
count1=100000*1+10000*1+100*1+1*1=110101
Список возможных комбинаций для count1 находится в массиве ok_fraza
ok_fraza=[
«110000»,»010000″,
«101000»,»001000″,
«100100»,»000100″,»100110″,»000110″
«100001»,»000001″,»100101″,»000101″]
Если count1 для нашей фразы присутствует в массиве ok_fraza, в тему ardrone1_command публикуем значение count2
Кроме этого в скрипте устанавливаются значения параметров (ardrone1_name, ardrone1_do, ardrone1_digit) на сервере параметров
Содержимое файла ros_ardrone1_text_to_command_julius.py
#!/usr/bin/env python
#-*-coding:utf-8 -*-
#
# Слушатель ros_ardrone1_sub_voice
# чтение сообщений из темы ardrone1_textspeech (строка голосовой команды)
# и отправка команд и установка параметров
# для управления ardrone 2.0
#
import roslib; roslib.load_manifest('vp_ardrone1')
import rospy
import subprocess
import shlex
from ardrone_autonomy.msg import *
from ardrone_autonomy.srv import *
from std_msgs.msg import String
from std_msgs.msg import Int32
from std_msgs.msg import Empty
from geometry_msgs.msg import Twist
arr_commands=[["DRONE","ROBERT"],
["HI","BUY","THANKS"],
["ON","OFF","SLEEP","BASE"],
["FORWARD","BACK","LEFT","RIGHT","TURN","UP","DOWN","HANG","DANCE"],
["ONE","TWO","THREE","FOUR","FIVE","SIX","SEVEN","EIGHT","NINE"],
["FAST","SLOW"]]
digits=[100000,10000,1000,100,10,1]
ok_fraza=[
"110000","010000",
"101000","001000",
"100100","000100","100110","000110"
"100001","000001","100101","000101"]
def controller(data):
if(data.data>0):
str = data.data
rospy.loginfo(str)
arr_str=str.split();
count1=0;count2=0;
# разбор предложения
#str1="";str2="";
for word in arr_str:
for ind1,frazes in enumerate(arr_commands):
for ind2,fraza in enumerate(frazes):
if fraza==word:
count1=count1+digits[ind1]
count2=count2+digits[ind1]*(ind2+1)
## добавить из параметров умолчания
# персонаж по умолчанию
if(count2/100000<1):
count2=rospy.get_param("ardrone1_name")*100000+count2;
else:
rospy.set_param("ardrone1_name",count2/100000);
# действие по умолчанию
if((count2%1000)/100<1):
params2=rospy.get_param("ardrone1_do")
count2=params2[rospy.get_param("ardrone1_name")-1]*100+count2
else:
params2=rospy.get_param("ardrone1_do")
params2[rospy.get_param("ardrone1_name")-1]=(count2%1000)/100
rospy.set_param("ardrone1_do",params2)
params3=rospy.get_param("ardrone1_digit")
params3[rospy.get_param("ardrone1_name")-1]=[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]
params3[rospy.get_param("ardrone1_name")-1][(count2%1000)/100-1]=0.1
rospy.set_param("ardrone1_digit",params3)
if((count2%1000)/100==8):
params3=rospy.get_param("ardrone1_digit")
params3[rospy.get_param("ardrone1_name")-1]=[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]
rospy.set_param("ardrone1_digit",params3)
# скорость текущая
if((count2%100)/10>0):
params3=rospy.get_param("ardrone1_do")
tdo=params3[rospy.get_param("ardrone1_name")-1]
params4=rospy.get_param("ardrone1_digit")
params4[rospy.get_param("ardrone1_name")-1]=[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]
params4[rospy.get_param("ardrone1_name")-1][tdo-1]=((count2%100)/10*1.0)/10
rospy.set_param("ardrone1_digit",params4)
if(tdo==8):
params4=rospy.get_param("ardrone1_digit")
params4[rospy.get_param("ardrone1_name")-1]=[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]
rospy.set_param("ardrone1_digit",params4);
# быстрее, медленнее
if((count2%10)==1):
params3=rospy.get_param("ardrone1_do")
tdo=params3[rospy.get_param("ardrone1_name")-1]
params4=rospy.get_param("ardrone1_digit")
tdigit=params4[rospy.get_param("ardrone1_name")-1][tdo-1]
params4[rospy.get_param("ardrone1_name")-1][tdo-1]=min(1.0,tdigit*10.0/10+0.1)
rospy.set_param("ardrone1_digit",params4)
elif((count2%10)==2):
params3=rospy.get_param("ardrone1_do")
tdo=params3[rospy.get_param("ardrone1_name")-1]
params4=rospy.get_param("ardrone1_digit")
tdigit=params4[rospy.get_param("ardrone1_name")-1][tdo-1]
params4[rospy.get_param("ardrone1_name")-1][tdo-1]=max(0.0,tdigit*10.0/10-0.1)
rospy.set_param("ardrone1_digit",params4)
# лог
rospy.loginfo(String(count2))
# публиковать в тему ardrone1_move
pub1=rospy.Publisher('ardrone1_command', Int32)
pub1.publish(count2)
#rospy.loginfo(str)
def listener():
rospy.init_node('ros_ardrone1_sub_voice')
sub = rospy.Subscriber("ardrone1_textspeech",String,controller)
rospy.spin()
if __name__ == '__main__':
listener()
3. Преобразование цифровой формы команды из темы ardrone1_command в сообщения для слушателей и сервисов пакета ardrone_autonomy
Обычно для управления Ardrone посылаются единичные команды сервисам или в тему. Исключения составляют команды для управления движениям arDrone, для движения Ardron в каком-нибудь направлении необходимо посылать ПОСТОЯННО сообщения в тему cmd_vel. Поэтому я создал два узла-слушателя subscriber из темы ardrone1_command. Один отправляет единичные команды, другой при необходимости ПОСТОЯННЫЕ в тему cmd_vel.
Скрипт ros_ardrone1_command_to_move.py — для одиночных сообщений
#!/usr/bin/env python
#-*-coding:utf-8 -*-
#
# Слушатель ros_ardrone1_command
# чтение сообщений из темы ardrone1_command (строка голосовой команды)
# и отправка команд и установка параметров
# для управления ardrone 2.0
# или iRobotCreate
import roslib; roslib.load_manifest('vp_ardrone1')
import rospy
import subprocess
import shlex
from ardrone_autonomy.msg import *
from ardrone_autonomy.srv import *
from std_msgs.msg import String
from std_msgs.msg import Empty
from std_msgs.msg import Int32
from geometry_msgs.msg import Twist
def controller(data):
if(data.data>0):
command = data.data
if(command/100000==1): # ArDrone 2.0
### приветствие
rospy.loginfo("ArDrone 2.0")
if((command%100000)/10000==1): # привет - анимация
rospy.loginfo("привет")
serv1=rospy.ServiceProxy('ardrone/setledanimation', LedAnim)
res1=serv1(1,4,3);
rospy.loginfo(res1)
# параметры
elif((command%100000)/10000==2): # пока - анимация
rospy.loginfo("пока")
serv1=rospy.ServiceProxy('ardrone/setledanimation', LedAnim)
res1=serv1(2,4,3);
rospy.loginfo(res1)
elif((command%100000)/10000==3): # молодец - анимация
rospy.loginfo("молодец")
serv1=rospy.ServiceProxy('ardrone/setledanimation', LedAnim)
res1=serv1(3,4,5);
rospy.loginfo(res1)
### взлет, посадка
elif((command%10000)/1000==1): # взлет
rospy.loginfo("взлет")
pub1=rospy.Publisher('ardrone/takeoff', Empty)
pub1.publish()
# параметры
elif((command%10000)/1000==2): # посадка
rospy.loginfo("посадка")
pub2=rospy.Publisher('ardrone/land', Empty)
pub2.publish()
elif((command%10000)/1000==3): # заснуть, сброс
rospy.loginfo("заснуть")
pub2=rospy.Publisher('ardrone/reset', Empty)
pub2.publish()
### dance анимация
elif((command%1000)/100==9): # полетная анимация
rospy.loginfo("flight анимации - па !!!!")
serv1=rospy.ServiceProxy('ardrone/setflightanimation',FlightAnim)
res1=serv1((command%100)/10,0);
rospy.loginfo(res1)
elif(command/100000==2): # iRobot
rospy.loginfo("iRorot Robert")
def listener():
rospy.init_node('ros_ardrone1_command')
sub = rospy.Subscriber("ardrone1_command",Int32,controller)
rospy.spin()
if __name__ == '__main__':
listener()
Скрипт ros_ardrone1_command_to_move_odom.py — для одиночных сообщений — проверяет состояние голосовой команда для ArDron (по ненулевым значениям ardrone_digit) и запускает посылку сообщений в тему cmd_vel. При выдаче новой голосовой команды изменяются значения параметра ardrone1_digit, и при переходе их в нулевые — посылка команд в тему cmd_vel прекращается.
Скрипт ros_ardrone1_command_to_move_odom.py
#!/usr/bin/env python
#-*-coding:utf-8 -*-
#
# Слушатель ros_ardrone1_command
# чтение сообщений из темы ardrone1_command (строка голосовой команды)
# и отправка команд в тему cmd_vel
# для управления ardrone 2.0
# или iRobotCreate
import roslib; roslib.load_manifest('vp_ardrone1')
import rospy
import subprocess
import shlex
from ardrone_autonomy.msg import *
from ardrone_autonomy.srv import *
from std_msgs.msg import String
from std_msgs.msg import Empty
from std_msgs.msg import Int32
from geometry_msgs.msg import Twist
def controller(data):
if(data.data>0):
command = data.data
### вперед, назад, влево, вправо, поворот, вверх, вниз, медленнее, быстрее
if(command/100000==1): # ArDrone 2.0
if((command%1000)/100>0): #
go=True
while go==True:
params1=rospy.get_param("ardrone1_name")
params2=rospy.get_param("ardrone1_do")
params3=rospy.get_param("ardrone1_digit")
fb=params3[params1-1][0]-params3[params1-1][1]
rl=params3[params1-1][2]-params3 [params1-1][3]
ud=params3[params1-1][4]-params3[params1-1][5]
tt=params3[params1-1][6]
if(abs(fb)>0.0 or abs(rl)>0.0 or abs(ud)>0.0 or abs(tt)>0.0):
go=True
else:
go=False
pub3=rospy.Publisher('cmd_vel', Twist)
odom=Twist()
odom.linear.x=fb
odom.linear.y=rl
odom.linear.z=ud
odom.angular.x=0.0
odom.angular.y=0.0
odom.angular.z=tt
pub3.publish(odom)
rospy.loginfo("движение!!!!")
rospy.loginfo("конец движения!!!!")
elif(command/100000==2): # iRobot
rospy.loginfo("iRorot Robert")
def listener():
rospy.init_node('ros_ardrone1_command_odom')
sub = rospy.Subscriber("ardrone1_command",Int32,controller)
rospy.spin()
if __name__ == '__main__':
listener()
В проект включаем для подстраховки скрипты для управления квадрокоптера ArDrone с помощью джойстика,
$ rosparam set joy_node/dev "/dev/input/jsX0" $ rosrun joy joy_node $ rosrun vp_ardrone1 ros_ardrone1_joystick_to_move.py
где при нажатии кнопки RB6 происходит обнуление данных в параметре ardrone_digit и соответственно останов Ardron-а.
Теперь можно подключаться к Ardronб запускать драйвер ardrone_autonomy и проверять работу пакета.
Далее — Часть 4 — пошаговая инструкция по запуску системы и видео.