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 |
#!/usr/bin/env python3 |
||||||
import subprocess |
|
||||||
import time |
|
||||||
import unittest |
import unittest |
||||||
|
|
||||||
from cereal import log, car |
from cereal import car |
||||||
import cereal.messaging as messaging |
from cereal import messaging |
||||||
from openpilot.selfdrive.test.helpers import phone_only, with_processes |
from cereal.messaging import SubMaster, PubMaster |
||||||
# TODO: rewrite for unittest |
from openpilot.selfdrive.ui.soundd import CONTROLS_TIMEOUT, check_controls_timeout_alert |
||||||
from openpilot.common.realtime import DT_CTRL |
|
||||||
from openpilot.system.hardware import HARDWARE |
|
||||||
|
|
||||||
AudibleAlert = car.CarControl.HUDControl.AudibleAlert |
import time |
||||||
|
|
||||||
SOUNDS = { |
AudibleAlert = car.CarControl.HUDControl.AudibleAlert |
||||||
# 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, |
|
||||||
} |
|
||||||
|
|
||||||
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): |
class TestSoundd(unittest.TestCase): |
||||||
def test_sound_card_init(self): |
def test_check_controls_timeout_alert(self): |
||||||
assert HARDWARE.get_sound_card_online() |
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 |
time.sleep(0.01) |
||||||
@with_processes(['soundd']) |
|
||||||
def test_alert_sounds(self): |
|
||||||
pm = messaging.PubMaster(['deviceState', 'controlsState']) |
|
||||||
|
|
||||||
# make sure they're all defined |
sm.update(0) |
||||||
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}" |
|
||||||
|
|
||||||
# wait for procs to init |
self.assertFalse(check_controls_timeout_alert(sm)) |
||||||
time.sleep(1) |
|
||||||
|
|
||||||
for sound, expected_writes in SOUNDS.items(): |
for _ in range(CONTROLS_TIMEOUT * 110): |
||||||
print(f"testing {alert_sounds[sound]}") |
sm.update(0) |
||||||
start_writes = get_total_writes() |
time.sleep(0.01) |
||||||
|
|
||||||
for i in range(int(10 / DT_CTRL)): |
self.assertTrue(check_controls_timeout_alert(sm)) |
||||||
msg = messaging.new_message('deviceState') |
|
||||||
msg.deviceState.started = True |
|
||||||
pm.send('deviceState', msg) |
|
||||||
|
|
||||||
msg = messaging.new_message('controlsState') |
# TODO: add test with micd for checking that soundd actually outputs sounds |
||||||
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) |
|
||||||
|
|
||||||
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__": |
if __name__ == "__main__": |
||||||
unittest.main() |
unittest.main() |
||||||
|
|||||||
Loading…
Reference in new issue