3. Написание своего действия для head_controller

Кроме изменения стандартных действий можно добавлять свои! Для этого нужно проделать 3 шага:

1. Добавление голосовой команды во множество распознаваемых команд

2. Создание скрипта действий

3. Модификация пакета head_controller для работы с новым действием

Рассмотрим пример создания действия, по которому голова будет улыбаться. Эдакая, собака-улыбака:

(:

---

1. Добавление голосовой команды во множество распознаваемых команд

Этот шаг во многом повторяет описание пакета voice_recognizer. Наша задача - заставить речевой движок распознавать команду "Слушай, Морс! Улыбнись!"

1. Файл формальных грамматик ~/robohead_ws/src/robohead/voice_recognizer/config/gram.gram содержит:

#JSGF V1.0;

grammar robot_cmd;

public <commands> = <command> ;

<command> = <command_1> | <command_2> | <command_3> | 
            <command_4> | <command_5> | <command_6> | 
            <command_7>; 

<command_1> = голос ;
<command_2> = дай ( другую | левую | правую ) лапу ;
<command_3> = сидеть ;
<command_4> = лежать ;
<command_5> = вставай ;
<command_6> = отключись ;
<command_7> = что видишь ;

Добавим поле <command_8> = улыбнись ; и включим это поле в <command>, получится:

#JSGF V1.0;

grammar robot_cmd;

public <commands> = <command> ;

<command> = <command_1> | <command_2> | <command_3> | 
            <command_4> | <command_5> | <command_6> | 
            <command_7> | <command_8> ; 

<command_1> = голос ;
<command_2> = дай ( другую | левую | правую ) лапу ;
<command_3> = сидеть ;
<command_4> = лежать ;
<command_5> = вставай ;
<command_6> = отключись ;
<command_7> = что видишь ;
<command_8> = улыбнись ;

1. Добавим новое слово "улыбнись" в файл-словарь ~/robohead_ws/src/robohead/voice_recognizer/config/dictionary.txt

   слушайморс
   голос
   дай
   левую
   правую
   другую
   лапу
   сидеть
   лежать
   проснись
   усни
   что
   видишь
   улыбнись

2. Сконвертируем файл-словарь в фонетический словарь

cd ~/robohead_ws/src/robohead/voice_recognizer/config/ru4sphinx/text2dict

./dict2transcript.pl ~/robohead_ws/src/robohead/voice_recognizer/config/dictionary.txt ~/robohead_ws/src/robohead/voice_recognizer/config/dictionary.dic

1. Получим файл dictionary.dic не забудьте в нем подправить команды под особенности произношения: например, не мАрс - а мОрс (можно просто скопировать файл ниже)

видишь vv ii dd i sh
голос g oo l ay s
дай d aa j
другую d r u g uu uj
лапу l aa p u
левую ll je v u uj
лежать ll je zh aa tt
правую p r aa v u uj
проснись p r aa s nn i ss
сидеть ss i dd je tt
слушайморс s l uu sh ay j m oo r s
улыбнись u l y b nn ii ss
усни u s nn ii
что ch t oo
что(2) sh t oo

---

2. Создание скрипта действий

1. Перейдите в директорию ~/robohead_ws/src/robohead/head_controller/scripts/actions и создайте директорию для своего действия:

mkdir smile
cd smile

1. Внутри этой директории создадим файл скрипта smile.py и добавим медиа-файлы smile.png и smile.mp3 (через SFTP или VScode)

touch smile.py

14KB

smile.mp3

Аудио-файл, воспроизводимый через динамики

1MB

smile.png

image

Картинка, выводимая на экран

1. В smile.py скопируйте шаблон действия:

import os

class NEW_ACTION():    # Change it
    def __init__(self, srv_display_player=None, srv_set_neck=None, srv_set_ears=None,
                srv_play_sound=None, srv_mors_mode=None, srv_mors_action=None,
                srv_mors_cmd_vel=None, srv_mors_cmd_pos=None, srv_mors_ef_pos=None,
                srv_mors_joint_pos=None, srv_mors_joints_kp=None, srv_mors_joints_kd=None,
                srv_mors_stride_height=None, sound_direction=270):

        self.srv_display_player = srv_display_player
        self.srv_set_neck = srv_set_neck
        self.srv_set_ears = srv_set_ears
        self.srv_play_sound = srv_play_sound

        self.srv_mors_mode=srv_mors_mode
        self.srv_mors_action=srv_mors_action
        self.srv_mors_cmd_vel=srv_mors_cmd_vel
        self.srv_mors_cmd_pos=srv_mors_cmd_pos
        self.srv_mors_ef_pos=srv_mors_ef_pos
        self.srv_mors_joint_pos=srv_mors_joint_pos
        self.srv_mors_joints_kp=srv_mors_joints_kp
        self.srv_mors_joints_kd=srv_mors_joints_kd
        self.srv_mors_stride_height=srv_mors_stride_height
        self.sound_direction=sound_direction

        self._script_path = os.path.dirname(os.path.abspath(__file__))
        
    def start_action(self)->int:

        if self.srv_display_player!=None:
            path = "picture_filename"    # Change it
            self.srv_display_player(self._script_path+path)


        if self.srv_set_neck!=None:
            neck_angle_v = 15    # Change it
            neck_angle_h = 15    # Change it
            duration=1           # Change it
            self.srv_set_neck(neck_angle_v, neck_angle_h, duration)
        
        if self.srv_set_ears!=None:
            ear_angle_l = 10    # Change it
            ear_angle_r = 10    # Change it
            self.srv_set_ears(ear_angle_l, ear_angle_r)

        if self.srv_play_sound!=None:
            path = "audio_filename"    # Change it
            isBlocking = True         # Change it
            self.srv_play_sound(self._script_path+path, isBlocking)
        
        return 0

1. Опишем, что должно происходить по команде "Слушай, Морс! Улыбнись!": - на экран выводится изображение smile.png - включается воспроизведение аудио smile.mp3 - уши поворачиваются в разные стороны - за счет шейного сустава голова приподнимается и смотрит в сторону, откуда пришел звук

2. Описанные выше действия выполняет следующий код:

import os
import math
class SMILE():
    def __init__(self, srv_display_player=None, srv_set_neck=None, srv_set_ears=None,
                srv_play_sound=None, srv_mors_mode=None, srv_mors_action=None,
                srv_mors_cmd_vel=None, srv_mors_cmd_pos=None, srv_mors_ef_pos=None,
                srv_mors_joint_pos=None, srv_mors_joints_kp=None, srv_mors_joints_kd=None,
                srv_mors_stride_height=None, sound_direction=270):

        self.srv_display_player = srv_display_player
        self.srv_set_neck = srv_set_neck
        self.srv_set_ears = srv_set_ears
        self.srv_play_sound = srv_play_sound

        self.srv_mors_mode=srv_mors_mode
        self.srv_mors_action=srv_mors_action
        self.srv_mors_cmd_vel=srv_mors_cmd_vel
        self.srv_mors_cmd_pos=srv_mors_cmd_pos
        self.srv_mors_ef_pos=srv_mors_ef_pos
        self.srv_mors_joint_pos=srv_mors_joint_pos
        self.srv_mors_joints_kp=srv_mors_joints_kp
        self.srv_mors_joints_kd=srv_mors_joints_kd
        self.srv_mors_stride_height=srv_mors_stride_height
        self.sound_direction=sound_direction

        self._script_path = os.path.dirname(os.path.abspath(__file__))
        
    def start_action(self)->int:

        if self.srv_display_player!=None:
            path = "/smile.png" # Выводим изображение smile.png на экран
            self.srv_display_player(self._script_path+path)


        if self.srv_set_neck!=None:
            neck_angle_v = 15    # шейный сустав - чуть вверх
            neck_angle_h = 30

            # Нулевое положение входящего луча микрофонного модуля сдвинуто, ниже идет пересчет
            rad = math.radians(self.sound_direction)
            x = math.cos(rad)
            y = math.sin(rad)
            rotate = math.radians(90)
            x_ = x*math.cos(rotate)-y*math.sin(rotate)
            y_ = x*math.sin(rotate)+y*math.cos(rotate)
            neck_angle_h = -int(math.degrees(math.atan2(y_, x_)))

            neck_angle_h = min(max(neck_angle_h, -30), 30) # ограничиваем углы, чтобы не выйти за диапазон и не сломать механизм
    
            duration=0.5         # достичь заданного положения за 0.5 секунд
            self.srv_set_neck(neck_angle_v, neck_angle_h, duration)
        
        if self.srv_set_ears!=None:
            ear_angle_l = 90    # левое ухо до упора вперед
            ear_angle_r = -90   # правое ухо до упора назад
            self.srv_set_ears(ear_angle_l, ear_angle_r)

        if self.srv_play_sound!=None:
            path = "/smile.mp3" # включить воспроизведение аудио smile.mp3
            isBlocking = True  # воспроизведение аудио - блокирующее -> действие будет выполнять пока не закончится аудио
            self.srv_play_sound(self._script_path+path, isBlocking)
        
        return 0
 

---

2. Модификация пакета head_controller

Добавьте новое действие в головной модуль head_controller. Для этого перейдите в ~/robohead_ws/src/robohead/head_controller/scripts и отредактируйте файл main.py

В блок импорта добавьте строку

from actions.smile import smile

Здесь actioins.smile - локальный путь до папки с действием smile - название файла со скриптом (smile.py)

Добавьте в функцию def select_state(self, msg:Empty) блок кода:

            elif cmd=="улыбнись":
                importlib.reload(smile) # Перезагружает скрипт с действием
                self.action_smile = smile.SMILE(srv_display_player=self._service_display_player,
                                            srv_set_neck=self._service_set_neck,
                                            srv_set_ears=self._service_set_ears,
                                            srv_play_sound=self._service_play_sound,
                                            srv_mors_mode=self._srv_mors_mode,
                                            srv_mors_action=self._srv_mors_action,
                                            srv_mors_cmd_vel=self._srv_mors_cmd_vel,
                                            srv_mors_cmd_pos=self._srv_mors_cmd_pos,
                                            srv_mors_ef_pos=self._srv_mors_ef_pos,
                                            srv_mors_joint_pos=self._srv_mors_joint_pos,
                                            srv_mors_joints_kp=self._srv_mors_joints_kp,
                                            srv_mors_joints_kd=self._srv_mors_joints_kd,
                                            srv_mors_stride_height=self._srv_mors_stride_height,
                                            sound_direction=self.__sound_direction)
                resp = self.action_smile.start_action() # Запускает выполнение скрипта

---

И, наконец, перезагрузите сервис, чтобы применить внесенные изменения

sudo systemctl restart robohead.service

Иногда бывает удобно видеть вывод отладочной информации в терминале. Для этого head_controller со всеми необходимыми зависимостями можно запустить вручную:

# Остановить сервис
sudo systemctl stop robohead.service 

# Запустить head_controller с зависимостями
roslaunch head_controller head_controller.launch

Если вы хотите взаимодействовать с МОРСом, то изучите стандартные действия в пакете head_controller, сервисы пакета mors_driver и интеграцию с МОРСом.