Создадим новую немного модифицированную версию команды "Улыбнись" без использования robohead_controller
Опишем, что должно происходить при выполнении нашей программы:
- на экран выводится изображение smile.png
- включается воспроизведение аудио smile.mp3
- голова попеременно двигает ушами вперед-назад с частотой 2 Гц в течение 3 секунд
- за счет шейного сустава голова приподнимается и поворачивается в бок
Прямо в домашнем каталоге ~/ создадим папку scripts командой:
mkdir ~/scripts
cd ~/scripts
Добавим в нее медиа-файлы smile.png и smile.mp3 и создадим файл-скрипт main.py
touch main.py
Вставьте в файл 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 и запустить зависимости.
