4. Создание своих сценариев взаимодействия без использования head_controller

Создадим новую немного модифицированную версию команды "Улыбнись" без использования head_controller

Опишем, что должно происходить при выполнении нашей программы: - на экран выводится изображение smile.png - включается воспроизведение аудио smile.mp3 - до завершения аудио голова попеременно двигает ушами вперед-назад с частотой 2 Гц - за счет шейного сустава голова приподнимается и 5 секунд мотается в стороны

---

Прямо в домашнем каталоге ~/ создадим папку scripts командой:

mkdir ~/scripts

cd ~/scripts

Добавим в нее медиа-файлы smile.png и smile.mp3 и создадим файл-скрипт main.py

touch main.py

1MB

smile.png

image

14KB

smile.mp3

Вставьте в файл main.py следующий код

#!/usr/bin/env python3

# -*- coding: utf-8 -*-

import os
import sys

sys.path.append(os.path.dirname(os.path.abspath(__file__)))

from mors_driver.srv import TwistDuration, PoseArrayDuration, JointTrajectoryPointDuration

import rospy

import time

from std_msgs.msg import String, Empty, Int32
from sensor_msgs.msg import BatteryState
from display_controller.srv import displayControllerPlay
from neck_controller.srv import NeckSetAngle
from ears_controller.srv import EarsSetAngle
from speakers_controller.srv import playSound
from speakers_controller.srv import setVolume
from speakers_controller.srv import isPlaying


script_path = os.path.dirname(os.path.abspath(__file__))
print("Script path: " + script_path)

rospy.init_node("smile_node")

# Дожидаемся запуска всех сервисов
rospy.wait_for_service('displayControllerPlay')
rospy.wait_for_service('NeckSetAngle')
rospy.wait_for_service('EarsSetAngle')
rospy.wait_for_service('playSound')
rospy.wait_for_service('isPlaying')
rospy.wait_for_service('setVolume')

# Инициализация сервисом
service_display_player = rospy.ServiceProxy('displayControllerPlay', displayControllerPlay) # Сервис воспроизведения медиа на экране
service_set_neck = rospy.ServiceProxy('NeckSetAngle', NeckSetAngle) # Сервис задания положения шейного сустава
service_set_ears = rospy.ServiceProxy('EarsSetAngle', EarsSetAngle) # Сервис задания положения ушей
service_play_sound = rospy.ServiceProxy('playSound', playSound) # Сервис проигрывания аудио
service_is_playing = rospy.ServiceProxy('isPlaying', isPlaying) # Сервис проверки проигрывается ли сейчас аудио
service_set_volume = rospy.ServiceProxy('setVolume', setVolume) # Сервис установки громкости проигрывания аудио

service_display_player(f"{script_path}/smile.png") # Выводим картинку на экран

service_set_volume(60) # Устанавливаем громкость воспроизведения аудио, диапазон 0..100

service_play_sound(f"{script_path}/smile.mp3", 0)

start = time.time()

ear_k = 1
service_set_ears(-90*ear_k, 90*ear_k) # Установка первоначального положения
ears_timer = time.time()

neck_k = 1
service_set_neck(15, 20 * neck_k, 1)
neck_timer = time.time()
while ((time.time()-start)<=5):

    if service_is_playing().Data and ((time.time()-ears_timer)>0.5): # Пока проигрывается аудио, каждые 0.5 секунды двигаются уши
        ear_k = ear_k*(-1) # Меняем направление

        service_set_ears(-90*ear_k, 90*ear_k) # Устанавливаем положение
        ears_timer = time.time() # Обновляем таймер
    
    if (time.time()-neck_timer)>1: # Каждую секунду голова движется в новую сторону
        neck_k = neck_k*(-1) # Меняем направление
        service_set_neck(15, 20 * neck_k, 1) # Задать положение
        neck_timer = time.time() # Обновляем таймер

service_set_neck(0,0,1) # Возвращаем шейный сустав в начальное положение
service_set_ears(0,0) # Возвращаем уши в начальное положение
service_display_player(f"__BLANK__") # Убираем изображение с экрана

Остановите сервис robohead

sudo systemctl stop robohead.service

В отдельном терминале запустите все необходимые пакеты без head_controller

roslaunch head_controller dependencies.launch

И запустите созданный скрипт main.py

python3 ~/scripts/main.py

Псле запуска скрипта произойдет выполнение действий команды "Улыбнись".

Дальше можно перезапускать только скрипт main.py После перезагрузки нужно будет вновь остановить сервис robohead и запустить зависимости