Justin Newberry
1 year ago
committed by
GitHub
parent
f32e3ae799
commit
abe39e5076
18 changed files with 186 additions and 298 deletions
Binary file not shown.
@ -0,0 +1,160 @@ |
||||
import math |
||||
import time |
||||
import numpy as np |
||||
import os |
||||
import wave |
||||
|
||||
from typing import Dict, Optional, Tuple |
||||
|
||||
from cereal import car, messaging |
||||
from openpilot.common.basedir import BASEDIR |
||||
from openpilot.common.realtime import Ratekeeper |
||||
from openpilot.system.hardware import PC |
||||
from openpilot.system.swaglog import cloudlog |
||||
|
||||
SAMPLE_RATE = 48000 |
||||
MAX_VOLUME = 1.0 |
||||
MIN_VOLUME = 0.1 |
||||
CONTROLS_TIMEOUT = 5 # 5 seconds |
||||
|
||||
AMBIENT_DB = 30 # DB where MIN_VOLUME is applied |
||||
DB_SCALE = 30 # AMBIENT_DB + DB_SCALE is where MAX_VOLUME is applied |
||||
|
||||
AudibleAlert = car.CarControl.HUDControl.AudibleAlert |
||||
|
||||
|
||||
sound_list: Dict[int, Tuple[str, Optional[int], float]] = { |
||||
# AudibleAlert, file name, play count (none for infinite) |
||||
AudibleAlert.engage: ("engage.wav", 1, MAX_VOLUME), |
||||
AudibleAlert.disengage: ("disengage.wav", 1, MAX_VOLUME), |
||||
AudibleAlert.refuse: ("refuse.wav", 1, MAX_VOLUME), |
||||
|
||||
AudibleAlert.prompt: ("prompt.wav", 1, MAX_VOLUME), |
||||
AudibleAlert.promptRepeat: ("prompt.wav", None, MAX_VOLUME), |
||||
AudibleAlert.promptDistracted: ("prompt_distracted.wav", None, MAX_VOLUME), |
||||
|
||||
AudibleAlert.warningSoft: ("warning_soft.wav", None, MAX_VOLUME), |
||||
AudibleAlert.warningImmediate: ("warning_immediate.wav", None, MAX_VOLUME), |
||||
} |
||||
|
||||
def check_controls_timeout_alert(sm): |
||||
controls_missing = time.monotonic() - sm.rcv_time['controlsState'] |
||||
|
||||
if controls_missing > CONTROLS_TIMEOUT: |
||||
if sm['controlsState'].enabled and (controls_missing - CONTROLS_TIMEOUT) < 10: |
||||
return True |
||||
|
||||
return False |
||||
|
||||
|
||||
class Soundd: |
||||
def __init__(self): |
||||
self.load_sounds() |
||||
|
||||
self.current_alert = AudibleAlert.none |
||||
self.current_volume = MIN_VOLUME |
||||
self.current_sound_frame = 0 |
||||
|
||||
self.controls_timeout_alert = False |
||||
|
||||
if not PC: |
||||
os.system("pactl set-sink-volume @DEFAULT_SINK@ 0.9") # set to max volume and control volume within soundd |
||||
|
||||
def load_sounds(self): |
||||
self.loaded_sounds: Dict[int, np.ndarray] = {} |
||||
|
||||
# Load all sounds |
||||
for sound in sound_list: |
||||
filename, play_count, volume = sound_list[sound] |
||||
|
||||
wavefile = wave.open(BASEDIR + "/selfdrive/assets/sounds/" + filename, 'r') |
||||
|
||||
assert wavefile.getnchannels() == 1 |
||||
assert wavefile.getsampwidth() == 2 |
||||
assert wavefile.getframerate() == SAMPLE_RATE |
||||
|
||||
length = wavefile.getnframes() |
||||
self.loaded_sounds[sound] = np.frombuffer(wavefile.readframes(length), dtype=np.int16).astype(np.float32) / (2**16/2) |
||||
|
||||
def get_sound_data(self, frames): # get "frames" worth of data from the current alert sound, looping when required |
||||
|
||||
ret = np.zeros(frames, dtype=np.float32) |
||||
|
||||
if self.current_alert != AudibleAlert.none: |
||||
num_loops = sound_list[self.current_alert][1] |
||||
sound_data = self.loaded_sounds[self.current_alert] |
||||
written_frames = 0 |
||||
|
||||
current_sound_frame = self.current_sound_frame % len(sound_data) |
||||
loops = self.current_sound_frame // len(sound_data) |
||||
|
||||
while written_frames < frames and (num_loops is None or loops < num_loops): |
||||
available_frames = sound_data.shape[0] - current_sound_frame |
||||
frames_to_write = min(available_frames, frames - written_frames) |
||||
ret[written_frames:written_frames+frames_to_write] = sound_data[current_sound_frame:current_sound_frame+frames_to_write] |
||||
written_frames += frames_to_write |
||||
self.current_sound_frame += frames_to_write |
||||
|
||||
return ret * self.current_volume |
||||
|
||||
def callback(self, data_out: np.ndarray, frames: int, time, status) -> None: |
||||
if status: |
||||
cloudlog.warning(f"soundd stream over/underflow: {status}") |
||||
data_out[:frames, 0] = self.get_sound_data(frames) |
||||
|
||||
def update_alert(self, new_alert): |
||||
current_alert_played_once = self.current_alert == AudibleAlert.none or self.current_sound_frame > len(self.loaded_sounds[self.current_alert]) |
||||
if self.current_alert != new_alert and (new_alert != AudibleAlert.none or current_alert_played_once): |
||||
self.current_alert = new_alert |
||||
self.current_sound_frame = 0 |
||||
|
||||
def get_audible_alert(self, sm): |
||||
if sm.updated['controlsState']: |
||||
new_alert = sm['controlsState'].alertSound.raw |
||||
self.update_alert(new_alert) |
||||
elif check_controls_timeout_alert(sm): |
||||
self.update_alert(AudibleAlert.warningImmediate) |
||||
self.controls_timeout_alert = True |
||||
elif self.controls_timeout_alert: |
||||
self.update_alert(AudibleAlert.none) |
||||
self.controls_timeout_alert = False |
||||
|
||||
def calculate_volume(self, weighted_db): |
||||
volume = ((weighted_db - AMBIENT_DB) / DB_SCALE) * (MAX_VOLUME - MIN_VOLUME) + MIN_VOLUME |
||||
return math.pow(10, (np.clip(volume, MIN_VOLUME, MAX_VOLUME) - 1)) |
||||
|
||||
def soundd_thread(self): |
||||
# sounddevice must be imported after forking processes |
||||
import sounddevice as sd |
||||
|
||||
rk = Ratekeeper(20) |
||||
|
||||
sm = messaging.SubMaster(['controlsState', 'microphone']) |
||||
|
||||
if PC: |
||||
device = None |
||||
else: |
||||
device = "pulse" # "sdm845-tavil-snd-card: - (hw:0,0)" |
||||
|
||||
with sd.OutputStream(device=device, channels=1, samplerate=SAMPLE_RATE, callback=self.callback) as stream: |
||||
cloudlog.info(f"soundd stream started: {stream.samplerate=} {stream.channels=} {stream.dtype=} {stream.device=}") |
||||
while True: |
||||
sm.update(0) |
||||
|
||||
if sm.updated['microphone']: |
||||
self.current_volume = self.calculate_volume(sm["microphone"].filteredSoundPressureWeightedDb) |
||||
|
||||
self.get_audible_alert(sm) |
||||
|
||||
rk.keep_time() |
||||
|
||||
assert stream.active |
||||
|
||||
|
||||
def main(): |
||||
s = Soundd() |
||||
s.soundd_thread() |
||||
|
||||
|
||||
if __name__ == "__main__": |
||||
main() |
||||
@ -1 +0,0 @@ |
||||
_soundd |
||||
@ -1,18 +0,0 @@ |
||||
#include <sys/resource.h> |
||||
|
||||
#include <QApplication> |
||||
|
||||
#include "selfdrive/ui/qt/util.h" |
||||
#include "selfdrive/ui/soundd/sound.h" |
||||
|
||||
int main(int argc, char **argv) { |
||||
qInstallMessageHandler(swagLogMessageHandler); |
||||
setpriority(PRIO_PROCESS, 0, -20); |
||||
|
||||
QApplication a(argc, argv); |
||||
std::signal(SIGINT, sigTermHandler); |
||||
std::signal(SIGTERM, sigTermHandler); |
||||
|
||||
Sound sound; |
||||
return a.exec(); |
||||
} |
||||
@ -1,67 +0,0 @@ |
||||
#include "selfdrive/ui/soundd/sound.h" |
||||
|
||||
#include <cmath> |
||||
|
||||
#include <QAudio> |
||||
#include <QAudioDeviceInfo> |
||||
#include <QDebug> |
||||
|
||||
#include "cereal/messaging/messaging.h" |
||||
#include "common/util.h" |
||||
|
||||
// TODO: detect when we can't play sounds
|
||||
// TODO: detect when we can't display the UI
|
||||
|
||||
Sound::Sound(QObject *parent) : sm({"controlsState", "microphone"}) { |
||||
qInfo() << "default audio device: " << QAudioDeviceInfo::defaultOutputDevice().deviceName(); |
||||
|
||||
for (auto &[alert, fn, loops, volume] : sound_list) { |
||||
QSoundEffect *s = new QSoundEffect(this); |
||||
QObject::connect(s, &QSoundEffect::statusChanged, [=]() { |
||||
assert(s->status() != QSoundEffect::Error); |
||||
}); |
||||
s->setSource(QUrl::fromLocalFile("../../assets/sounds/" + fn)); |
||||
s->setVolume(volume); |
||||
sounds[alert] = {s, loops}; |
||||
} |
||||
|
||||
QTimer *timer = new QTimer(this); |
||||
QObject::connect(timer, &QTimer::timeout, this, &Sound::update); |
||||
timer->start(1000 / UI_FREQ); |
||||
} |
||||
|
||||
void Sound::update() { |
||||
sm.update(0); |
||||
|
||||
// scale volume using ambient noise level
|
||||
if (sm.updated("microphone")) { |
||||
float volume = util::map_val(sm["microphone"].getMicrophone().getFilteredSoundPressureWeightedDb(), 30.f, 60.f, 0.f, 1.f); |
||||
volume = QAudio::convertVolume(volume, QAudio::LogarithmicVolumeScale, QAudio::LinearVolumeScale); |
||||
// set volume on changes
|
||||
if (std::exchange(current_volume, std::nearbyint(volume * 10)) != current_volume) { |
||||
Hardware::set_volume(volume); |
||||
} |
||||
} |
||||
|
||||
setAlert(Alert::get(sm, 0)); |
||||
} |
||||
|
||||
void Sound::setAlert(const Alert &alert) { |
||||
if (!current_alert.equal(alert)) { |
||||
current_alert = alert; |
||||
// stop sounds
|
||||
for (auto &[s, loops] : sounds) { |
||||
// Only stop repeating sounds
|
||||
if (s->loopsRemaining() > 1 || s->loopsRemaining() == QSoundEffect::Infinite) { |
||||
s->stop(); |
||||
} |
||||
} |
||||
|
||||
// play sound
|
||||
if (alert.sound != AudibleAlert::NONE) { |
||||
auto &[s, loops] = sounds[alert.sound]; |
||||
s->setLoopCount(loops); |
||||
s->play(); |
||||
} |
||||
} |
||||
} |
||||
@ -1,41 +0,0 @@ |
||||
#pragma once |
||||
|
||||
#include <tuple> |
||||
|
||||
#include <QMap> |
||||
#include <QSoundEffect> |
||||
#include <QString> |
||||
|
||||
#include "system/hardware/hw.h" |
||||
#include "selfdrive/ui/ui.h" |
||||
|
||||
|
||||
const float MAX_VOLUME = 1.0; |
||||
|
||||
const std::tuple<AudibleAlert, QString, int, float> sound_list[] = { |
||||
// AudibleAlert, file name, loop count
|
||||
{AudibleAlert::ENGAGE, "engage.wav", 0, MAX_VOLUME}, |
||||
{AudibleAlert::DISENGAGE, "disengage.wav", 0, MAX_VOLUME}, |
||||
{AudibleAlert::REFUSE, "refuse.wav", 0, MAX_VOLUME}, |
||||
|
||||
{AudibleAlert::PROMPT, "prompt.wav", 0, MAX_VOLUME}, |
||||
{AudibleAlert::PROMPT_REPEAT, "prompt.wav", QSoundEffect::Infinite, MAX_VOLUME}, |
||||
{AudibleAlert::PROMPT_DISTRACTED, "prompt_distracted.wav", QSoundEffect::Infinite, MAX_VOLUME}, |
||||
|
||||
{AudibleAlert::WARNING_SOFT, "warning_soft.wav", QSoundEffect::Infinite, MAX_VOLUME}, |
||||
{AudibleAlert::WARNING_IMMEDIATE, "warning_immediate.wav", QSoundEffect::Infinite, MAX_VOLUME}, |
||||
}; |
||||
|
||||
class Sound : public QObject { |
||||
public: |
||||
explicit Sound(QObject *parent = 0); |
||||
|
||||
protected: |
||||
void update(); |
||||
void setAlert(const Alert &alert); |
||||
|
||||
SubMaster sm; |
||||
Alert current_alert = {}; |
||||
QMap<AudibleAlert, QPair<QSoundEffect *, int>> sounds; |
||||
int current_volume = -1; |
||||
}; |
||||
@ -1,4 +0,0 @@ |
||||
#!/bin/sh |
||||
cd "$(dirname "$0")" |
||||
export QT_QPA_PLATFORM="offscreen" |
||||
exec ./_soundd |
||||
@ -1,75 +0,0 @@ |
||||
#include <QEventLoop> |
||||
#include <QMap> |
||||
#include <QThread> |
||||
|
||||
#include "catch2/catch.hpp" |
||||
#include "selfdrive/ui/soundd/sound.h" |
||||
|
||||
class TestSound : public Sound { |
||||
public: |
||||
TestSound() : Sound() { |
||||
for (auto i = sounds.constBegin(); i != sounds.constEnd(); ++i) { |
||||
sound_stats[i.key()] = {0, 0}; |
||||
QObject::connect(i.value().first, &QSoundEffect::playingChanged, [=, s = i.value().first, a = i.key()]() { |
||||
if (s->isPlaying()) { |
||||
sound_stats[a].first++; |
||||
} else { |
||||
sound_stats[a].second++; |
||||
} |
||||
}); |
||||
} |
||||
} |
||||
|
||||
QMap<AudibleAlert, std::pair<int, int>> sound_stats; |
||||
}; |
||||
|
||||
void controls_thread(int loop_cnt) { |
||||
PubMaster pm({"controlsState", "deviceState"}); |
||||
MessageBuilder deviceStateMsg; |
||||
auto deviceState = deviceStateMsg.initEvent().initDeviceState(); |
||||
deviceState.setStarted(true); |
||||
|
||||
const int DT_CTRL = 10; // ms
|
||||
for (int i = 0; i < loop_cnt; ++i) { |
||||
for (auto &[alert, fn, loops, volume] : sound_list) { |
||||
printf("testing %s\n", qPrintable(fn)); |
||||
for (int j = 0; j < 1000 / DT_CTRL; ++j) { |
||||
MessageBuilder msg; |
||||
auto cs = msg.initEvent().initControlsState(); |
||||
cs.setAlertSound(alert); |
||||
cs.setAlertType(fn.toStdString()); |
||||
pm.send("controlsState", msg); |
||||
pm.send("deviceState", deviceStateMsg); |
||||
QThread::msleep(DT_CTRL); |
||||
} |
||||
} |
||||
} |
||||
|
||||
// send no alert sound
|
||||
for (int j = 0; j < 1000 / DT_CTRL; ++j) { |
||||
MessageBuilder msg; |
||||
msg.initEvent().initControlsState(); |
||||
pm.send("controlsState", msg); |
||||
QThread::msleep(DT_CTRL); |
||||
} |
||||
|
||||
QThread::currentThread()->quit(); |
||||
} |
||||
|
||||
TEST_CASE("test soundd") { |
||||
QEventLoop loop; |
||||
TestSound test_sound; |
||||
const int test_loop_cnt = 2; |
||||
|
||||
QThread t; |
||||
QObject::connect(&t, &QThread::started, [=]() { controls_thread(test_loop_cnt); }); |
||||
QObject::connect(&t, &QThread::finished, [&]() { loop.quit(); }); |
||||
t.start(); |
||||
loop.exec(); |
||||
|
||||
for (const AudibleAlert alert : test_sound.sound_stats.keys()) { |
||||
auto [play, stop] = test_sound.sound_stats[alert]; |
||||
REQUIRE(play == test_loop_cnt); |
||||
REQUIRE(stop == test_loop_cnt); |
||||
} |
||||
} |
||||
@ -1,75 +1,41 @@ |
||||
#!/usr/bin/env python3 |
||||
import subprocess |
||||
import time |
||||
import unittest |
||||
|
||||
from cereal import log, car |
||||
import cereal.messaging as messaging |
||||
from openpilot.selfdrive.test.helpers import phone_only, with_processes |
||||
# TODO: rewrite for unittest |
||||
from openpilot.common.realtime import DT_CTRL |
||||
from openpilot.system.hardware import HARDWARE |
||||
from cereal import car |
||||
from cereal import messaging |
||||
from cereal.messaging import SubMaster, PubMaster |
||||
from openpilot.selfdrive.ui.soundd import CONTROLS_TIMEOUT, check_controls_timeout_alert |
||||
|
||||
AudibleAlert = car.CarControl.HUDControl.AudibleAlert |
||||
import time |
||||
|
||||
SOUNDS = { |
||||
# sound: total writes |
||||
AudibleAlert.none: 0, |
||||
AudibleAlert.engage: 184, |
||||
AudibleAlert.disengage: 186, |
||||
AudibleAlert.refuse: 194, |
||||
AudibleAlert.prompt: 184, |
||||
AudibleAlert.promptRepeat: 487, |
||||
AudibleAlert.promptDistracted: 508, |
||||
AudibleAlert.warningSoft: 471, |
||||
AudibleAlert.warningImmediate: 470, |
||||
} |
||||
AudibleAlert = car.CarControl.HUDControl.AudibleAlert |
||||
|
||||
def get_total_writes(): |
||||
audio_flinger = subprocess.check_output('dumpsys media.audio_flinger', shell=True, encoding='utf-8').strip() |
||||
write_lines = [l for l in audio_flinger.split('\n') if l.strip().startswith('Total writes')] |
||||
return sum(int(l.split(':')[1]) for l in write_lines) |
||||
|
||||
class TestSoundd(unittest.TestCase): |
||||
def test_sound_card_init(self): |
||||
assert HARDWARE.get_sound_card_online() |
||||
def test_check_controls_timeout_alert(self): |
||||
sm = SubMaster(['controlsState']) |
||||
pm = PubMaster(['controlsState']) |
||||
|
||||
for _ in range(100): |
||||
cs = messaging.new_message('controlsState') |
||||
cs.controlsState.enabled = True |
||||
|
||||
pm.send("controlsState", cs) |
||||
|
||||
@phone_only |
||||
@with_processes(['soundd']) |
||||
def test_alert_sounds(self): |
||||
pm = messaging.PubMaster(['deviceState', 'controlsState']) |
||||
time.sleep(0.01) |
||||
|
||||
# make sure they're all defined |
||||
alert_sounds = {v: k for k, v in car.CarControl.HUDControl.AudibleAlert.schema.enumerants.items()} |
||||
diff = set(SOUNDS.keys()).symmetric_difference(alert_sounds.keys()) |
||||
assert len(diff) == 0, f"not all sounds defined in test: {diff}" |
||||
sm.update(0) |
||||
|
||||
# wait for procs to init |
||||
time.sleep(1) |
||||
self.assertFalse(check_controls_timeout_alert(sm)) |
||||
|
||||
for sound, expected_writes in SOUNDS.items(): |
||||
print(f"testing {alert_sounds[sound]}") |
||||
start_writes = get_total_writes() |
||||
for _ in range(CONTROLS_TIMEOUT * 110): |
||||
sm.update(0) |
||||
time.sleep(0.01) |
||||
|
||||
for i in range(int(10 / DT_CTRL)): |
||||
msg = messaging.new_message('deviceState') |
||||
msg.deviceState.started = True |
||||
pm.send('deviceState', msg) |
||||
self.assertTrue(check_controls_timeout_alert(sm)) |
||||
|
||||
msg = messaging.new_message('controlsState') |
||||
if i < int(6 / DT_CTRL): |
||||
msg.controlsState.alertSound = sound |
||||
msg.controlsState.alertType = str(sound) |
||||
msg.controlsState.alertText1 = "Testing Sounds" |
||||
msg.controlsState.alertText2 = f"playing {alert_sounds[sound]}" |
||||
msg.controlsState.alertSize = log.ControlsState.AlertSize.mid |
||||
pm.send('controlsState', msg) |
||||
time.sleep(DT_CTRL) |
||||
# TODO: add test with micd for checking that soundd actually outputs sounds |
||||
|
||||
tolerance = expected_writes / 8 |
||||
actual_writes = get_total_writes() - start_writes |
||||
print(f" expected {expected_writes} writes, got {actual_writes}") |
||||
assert abs(expected_writes - actual_writes) <= tolerance, f"{alert_sounds[sound]}: expected {expected_writes} writes, got {actual_writes}" |
||||
|
||||
if __name__ == "__main__": |
||||
unittest.main() |
||||
|
||||
Loading…
Reference in new issue