void NaoImageTranscriber::run() { Thread::running = true; Thread::trigger->on(); long long lastProcessTimeAvg = VISION_FRAME_LENGTH_uS; struct timespec interval, remainder; while (Thread::running) { PROF_ENTER(P_MAIN); PROF_ENTER(P_GETIMAGE); //start timer const long long startTime = monotonic_micro_time(); topImageTranscriber.waitForImage(); bottomImageTranscriber.waitForImage(); subscriber->notifyNextVisionImage(); PROF_EXIT(P_GETIMAGE); //stop timer const long long processTime = monotonic_micro_time() - startTime; //sleep until next frame lastProcessTimeAvg = lastProcessTimeAvg/2 + processTime/2; if (processTime > VISION_FRAME_LENGTH_uS) { if (processTime > VISION_FRAME_LENGTH_PRINT_THRESH_uS) { #ifdef DEBUG_ALIMAGE_LOOP cout << "Time spent in ALImageTranscriber loop longer than" << " frame length: " << processTime <<endl; #endif } //Don't sleep at all } else{ const long int microSleepTime = static_cast<long int>(VISION_FRAME_LENGTH_uS - processTime); const long int nanoSleepTime = static_cast<long int>((microSleepTime %(1000 * 1000)) * 1000); const long int secSleepTime = static_cast<long int>(microSleepTime / (1000*1000)); // cout << "Sleeping for nano: " << nanoSleepTime // << " and sec:" << secSleepTime << endl; interval.tv_sec = static_cast<time_t>(secSleepTime); interval.tv_nsec = nanoSleepTime; nanosleep(&interval, &remainder); } PROF_EXIT(P_MAIN); PROF_NFRAME(); } Thread::trigger->off(); }
// Overrides the RoboGram::run() method to do timing as well void DiagramThread::RobotDiagram::run() { // Start timer const long long startTime = realtime_micro_time(); if (name == "cognition") { PROF_ENTER(P_COGNITION_THREAD); } else if (name == "sensors") { PROF_ENTER(P_MOTION_THREAD); } else if (name == "comm") { PROF_ENTER(P_COMM_THREAD); } else if (name == "guardian") { PROF_ENTER(P_GUARDIAN_THREAD); } RoboGram::run(); if (name == "cognition") { PROF_EXIT(P_COGNITION_THREAD); // Count cognition frames PROF_NFRAME(); } else if (name == "sensors") { PROF_EXIT(P_MOTION_THREAD); } else if (name == "comm") { PROF_EXIT(P_COMM_THREAD); } else if (name == "guardian") { PROF_EXIT(P_GUARDIAN_THREAD); } // Stop timer const long long processTime = realtime_micro_time() - startTime; // If we're under the frame length, this is good. // We can sleep for the rest of the frame and let others process. if (processTime < frameLengthMicro) { // Compute the time we should sleep from the amount of time // we processed this frame and the amount of time allotted to a frame const long int microSleepTime = static_cast<long int>(frameLengthMicro - processTime); const long int nanoSleepTime = static_cast<long int>((microSleepTime %(1000 * 1000)) * 1000); const long int secSleepTime = static_cast<long int>(microSleepTime / (1000*1000)); interval.tv_sec = static_cast<time_t>(secSleepTime); interval.tv_nsec = nanoSleepTime; // Sleep! nanosleep(&interval, &remainder); } #ifdef DEBUG_THREADS else if (processTime > frameLengthMicro*1.5) { std::cout<< "Warning: time spent in " << name << " thread longer" << " than frame length: "<< processTime << " uS" << std::endl; } #endif }