void Logger::execute()
{
if(parameters.enabled)
{
if(Blackboard::getInstance().getVersion() != blackboardVersion)
{
loggables.clear();
for(const std::string& representation : parameters.representations)
if(Blackboard::getInstance().exists(representation.c_str()))
{
int i;
for(i = 0; i < numOfDataMessageIDs; ++i)
if(getName((MessageID) i) == "id" + representation)
{
loggables.push_back(Loggable(&Blackboard::getInstance()[representation.c_str()], (MessageID) i));
break;
}
if(i == numOfDataMessageIDs)
OUTPUT_WARNING("Logger: " << representation << " has no message id.");
}
else
{ // This pair of braces avoids strange bug in VS2013
OUTPUT_WARNING("Logger: " << representation << " is not available in blackboard.");
}
blackboardVersion = Blackboard::getInstance().getVersion();
}
beginFrame(SystemCall::getCurrentSystemTime());
Cabsl<Logger>::execute(OptionInfos::getOption("Root"));
endFrame();
}
}
/*
* get back to stand
*/
void BallTaking::phaseFive(BallTakingOutput& output)
{
int t = theFrameInfo.getTimeSince(phaseStart);
float tp = sinUp((float)t / phaseDuration);
targetLeftFootPositon = leftEndPhaseFour;
targetRightFootPositon = rightEndPhaseFour;
targetLeftFootPositon.translation = leftEndPhaseFour.translation + (standLeftFoot.translation - leftEndPhaseFour.translation) * tp;
targetRightFootPositon.translation = rightEndPhaseFour.translation + (standRightFoot.translation - rightEndPhaseFour.translation) * tp;
if(side > 0) //left swing
{
targetLeftFootPositon.rotateZ(-rot * tp);
}
else //right swing
{
targetRightFootPositon.rotateZ(rot * tp);
}
if(!InverseKinematic::calcLegJoints(targetLeftFootPositon, targetRightFootPositon, output, theRobotDimensions, 0.5f))
OUTPUT_WARNING("not reachable 5");
//leaving possible ?
if(theFrameInfo.getTimeSince(phaseStart) >= phaseDuration)
{
phaseLeavingPossible = true;
}
}
/*
* move ball out 2
*/
void BallTaking::phaseFour(BallTakingOutput& output)
{
int t = theFrameInfo.getTimeSince(phaseStart);
float tp = sinUp((float)t / phaseDuration);
targetLeftFootPositon = leftEndPhaseThree;
targetRightFootPositon = rightEndPhaseThree;
if(side > 0) //left swing
{
targetLeftFootPositon.translation += Vector3<>(tp * 100, 0, tp * 25);
targetRightFootPositon.translation += Vector3<>(0, 0, tp * 20);
}
else //right swing
{
targetLeftFootPositon.translation += Vector3<>(0, 0, tp * 20);
targetRightFootPositon.translation += Vector3<>(tp * 100, 0, tp * 25);
}
if(!InverseKinematic::calcLegJoints(targetLeftFootPositon, targetRightFootPositon, output, theRobotDimensions, 0.5f))
OUTPUT_WARNING("not reachable 4");
//leaving possible ?
if(theFrameInfo.getTimeSince(phaseStart) >= phaseDuration)
{
leftEndPhaseFour = targetLeftFootPositon;
rightEndPhaseFour = targetRightFootPositon;
phaseLeavingPossible = true;
}
}
bool NaoCamera::setControlSettings(std::list<CameraSettingsBH::V4L2Setting> controlsettings,
std::list<CameraSettingsBH::V4L2Setting> appliedControlSettings)
{
std::list<CameraSettingsBH::V4L2Setting>::iterator ait = appliedControlSettings.begin();
std::list<CameraSettingsBH::V4L2Setting>::const_iterator it = controlsettings.begin();
std::list<CameraSettingsBH::V4L2Setting>::const_iterator end = controlsettings.end();
bool success = true;
int counter = 0;
for(; it != end && timeStamp - lastCameraSettingTimestamp >= cameraSettingApplicationRate; ++it, ++ait, counter++)
{
if(it->value == ait->value)
{
continue; // This setting has successfully been applied, so we don't have to deal with it.
}
if(!setControlSetting(it->command, it->value))
{
OUTPUT_WARNING("NaoCamera: Setting camera control value failed: " << it->command);
success = false; // The settings should have been set but wasn't => no success.
}
else
{
appliedSettings.setSetting(*it);
lastCameraSettingTimestamp = timeStamp;
}
}
return success;
}
void Logger::logFrame()
{
if(writeIndex == (readIndex + parameters.maxBufferSize - 1) % parameters.maxBufferSize)
{ // Buffer is full, can't do anything this frame
OUTPUT_WARNING("Logger: Writer thread too slow, discarding frame.");
return;
}
OutMessage& out = buffer[writeIndex]->out;
out.bin << 'c';
out.finishMessage(idProcessBegin);
// Stream all representations to the queue
for(const Loggable& loggable : loggables)
{
out.bin << *loggable.representation;
if(!out.finishMessage(loggable.id))
OUTPUT_WARNING("Logging of " << ::getName(loggable.id) << " failed. The buffer is full.");
}
// Append timing data if any
MessageQueue& timingData = Global::getTimingManager().getData();
if(timingData.getNumberOfMessages() > 0)
timingData.copyAllMessages(*buffer[writeIndex]);
else
OUTPUT_WARNING("Logger: No timing data available.");
out.bin << 'c';
out.finishMessage(idProcessFinished);
// Cognition runs at 60 fps. Therefore use a new block every 60 frames.
// Thus one block is used per second.
if(++frameCounter == 60)
{
// The next call to logFrame will use a new block.
writeIndex = (writeIndex + 1) % parameters.maxBufferSize;
framesToWrite.post(); // Signal to the writer thread that another block is ready
if(parameters.debugStatistics)
{
int diff = (parameters.maxBufferSize + readIndex - writeIndex) % parameters.maxBufferSize;
OUTPUT_WARNING("Logger buffer is " << diff / (float) parameters.maxBufferSize * 100 << "% free.");
}
frameCounter = 0;
}
}
void AudioProvider::update(AudioData& audioData)
{
audioData.channels = channels;
audioData.sampleRate = sampleRate;
unsigned available = std::min((unsigned) snd_pcm_avail(handle), maxFrames);
audioData.samples.resize(available * channels);
int status = snd_pcm_readi(handle, audioData.samples.data(), available);
if(status < 0)
{
OUTPUT_WARNING("Lost audio stream (" << status << "), recovering...");
snd_pcm_recover(handle, status, 1);
ASSERT(channels <= 4);
short buf[4];
VERIFY(snd_pcm_readi(handle, buf, 1) >= 0);
audioData.samples.clear();
}
}
/*
* bank in
*/
void BallTaking::phaseOne(BallTakingOutput& output)
{
int t = theFrameInfo.getTimeSince(phaseStart);
float tp = (float)t / phaseDuration;
y = sinUp(tp) * xFactor;
rotX = sinUp(tp) * rotXFactor;
zStand = sinUp(tp) * zStandFactor;
rotZ = sinUp(tp) * rot;
y2 = sinUp(tp) * y2Factor;
if(side > 0) //left swing
{
targetLeftFootPositon = Pose3D(standLeftFoot.translation.x + (y2 * 80), standLeftFoot.translation.y + y + (y2 * 80), standLeftFoot.translation.z + zStand);
targetRightFootPositon = Pose3D(standRightFoot.translation.x, standRightFoot.translation.y + y, standRightFoot.translation.z + zStand);
targetLeftFootPositon.rotateZ(rotZ);
output.angles[JointData::RHipRoll] = -rotX;
output.angles[JointData::LHipRoll] = rotX - y2;
}
else //right swing
{
targetLeftFootPositon = Pose3D(standLeftFoot.translation.x, standLeftFoot.translation.y - y, standLeftFoot.translation.z + zStand);
targetRightFootPositon = Pose3D(standRightFoot.translation.x + (y2 * 80), standRightFoot.translation.y - y - (y2 * 80), standRightFoot.translation.z + zStand);
targetRightFootPositon.rotateZ(-rotZ);
output.angles[JointData::RHipRoll] = rotX - y2;
output.angles[JointData::LHipRoll] = -rotX;
}
if(!InverseKinematic::calcLegJoints(targetLeftFootPositon, targetRightFootPositon, output, theRobotDimensions, 0.5f))
OUTPUT_WARNING("not reachable 1");
//leaving possible ?
if(theFrameInfo.getTimeSince(phaseStart) >= phaseDuration)
{
leftEndPhaseOne = targetLeftFootPositon;
rightEndPhaseOne = targetRightFootPositon;
phaseLeavingPossible = true;
}
}
/*
* wait for taking
*/
void BallTaking::phaseTwo(BallTakingOutput& output)
{
if(!InverseKinematic::calcLegJoints(leftEndPhaseOne, rightEndPhaseOne, output, theRobotDimensions, 0.5f))
OUTPUT_WARNING("not reachable 2");
bool time = theFrameInfo.getTimeSince(theBallModel.timeWhenLastSeen) > 500;
bool position = theBallModel.estimate.position.x < 0;
bool moving = moved.isFilled() && moved.getAverageFloat() < 1; //todo test
if(time || position || moving)
{
if(theBallModel.estimate.position.abs() > 200)
{
phase = 4;
leftEndPhaseFour = leftEndPhaseOne;
rightEndPhaseFour = rightEndPhaseOne;
}
phaseLeavingPossible = true;
}
}
bool NaoCamera::setControlSetting(unsigned int id, int value)
{
struct v4l2_queryctrl queryctrl;
queryctrl.id = id;
if(ioctl(fd, VIDIOC_QUERYCTRL, &queryctrl) < 0)
{
OUTPUT_WARNING("NaoCamera: VIDIOC_QUERYCTRL call failed");
return false;
}
if(queryctrl.flags & V4L2_CTRL_FLAG_DISABLED)
{
OUTPUT_WARNING("NaoCamera: VIDIOC_QUERYCTRL call failed. Command " << id << " disabled");
return false; // not available
}
if(queryctrl.type != V4L2_CTRL_TYPE_BOOLEAN && queryctrl.type != V4L2_CTRL_TYPE_INTEGER && queryctrl.type != V4L2_CTRL_TYPE_MENU)
{
OUTPUT_WARNING("NaoCamera: VIDIOC_QUERYCTRL call failed. Command " << id << " not supported");
return false; // not supported
}
// clip value
if(value < queryctrl.minimum)
{
OUTPUT_WARNING("NaoCamera: Clipping control value. ID: " << id << " to " << queryctrl.minimum);
value = queryctrl.minimum;
}
if(value > queryctrl.maximum)
{
value = queryctrl.maximum;
OUTPUT_WARNING("NaoCamera: Clipping control value. ID: " << id << " to " << queryctrl.maximum);
}
struct v4l2_control control_s;
control_s.id = id;
control_s.value = value;
if(ioctl(fd, VIDIOC_S_CTRL, &control_s) < 0)
{
OUTPUT_WARNING("NaoCamera: Setting value ID: " << id << " failed. VIDIOC_S_CTRL return value < 0");
return false;
}
return true;
}
void TeamMarkerPerceptor::extractTeamMarkers(TeamMarkerSpots& teamMarkerSpots)
{
typedef const RegionPercept::Region* R;
unsigned start = SystemCall::getRealSystemTime();
for(R region = theRegionPercept.regions, end = region + theRegionPercept.regionsCounter; region < end; ++region)
{
switch(region->color)
{
case ColorClasses::blue:
case ColorClasses::robotBlue:
case ColorClasses::red:
{
unsigned duration = SystemCall::getRealSystemTime() - start;
if(duration > params.maxDuration)
{
DEBUG_RESPONSE("module:TeamMarkerPerceptor:time",
{
OUTPUT_WARNING("SKIPPED team marker perception (duration: " << duration << " ms)");
});
return;
}
TeamMarkerSpots::TeamMarkerSpot teamMarker;
teamMarker.color = region->color;
ABORTABLE_STEP(regionCheck(region), *region->childs[0], ColorClasses::red)
ABORTABLE_STEP(regionGrowing(teamMarker), approxCog, ColorClasses::green)
ABORTABLE_STEP(scanlineRegionArea(teamMarker), approxCog, ColorClasses::yellow)
ABORTABLE_STEP(scanlineRegionPrincipalAxis(teamMarker), approxCog, ColorClasses::orange)
polygonDebugDrawing(teamMarker);
teamMarkerSpots.teamMarkers.push_back(teamMarker);
break;
}
void warning(const std::string &msg)
{
#ifndef TARGET_TOOL
OUTPUT_WARNING("BehaviorWarning: " << msg);
#endif
}