bool Debug::handleMessage(InMessage& message)
{
switch(message.getMessageID())
{
case idText: // loop back to GUI
message >> theDebugSender;
return true;
// messages to Cognition
case idColorCalibration:
message >> theCognitionSender;
return true;
// messages to Motion
case idMotionNet:
case idWalkingEngineKick:
message >> theMotionSender;
return true;
// messages to Debug
case idQueueFillRequest:
// Read message queue settings and compute time when next to send (if in a timed mode)
message.bin >> outQueueMode;
sendTime = SystemCall::getCurrentSystemTime() + outQueueMode.timingMilliseconds;
if(fout)
{
delete fout;
fout = 0;
}
return true;
// messages to Cognition and Motion
case idModuleRequest:
case idDebugDataChangeRequest:
case idStreamSpecification:
message >> theCognitionSender;
message >> theMotionSender;
return true;
// messages to all processes
case idDebugRequest:
message >> theCognitionSender;
message >> theMotionSender;
return Process::handleMessage(message);
case idProcessBegin:
message.bin >> processIdentifier;
message.resetReadPosition();
// no break
default:
if(processIdentifier == 'm')
message >> theMotionSender;
else
message >> theCognitionSender;
return true;
}
bool ModuleInfo::handleMessage(InMessage& message, char processIdentifier)
{
if(message.getMessageID() == idModuleTable)
{
int numOfModules;
message.bin >> numOfModules;
for(int i = 0; i < numOfModules; ++i)
{
Module module;
module.processIdentifier = processIdentifier;
int numOfRequirements;
message.bin >> module.name >> module.category >> numOfRequirements;
module.requirements.resize(numOfRequirements);
for(unsigned j = 0; j < module.requirements.size(); ++j)
{
message.bin >> module.requirements[j];
representations.insert(module.requirements[j]);
}
int numOfRepresentations;
message.bin >> numOfRepresentations;
module.representations.resize(numOfRepresentations);
for(unsigned j = 0; j < module.representations.size(); ++j)
{
message.bin >> module.representations[j];
representations.insert(module.representations[j]);
}
std::list<Module>::iterator k;
for(k = modules.begin(); k != modules.end() && *k < module; ++k)
;
modules.insert(k, module);
}
message.bin >> config;
timeStamp = SystemCall::getCurrentSystemTime();
return true;
}
bool TeamDataProvider::handleMessage(InMessage& message)
{
// The robotNumber and the three flags hasGroundContact, isUpright and isPenalized should always be updated.
switch(message.getMessageID())
{
case idNTPHeader:
VERIFY(ntp.handleMessage(message));
timeStamp = ntp.receiveTimeStamp;
return false;
case idNTPIdentifier:
case idNTPRequest:
case idNTPResponse:
return ntp.handleMessage(message);
case idRobot:
message.bin >> robotNumber;
if(robotNumber != theRobotInfo.number)
if(robotNumber >= TeammateData::firstPlayer && robotNumber < TeammateData::numOfPlayers)
{
theTeammateData.timeStamps[robotNumber] = timeStamp;
if(!theTeammateData.isBHumanPlayer[robotNumber])
{
char team;
message.bin >> team;
theTeammateData.behaviorStatus[robotNumber].teamColor = (team == 0) ? BehaviorStatus::blue : BehaviorStatus::red;
}
}
bool LogConverter::isConversionRequired(InMessage& message, const int representationId)
{
switch(representationId)
{
case idSensorData:
case idFilteredSensorData:
return (std::size_t)message.getMessageSize() == sizeofSensorDataRev5703 ||
(std::size_t)message.getMessageSize() == sizeofSensorData_2d284;
case idFrameInfo:
return (std::size_t)message.getMessageSize() == sizeofFrameInfo_83e22;
case idJointData:
case idFilteredJointData:
return (std::size_t)message.getMessageSize() == sizeofJointData_d198df791237;
case idCameraInfo:
return (std::size_t)message.getMessageSize() == 56; // very old logfiles
default:
return false;
}
}
bool ColorTableHandler::handleMessage(InMessage& message)
{
if(message.getMessageID() == idColorTable64)
{
message.bin >> colorTable;
// this is a hack: avoid sending color table to robot, but send any change
// that may be done innediately after this function (see class RobotConsole).
timeStamp = SystemCall::getCurrentSystemTime() - 1;
return true;
}
bool CognitionConfigurationDataProvider::handleMessage(InMessage& message)
{
if(theInstance && message.getMessageID() == idColorCalibration)
{
if(!theInstance->theFieldColors)
theInstance->theFieldColors = std::make_unique<FieldColors>();
message.bin >> *theInstance->theFieldColors;
return true;
}
else
return false;
bool SpecialActions::handleMessage2(InMessage& message)
{
if(message.getMessageID() == idMotionNet)
{
motionNetData.load(message.config);
wasActive = false;
dataRepetitionCounter = 0;
return true;
}
else
return false;
}
bool TimeInfo::handleMessage(InMessage& message)
{
if(message.getMessageID() == idStopwatch)
{
timeStamp = SystemCall::getCurrentSystemTime();
//first get the names of some of the stopwatches (usually we get 3 names per frame)
unsigned short nameCount;
message.bin >> nameCount;
for(int i = 0; i < nameCount; ++i)
{
string watchName;
unsigned short watchId;
message.bin >> watchId;
message.bin >> watchName;
if(names.find(watchId) == names.end()) //new name
{
names[watchId] = watchName;
infos[watchId] = Info();
}
}
//now get timing data
unsigned short dataCount;
message.bin >> dataCount;
for(int i = 0; i < dataCount; ++i)
{
unsigned short watchId;
unsigned time;
message.bin >> watchId;
message.bin >> time;
infos[watchId].add(static_cast<float>(time));
}
unsigned processStartTime;
message.bin >> processStartTime;
unsigned frameNo;
message.bin >> frameNo;
int diff = frameNo - lastFrameNo;
//sometimes we do not get data every frame. Compensate by assuming that the missing frames have
// the same timing as the last one
for(int i = 0; i < diff; ++i)
{
processDeltas.add(static_cast<float>(processStartTime - lastStartTime) / static_cast<float>(diff));
}
lastFrameNo = frameNo;
lastStartTime = processStartTime;
return true;
}
bool Teammate::handleMessage(InMessage& message)
{
switch(message.getMessageID())
{
HANDLE_PARTICLE(RobotPose);
HANDLE_PARTICLE(BallModel);
HANDLE_PARTICLE(ObstacleModel);
HANDLE_PARTICLE(Whistle);
HANDLE_PARTICLE(SideConfidence);
HANDLE_PARTICLE(FieldCoverage);
HANDLE_PARTICLE(RobotHealth);
default:
return false;
}
}
bool SpecialActions::handleMessage2(InMessage& message)
{
if(message.getMessageID() == idMotionNet)
{
std::vector<float> motionData;
float f;
while(!message.bin.eof())
{
message.bin >> f;
motionData.push_back(f);
}
motionNetData.load(motionData);
wasActive = false;
dataRepetitionCounter = 0;
return true;
}
bool TimeInfo::handleMessage(InMessage& message)
{
if(message.getMessageID() == idStopwatch)
{
unsigned time1, time2, counter1;
std::string id;
message.bin >> id >> time1 >> time2 >> counter1;
Info& info = infos[id];
if(SystemCall::getTimeSince(info.lastReceived) > 2000)
reset(info);
info.entries.add(Entry(time1, time2, counter1));
timeStamp = info.lastReceived = SystemCall::getCurrentSystemTime();
return true;
}
bool Process::handleMessage(InMessage& message)
{
switch(message.getMessageID())
{
case idDebugRequest:
{
DebugRequest debugRequest;
message.bin >> debugRequest;
Global::getDebugRequestTable().addRequest(debugRequest);
return true;
}
case idDebugDataChangeRequest:
Global::getDebugDataTable().processChangeRequest(message);
return true;
default:
return false;
}
}
bool TeamDataProvider::handleMessage(InMessage& message)
{
/*
The robotNumber and the three flags hasGroundContact, isUpright and isPenalized should always be updated.
*/
switch(message.getMessageID())
{
case idNTPHeader:
VERIFY(ntp.handleMessage(message));
timeStamp = ntp.receiveTimeStamp;
return false;
case idNTPIdentifier:
case idNTPRequest:
case idNTPResponse:
return ntp.handleMessage(message);
case idRobot:
message.bin >> robotNumber;
if(robotNumber != theRobotInfoBH.number)
if(robotNumber >= TeamMateDataBH::firstPlayer && robotNumber < TeamMateDataBH::numOfPlayers)
theTeamMateDataBH.timeStamps[robotNumber] = timeStamp;
return true;
case idGroundTruthBallModel:
{
Vector2BH<> position;
message.bin >> theGroundTruthBallModelBH.timeWhenLastSeen >> position;
REMOTE_TO_LOCAL_TIME(theGroundTruthBallModelBH.timeWhenLastSeen);
if(theOwnTeamInfoBH.teamColor == TEAM_BLUE)
position *= -1;
theGroundTruthBallModelBH.lastPerception = theGroundTruthRobotPoseBH.invert() * position;
theGroundTruthBallModelBH.estimate.position = theGroundTruthBallModelBH.lastPerception;
}
return true;
case idGroundTruthRobotPose:
{
char teamColor,
id;
unsigned timeStamp;
Pose2DBH robotPose;
message.bin >> teamColor >> id >> timeStamp >> robotPose;
if(teamColor == (int) theOwnTeamInfoBH.teamColor && id == theRobotInfoBH.number)
{
if(theOwnTeamInfoBH.teamColor == TEAM_BLUE)
robotPose = Pose2DBH(pi) + robotPose;
(Pose2DBH&) theGroundTruthRobotPoseBH = robotPose;
}
}
return true;
case idTeamMateIsPenalized:
if(robotNumber != theRobotInfoBH.number)
if(robotNumber >= TeamMateDataBH::firstPlayer && robotNumber < TeamMateDataBH::numOfPlayers)
message.bin >> theTeamMateDataBH.isPenalized[robotNumber];
return true;
case idTeamMateHasGroundContact:
if(robotNumber != theRobotInfoBH.number)
if(robotNumber >= TeamMateDataBH::firstPlayer && robotNumber < TeamMateDataBH::numOfPlayers)
{
message.bin >> theTeamMateData.hasGroundContact[robotNumber];
// This is a potentially evil quick workaround that should be replaced by a better handling of ground contacts of team mates
// at many different places in our code! For a detailed problem description, ask Tim.
if(!theTeamMateData.hasGroundContact[robotNumber])
theTeamMateData.hasGroundContact[robotNumber] = theFrameInfoBH.getTimeSince(theTeamMateDataBH.timeLastGroundContact[robotNumber]) < 2000;
}
return true;
case idTeamMateIsUpright:
if(robotNumber != theRobotInfoBH.number)
if(robotNumber >= TeamMateDataBH::firstPlayer && robotNumber < TeamMateDataBH::numOfPlayers)
message.bin >> theTeamMateDataBH.isUpright[robotNumber];
return true;
case idTeamMateTimeSinceLastGroundContact:
if(robotNumber != theRobotInfoBH.number)
if(robotNumber >= TeamMateDataBH::firstPlayer && robotNumber < TeamMateDataBH::numOfPlayers)
{
message.bin >> theTeamMateDataBH.timeLastGroundContact[robotNumber];
REMOTE_TO_LOCAL_TIME(theTeamMateDataBH.timeLastGroundContact[robotNumber]);
}
bool TeamDataProvider::handleMessage(InMessage& message)
{
/*
The robotNumber and the three flags hasGroundContact, isUpright and isPenalized should always be updated.
*/
switch(message.getMessageID())
{
case idNTPHeader:
VERIFY(ntp.handleMessage(message));
timeStamp = ntp.receiveTimeStamp;
return false;
case idNTPIdentifier:
case idNTPRequest:
case idNTPResponse:
return ntp.handleMessage(message);
case idRobot:
message.bin >> robotNumber;
if(robotNumber != theRobotInfo.number)
if(robotNumber >= TeamMateData::firstPlayer && robotNumber < TeamMateData::numOfPlayers)
theTeamMateData.timeStamps[robotNumber] = timeStamp;
return true;
case idReleaseOptions:
message.bin >> Global::getReleaseOptions();
return true;
case idSSLVisionData:
{
message.bin >> theSSLVisionData;
unsigned remoteTimestamp = theSSLVisionData.recentData.top().receiveTimestamp;
REMOTE_TO_LOCAL_TIME(theSSLVisionData.recentData.top().receiveTimestamp);
unsigned localTimestamp = theSSLVisionData.recentData.top().receiveTimestamp;
int offset = (int) localTimestamp - (int) remoteTimestamp;
PLOT("module:TeamDataProvider:sslVisionOffset", offset);
}
return true;
case idGroundTruthBallModel:
{
Vector2<> position;
message.bin >> theGroundTruthBallModel.timeWhenLastSeen >> position;
REMOTE_TO_LOCAL_TIME(theGroundTruthBallModel.timeWhenLastSeen);
if(theOwnTeamInfo.teamColor == TEAM_BLUE)
position *= -1;
theGroundTruthBallModel.lastPerception.setPositionAndVelocityInFieldCoordinates(
position, Vector2<>(), theGroundTruthRobotPose);
theGroundTruthBallModel.estimate = theGroundTruthBallModel.lastPerception;
}
return true;
case idGroundTruthRobotPose:
{
char teamColor,
id;
unsigned timeStamp;
Pose2D robotPose;
message.bin >> teamColor >> id >> timeStamp >> robotPose;
if(teamColor == (int)theOwnTeamInfo.teamColor && id == theRobotInfo.number)
{
if(theOwnTeamInfo.teamColor == TEAM_BLUE)
robotPose = Pose2D(pi) + robotPose;
(Pose2D&) theGroundTruthRobotPose = robotPose;
}
}
return true;
case idTeamMateIsPenalized:
if(robotNumber != theRobotInfo.number)
if(robotNumber >= TeamMateData::firstPlayer && robotNumber < TeamMateData::numOfPlayers)
message.bin >> theTeamMateData.isPenalized[robotNumber];
return true;
case idTeamMateHasGroundContact:
if(robotNumber != theRobotInfo.number)
if(robotNumber >= TeamMateData::firstPlayer && robotNumber < TeamMateData::numOfPlayers)
message.bin >> theTeamMateData.hasGroundContact[robotNumber];
return true;
case idTeamMateIsUpright:
if(robotNumber != theRobotInfo.number)
if(robotNumber >= TeamMateData::firstPlayer && robotNumber < TeamMateData::numOfPlayers)
message.bin >> theTeamMateData.isUpright[robotNumber];
return true;
case idTeamMateTimeSinceLastGroundContact:
if(robotNumber != theRobotInfo.number)
if(robotNumber >= TeamMateData::firstPlayer && robotNumber < TeamMateData::numOfPlayers)
{
message.bin >> theTeamMateData.timeSinceLastGroundContact[robotNumber];
REMOTE_TO_LOCAL_TIME(theTeamMateData.timeSinceLastGroundContact[robotNumber]);
}
return true;
default:
break;
}
bool CognitionLogDataProvider::handleMessage2(InMessage& message)
{
switch(message.getMessageID())
{
case idFrameInfo:
if(handle(message) && Blackboard::getInstance().exists("Image"))
((Image&) Blackboard::getInstance()["Image"]).timeStamp = ((const FrameInfo&) Blackboard::getInstance()["FrameInfo"]).time;
return true;
case idImage:
if(handle(message) && Blackboard::getInstance().exists("FrameInfo"))
{
FrameInfo& frameInfo = (FrameInfo&) Blackboard::getInstance()["FrameInfo"];
const Image& image = (const Image&) Blackboard::getInstance()["Image"];
frameInfo.cycleTime = (float) (image.timeStamp - frameInfo.time) * 0.001f;
frameInfo.time = image.timeStamp;
}
return true;
case idThumbnail:
if(Blackboard::getInstance().exists("Image"))
{
Thumbnail thumbnail;
message.bin >> thumbnail;
thumbnail.toImage((Image&) Blackboard::getInstance()["Image"]);
}
return true;
case idProcessFinished:
frameDataComplete = true;
return true;
case idStopwatch:
{
const int size = message.getMessageSize();
std::vector<unsigned char> data;
data.resize(size);
message.bin.read(&data[0], size);
Global::getDebugOut().bin.write(&data[0], size);
Global::getDebugOut().finishMessage(idStopwatch);
return true;
}
case idAnnotation:
{
const int size = message.getMessageSize();
std::vector<unsigned char> data;
data.resize(size);
message.bin.read(&data[0], size);
Global::getDebugOut().bin.write(&data[0], size);
Global::getDebugOut().finishMessage(idAnnotation);
return true;
}
case idJPEGImage:
if(Blackboard::getInstance().exists("Image"))
{
JPEGImage jpegImage;
message.bin >> jpegImage;
jpegImage.toImage((Image&) Blackboard::getInstance()["Image"]);
}
