Голосовое управление квадрокоптером 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:  '
       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 — пошаговая инструкция по запуску системы и видео.


Добавить комментарий

Arduino

Что такое Arduino?
Зачем мне Arduino?
Начало работы с Arduino
Для начинающих ардуинщиков
Радиодетали (точка входа для начинающих ардуинщиков)
Первые шаги с Arduino

Разделы

  1. Преимуществ нет, за исключением читабельности: тип bool обычно имеет размер 1 байт, как и uint8_t. Думаю, компилятор в обоих случаях…

  2. Добрый день! Я недавно начал изучать программирование под STM32 и ваши уроки просто бесценны! Хотел узнать зачем использовать переменную типа…

3D-печать AI Android Arduino Bluetooth CraftDuino DIY IDE iRobot Kinect LEGO OpenCV Open Source Python Raspberry Pi RoboCraft ROS swarm ИК автоматизация андроид балансировать бионика версия видео военный датчик дрон интерфейс камера кибервесна манипулятор машинное обучение наше нейронная сеть подводный пылесос работа распознавание робот робототехника светодиод сервомашинка собака управление ходить шаг за шагом шаговый двигатель шилд юмор

OpenCV
Робототехника
Будущее за бионическими роботами?
Нейронная сеть - введение