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

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

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

---

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

mkdir ~/scripts

cd ~/scripts

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

touch main.py

1MB

smile.png

image

14KB

smile.mp3

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

import os
import rospy

# imports for display_driver
from display_driver.srv import PlayMedia, PlayMediaRequest, PlayMediaResponse

# imports for ears_driver
from ears_driver.srv import EarsSetAngle, EarsSetAngleRequest, EarsSetAngleResponse

# imports for neck_driver
from neck_driver.srv import NeckSetAngle, NeckSetAngleRequest, NeckSetAngleResponse

# imports for speakers_driver
from speakers_driver.srv import PlayAudio, PlayAudioRequest, PlayAudioResponse
from speakers_driver.srv import SetVolume, SetVolumeRequest, SetVolumeResponse

rospy.init_node("smile_node")

script_path = os.path.dirname(os.path.abspath(__file__)) + '/'
print('Start Init')
# Дожидаемся запуска всех сервисов
rospy.wait_for_service('/robohead_controller/display_driver/PlayMedia')
rospy.wait_for_service('/robohead_controller/neck_driver/NeckSetAngle')
rospy.wait_for_service('/robohead_controller/ears_driver/EarsSetAngle')
rospy.wait_for_service('/robohead_controller/speakers_driver/PlayAudio')
rospy.wait_for_service('/robohead_controller/speakers_driver/SetVolume')
print('Services&topics exists')

# Инициализация сервисов и подписчика
service_display_driver_PlayMedia = rospy.ServiceProxy('/robohead_controller/display_driver/PlayMedia', PlayMedia) # Сервис воспроизведения медиа на экране
service_neck_driver_NeckSetAngle = rospy.ServiceProxy('/robohead_controller/neck_driver/NeckSetAngle', NeckSetAngle) # Сервис задания положения шейного сустава
service_ears_driver_EarsSetAngle = rospy.ServiceProxy('/robohead_controller/ears_driver/EarsSetAngle', EarsSetAngle) # Сервис задания положения ушей
service_speakers_driver_PlayAudio = rospy.ServiceProxy('/robohead_controller/speakers_driver/PlayAudio', PlayAudio) # Сервис проигрывания аудио
service_speakers_driver_SetVolume = rospy.ServiceProxy('/robohead_controller/speakers_driver/SetVolume', SetVolume) # Сервис установки громкости проигрывания аудио
print('Services&topcs inited')

print('run')
# Выводим картинку на экран
msg = PlayMediaRequest()
msg.path_to_file = script_path + 'smile.png'
msg.is_blocking = 0
msg.is_cycled = 0
service_display_driver_PlayMedia(msg)

# Устанавливаем положение шеи
msg = NeckSetAngleRequest()
msg.horizontal_angle = 30
msg.vertical_angle = 15
msg.is_blocking = 0
msg.duration = 1
service_neck_driver_NeckSetAngle(msg)

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

# Запускаем проигрывание звука
msg = PlayAudioRequest()
msg.path_to_file = script_path + 'smile.mp3'
msg.is_cycled = 0
msg.is_blocking = 0
service_speakers_driver_PlayAudio(msg)

# Дергаем ушами 5 секунд
start_time = rospy.get_time()
direction = 1
msg = EarsSetAngleRequest()
while (rospy.get_time()-start_time) < 3: # Пока не прошло 3 секунд
    msg.left_ear_angle = direction*45
    msg.right_ear_angle = -direction*45
    service_ears_driver_EarsSetAngle(msg)
    direction *= -1
    rospy.sleep(0.5)

service_display_driver_PlayMedia() # Убираем картинку с дисплея
service_ears_driver_EarsSetAngle(0,0) # Возвращаем уши в начальное положение
service_neck_driver_NeckSetAngle(0,0,1,1) # Возвращаем шею в начальное положение

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

sudo systemctl stop robohead.service

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

roslaunch robohead_controller dependencies.launch

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

python3 ~/scripts/main.py

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

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