void
VersionSlider::onVersionSliderChange(int newValue)
{
int version, i;
double wantedTimestamp = (double)newValue / 100.0;
if(settingCurrentVersion)
return;
//i < maximum(): if the timestamp is greater than any version before the last (maximum()) then we don't check for the last and will take that one.
for(i = 1; i <= maximum(); i++) {
if(rpldata_wsn_version_get_timestamp(i) > wantedTimestamp) {
if(i > 1 && (wantedTimestamp - rpldata_wsn_version_get_timestamp(i - 1)) < (rpldata_wsn_version_get_timestamp(i) - wantedTimestamp))
i--;
break;
}
}
if(i > maximum()) {
setRealtime();
} else {
version = i;
setValue(version);
}
emit changeWsnVersion(value());
}
void audioThreadProc(PyMutex& threadLock, bool& stopSignal) {
while(true) {
{
PyScopedLock l(threadLock);
if(stopSignal) return;
}
if(needRealtimeReset) {
needRealtimeReset = false;
setRealtime();
}
OUTSAMPLE_t buffer[48 * 2 * 10]; // 10ms stereo in 48khz
size_t frameCount = 0;
{
PyScopedLock lock(player->lock);
if(stopSignal) return;
player->readOutStream(buffer, sizeof(buffer)/sizeof(OUTSAMPLE_t), NULL);
frameCount = sizeof(buffer)/sizeof(OUTSAMPLE_t) / player->outNumChannels;
}
PaError ret = Pa_WriteStream(stream, buffer, frameCount);
if(ret == paOutputUnderflowed)
printf("warning: paOutputUnderflowed\n");
else if(ret != paNoError) {
printf("Pa_WriteStream error %i (%s)\n", ret, Pa_GetErrorText(ret));
// sleep half a second to avoid spamming
for(int i = 0; i < 50; ++i) {
usleep(10 * 1000);
PyScopedLock l(threadLock);
if(stopSignal) return;
}
}
}
}
void
VersionSlider::onChangeCurrentVersion(int newVersion)
{
if(newVersion == 0) {
setRealtime();
} else {
setValue(newVersion);
}
emit changeWsnVersion(value());
}
VersionSlider::VersionSlider(QWidget * parent):
QWidget(parent), ui(new Ui::VersionSlider), showUpdate(false), incrTime(false), currentTimestamp(0), currentMaxTimestamp(0), maxTimestamp(0)
{
ui->setupUi(this);
settingCurrentVersion = false;
connect(ui->versionSlider, SIGNAL(valueChanged(int)), this, SLOT(onVersionSliderChange(int)));
connect(ui->versionSpin, SIGNAL(valueChanged(int)), this, SLOT(onVersionSpinChange(int)));
setRealtime();
}
void
VersionSlider::onVersionSpinChange(int newValue)
{
int version = qMax(qMin(newValue, maximum() + 1), 1);
if(settingCurrentVersion)
return;
if(version > maximum()) {
setRealtime();
} else {
setValue(version);
}
emit changeWsnVersion(value());
}
void
VersionSlider::onUpdateVersionCount(int versionCount)
{
bool versionChange = false;
bool inRealtime = realtime();
if(value() > versionCount || inRealtime)
versionChange = true;
setMaximum(versionCount);
if(inRealtime) {
setRealtime();
}
if(versionChange)
emit changeWsnVersion(value());
}
/**
* The main method that starts MAESTOR! This is where the ros node is made and all of the
* services are advertised. There are also wrappers in here for all of the service methods that
* link up to the ones that are in RobotControl.
* @param argc Number of arguments
* @param argv Argument array
* @return 0 when it shuts down
*/
int main(int argc, char **argv) {
ros::init(argc, argv, "Maestor");
setRealtime();
//Check for the run type
if(argc == 2)
{
if(strcmp(argv[1], "sim") == 0)
{
//If simulation set the runtime to sim
robot.setSimType();
}
}
//Init the node
NodeHandle n; //Fully initializes the node
Scheduler timer(FREQ_200HZ);
robot.setPeriod(1.0/timer.getFrequency());
ServiceServer srv = n.advertiseService("fib", &fib);
ServiceServer Initsrv = n.advertiseService("initRobot", &initRobot);
ServiceServer SPsrv = n.advertiseService("setProperties", &setProperties);
ServiceServer Comsrv = n.advertiseService("command", &command);
ServiceServer RMsrv = n.advertiseService("requiresMotion", &requiresMotion);
ServiceServer GPsrv = n.advertiseService("getProperties", &getProperties);
ServiceServer LTsrv = n.advertiseService("loadTrajectory", &loadTrajectory);
ServiceServer IFsrv = n.advertiseService("ignoreFrom", &ignoreFrom);
ServiceServer IAFsrv = n.advertiseService("ignoreAllFrom", &ignoreAllFrom);
ServiceServer UFsrv = n.advertiseService("unignoreFrom", &unignoreFrom);
ServiceServer UAFsrv = n.advertiseService("unignoreAllFrom", &unignoreAllFrom);
ServiceServer STsrv = n.advertiseService("setTrigger", &setTrigger);
ServiceServer ETsrv = n.advertiseService("extendTrajectory", &extendTrajectory);
ServiceServer StTsrv = n.advertiseService("startTrajectory", &startTrajectory);
ServiceServer SpTsrv = n.advertiseService("stopTrajectory", &stopTrajectory);
ServiceServer SetPropsrv = n.advertiseService("setProperty", &setProperty);
while (ros::ok()) {
ros::spinOnce();
robot.updateHook();
timer.sleep();
timer.update();
}
return 0;
}
static int paStreamCallback(
const void *input, void *output,
unsigned long frameCount,
const PaStreamCallbackTimeInfo* timeInfo,
PaStreamCallbackFlags statusFlags,
void *userData )
{
OutStream* outStream = (OutStream*) userData;
// We must not hold the PyGIL here!
PyScopedLock lock(outStream->player->lock);
if(outStream->needRealtimeReset) {
outStream->needRealtimeReset = false;
setRealtime();
}
outStream->player->readOutStream((OUTSAMPLE_t*) output, frameCount * outStream->player->outNumChannels, NULL);
return paContinue;
}
static
int paStreamCallback(
const void *input, void *output,
unsigned long frameCount,
const PaStreamCallbackTimeInfo* timeInfo,
PaStreamCallbackFlags statusFlags,
void *userData )
{
PlayerObject* player = (PlayerObject*) userData;
// We must not hold the PyGIL here!
PyScopedLock lock(player->lock);
if(player->needRealtimeReset) {
player->needRealtimeReset = 0;
setRealtime();
}
player->readOutStream((int16_t*) output, frameCount * player->outNumChannels);
return paContinue;
}
