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();
}
void CommModule::send()
{
_worldModelInput.latch();
teamConnect->send(_worldModelInput.message(), myPlayerNumber(),
gameConnect->myTeamNumber(), 1);
PROF_ENTER(P_COMM_TIMER);
timer->teamPacketSent();
PROF_EXIT(P_COMM_TIMER);
}
void ScriptedProvider::setNextBodyCommand(std::vector<float>& sensorAngles) {
// If there are no more commands, don't try to enqueue one
if ( !bodyCommandQueue.empty() ) {
BodyJointCommand::ptr nextCommand = bodyCommandQueue.front();
bodyCommandQueue.pop();
PROF_ENTER(P_CHOPPED);
// Replace the current command
currCommand = chopper.chopCommand(nextCommand, sensorAngles);
PROF_EXIT(P_CHOPPED);
}
}
void CommModule::run_()
{
PROF_ENTER(P_COMM_RECEIVE);
receive();
PROF_EXIT(P_COMM_RECEIVE);
teamConnect->checkDeadTeammates(_worldModels,
timer->timestamp(),
myPlayerNumber());
burstRate = monitor->performHealthCheck(timer->timestamp());
#ifdef LOG_COMM
monitor->logOutput(timer->timestamp());
#endif
PROF_ENTER(P_COMM_SEND);
if (timer->timeToSend() && myPlayerNumber() > 0)
{
send();
}
else
{
// Make profiler happy. SO UGLY!!!
PROF_ENTER(P_COMM_BUILD_PACKET);
PROF_EXIT(P_COMM_BUILD_PACKET);
PROF_ENTER(P_COMM_SERIALIZE_PACKET);
PROF_EXIT(P_COMM_SERIALIZE_PACKET);
PROF_ENTER(P_COMM_TO_SOCKET);
PROF_EXIT(P_COMM_TO_SOCKET);
PROF_ENTER(P_COMM_TIMER);
PROF_EXIT(P_COMM_TIMER);
}
PROF_EXIT(P_COMM_SEND);
}
// The heart of the transcriber, returns an image full of pixels from video mem
messages::YUVImage ImageTranscriber::getNextImage()
{
// dequeue a frame buffer (this call blocks when there is
// no new image available)
if(cameraType == Camera::TOP)
{
PROF_ENTER(P_TOP_DQBUF);
}
else
{
PROF_ENTER(P_BOT_DQBUF);
}
verify(ioctl(fd, VIDIOC_DQBUF, &requestBuff),
"Dequeueing the frame buffer failed.");
if(cameraType == Camera::TOP)
{
PROF_EXIT(P_TOP_DQBUF);
}
else
{
PROF_EXIT(P_BOT_DQBUF);
}
if(requestBuff.bytesused != (unsigned int)SIZE)
std::cerr << "CAMERA::ERROR::Wrong buffer size!" << std::endl;
static bool shout = true;
if (shout) {
shout = false;
std::cerr << "CAMERA::Camera is working." << std::endl;
}
// Pixels -> TranscriberBuffer -> YUVImage
return messages::YUVImage(new TranscriberBuffer(mem[requestBuff.index],
fd,
requestBuff),
2*WIDTH, HEIGHT, 2*WIDTH);
}
void BehaviorsModule::run_ ()
{
PROF_ENTER(P_BEHAVIORS);
static unsigned int num_crashed = 0;
if (error_state && num_crashed < NUM_PYTHON_RESTARTS_MAX) {
this->reload_hard();
error_state = false;
num_crashed++;
}
// Latch incoming messages and prepare outgoing messages
prepareMessages();
PROF_ENTER(P_PYTHON);
// Call main run() method of Brain
if (brain_instance != NULL) {
PyObject *result = PyObject_CallMethod(brain_instance, "run", NULL);
if (result == NULL) {
// set BehaviorsModule in error state
error_state = true;
// report error
std::cout << "Error occurred in Brain.run() method" << std::endl;
if (PyErr_Occurred()) {
PyErr_Print();
} else {
std::cout << " No Python exception information available"
<< std::endl;
}
} else {
Py_DECREF(result);
}
}
PROF_EXIT(P_PYTHON);
// Send outgoing messages
sendMessages();
PROF_EXIT(P_BEHAVIORS);
}
void TeamConnect::send(const messages::WorldModel& model,
int player, int team, int burst = 1)
{
if (!model.IsInitialized())
{
#ifdef DEBUG_COMM
std::cerr << "Comm does not have a valid input to send." << std::endl;
#endif
return;
}
PROF_ENTER(P_COMM_BUILD_PACKET);
portals::Message<messages::TeamPacket> teamMessage(0);
messages::TeamPacket* packet = teamMessage.get();
packet->mutable_payload()->CopyFrom(model);
packet->set_sequence_number(myLastSeqNum++); // ONE LINE INCREMENT!!
packet->set_player_number(player);
packet->set_team_number(team);
packet->set_header(UNIQUE_ID);
packet->set_timestamp(timer->timestamp());
PROF_EXIT(P_COMM_BUILD_PACKET);
PROF_ENTER(P_COMM_SERIALIZE_PACKET);
char datagram[packet->ByteSize()];
packet->SerializeToArray(&datagram[0], packet->GetCachedSize());
PROF_EXIT(P_COMM_SERIALIZE_PACKET);
PROF_ENTER(P_COMM_TO_SOCKET);
for (int i = 0; i < burst; ++i)
{
socket->sendToTarget(&datagram[0], packet->GetCachedSize());
}
PROF_EXIT(P_COMM_TO_SOCKET);
}
void VisionModule::run_()
{
topThrImage.latch();
topYImage.latch();
topUImage.latch();
topVImage.latch();
botThrImage.latch();
botYImage.latch();
botUImage.latch();
botVImage.latch();
joint_angles.latch();
inertial_state.latch();
PROF_ENTER(P_VISION);
vision->notifyImage(topThrImage.message(), topYImage.message(),
topUImage.message(), topVImage.message(),
botThrImage.message(), botYImage.message(),
botUImage.message(), botVImage.message(),
joint_angles.message(), inertial_state.message());
PROF_EXIT(P_VISION);
updateVisionBall();
updateVisionRobot();
updateVisionField();
updateVisionObstacle();
#ifdef OFFLINE
portals::Message<messages::ThresholdImage> top, bot;
top = new messages::ThresholdImage(vision->thresh->betterDebugImage, 320, 240, 320);
bot = new messages::ThresholdImage(vision->thresh->thresholdedBottom, 320, 240, 320);
topOutPic.setMessage(top);
botOutPic.setMessage(bot);
#endif
/* In order to keep logs synced up, joint angs and inert states are passed
* thru the vision system. Joint angles are taken at around 100 hz, but
* images are taken at 30 hz, but by passing joint angles thru vision we
* get joint angles at 30 hz. */
#ifdef LOG_VISION
joint_angles_out.setMessage(portals::Message<messages::JointAngles>(
&joint_angles.message()));
inertial_state_out.setMessage(portals::Message<messages::InertialState>(
&inertial_state.message()));
#endif
}
void LocalizationModule::run_()
{
// Profiler
PROF_ENTER(P_SELF_LOC);
motionInput.latch();
visionInput.latch();
resetInput.latch();
update();
// Profiler
PROF_EXIT(P_SELF_LOC);
}
void LocalizationModule::run_()
{
PROF_ENTER(P_SELF_LOC);
motionInput.latch();
visionInput.latch();
#ifndef OFFLINE
gameStateInput.latch();
ballInput.latch();
resetInput[0].latch();
resetInput[1].latch();
#endif
update();
PROF_EXIT(P_SELF_LOC);
}
void ScriptedProvider::calculateNextJointsAndStiffnesses(
std::vector<float>& sensorAngles,
std::vector<float>& sensorCurrents,
const messages::InertialState& sensorInertials,
const messages::FSR& sensorFSRs)
{
PROF_ENTER(P_SCRIPTED);
if (currCommandEmpty())
setNextBodyCommand(sensorAngles);
// Go through the chains and enqueue the next
// joints from the ChoppedCommand.
boost::shared_ptr<std::vector <std::vector <float> > > currentChains(getCurrentChains(sensorAngles));
currCommand->nextFrame(); // so Python can keep track of progress
for (unsigned int id=0; id< Kinematics::NUM_CHAINS; ++id ) {
Kinematics::ChainID cid = static_cast<Kinematics::ChainID>(id);
if ( currCommand->isDone() ){
setNextChainJoints( cid,
currentChains->at(cid) );
}else{
setNextChainJoints( cid,
currCommand->getNextJoints(cid) );
}
// Curr command will allways provide the current stiffnesses
// even if it is finished providing new joint angles.
setNextChainStiffnesses( cid,
currCommand->getStiffness(cid) );
}
setActive();
PROF_EXIT(P_SCRIPTED);
}
void GuardianModule::run_()
{
PROF_ENTER(P_ROBOGUARDIAN);
temperaturesInput.latch();
chestButtonInput.latch();
footBumperInput.latch();
inertialInput.latch();
fsrInput.latch();
batteryInput.latch();
countButtonPushes();
checkFalling();
checkFallen();
checkFeetOnGround();
checkBatteryLevels();
checkTemperatures();
processFallingProtection();
processChestButtonPushes();
processFootBumperPushes();
checkAudio();
frameCount++;
PROF_EXIT(P_ROBOGUARDIAN);
}
// 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
}