Голосовое управление квадрокоптером ArDrone 2.0 через ROS.Часть 3

Продолжение. Начало — Часть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: <s> '
       endstring = ' </s>'
 
       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 — пошаговая инструкция по запуску системы и видео.

Комментарии (0)

RSS свернуть / развернуть

Только зарегистрированные и авторизованные пользователи могут оставлять комментарии.