void TeamDataProvider::update(TeamMateData& teamMateData)
{
DECLARE_PLOT("module:TeamDataProvider:sslVisionOffset");
DECLARE_PLOT("module:TeamDataProvider:ntpOffset1"); // 1-4: players
DECLARE_PLOT("module:TeamDataProvider:ntpOffset2");
DECLARE_PLOT("module:TeamDataProvider:ntpOffset3");
DECLARE_PLOT("module:TeamDataProvider:ntpOffset4");
DECLARE_PLOT("module:TeamDataProvider:ntpOffset12"); // SSL vision data
PLOT("module:TeamDataProvider:ntpOffset1", ntp.timeSyncBuffers[1].bestOffset);
PLOT("module:TeamDataProvider:ntpOffset2", ntp.timeSyncBuffers[2].bestOffset);
PLOT("module:TeamDataProvider:ntpOffset3", ntp.timeSyncBuffers[3].bestOffset);
PLOT("module:TeamDataProvider:ntpOffset4", ntp.timeSyncBuffers[4].bestOffset);
PLOT("module:TeamDataProvider:ntpOffset12", ntp.timeSyncBuffers[12].bestOffset);
teamMateData = theTeamMateData;
teamMateData.numOfConnectedPlayers = 0;
teamMateData.firstTeamMate = teamMateData.secondTeamMate = teamMateData.thirdTeamMate = TeamMateData::numOfPlayers;
for(int i = TeamMateData::firstPlayer; i < TeamMateData::numOfPlayers; ++i)
if(teamMateData.timeStamps[i] && theFrameInfo.getTimeSince(teamMateData.timeStamps[i]) < networkTimeout)
{
if(!teamMateData.isPenalized[i] && teamMateData.hasGroundContact[i])
{
++teamMateData.numOfConnectedPlayers;
if(teamMateData.numOfConnectedPlayers == 1) teamMateData.firstTeamMate = i;
else if(teamMateData.numOfConnectedPlayers == 2) teamMateData.secondTeamMate = i;
else if(teamMateData.numOfConnectedPlayers == 3) teamMateData.thirdTeamMate = i;
}
else
teamMateData.timeStamps[i] = 0;
}
if(teamMateData.numOfConnectedPlayers > 0)
teamMateData.wasConnected = theTeamMateData.wasConnected = true;
teamMateData.sendThisFrame = (ntp.doSynchronization(theFrameInfo.time, Global::getTeamOut()) ||
theFrameInfo.getTimeSince(lastSentTimeStamp) >= 100) // TODO config file?
#ifdef TARGET_ROBOT
&& !(theMotionRequest.motion == MotionRequest::specialAction && theMotionRequest.specialActionRequest.specialAction == SpecialActionRequest::playDead)
&& !(theMotionInfo.motion == MotionRequest::specialAction && theMotionInfo.specialActionRequest.specialAction == SpecialActionRequest::playDead)
#endif
;
if(teamMateData.sendThisFrame)
{
lastSentTimeStamp = theFrameInfo.time;
TEAM_OUTPUT(idTeamMateIsPenalized, bin, (theRobotInfo.penalty != PENALTY_NONE));
TEAM_OUTPUT(idTeamMateHasGroundContact, bin, (theGroundContactState.contact || !theDamageConfiguration.useGroundContactDetection));
TEAM_OUTPUT(idTeamMateIsUpright, bin, (theFallDownState.state == theFallDownState.upright));
if(theGroundContactState.contact || !theDamageConfiguration.useGroundContactDetection)
TEAM_OUTPUT(idTeamMateTimeSinceLastGroundContact, bin, lastSentTimeStamp);
}
}
void BH2009BehaviorControl::update(BehaviorControlOutput& behaviorControlOutput)
{
this->behaviorControlOutput.ownTeamInfo = theOwnTeamInfo;
this->behaviorControlOutput.robotInfo = theRobotInfo;
this->behaviorControlOutput.gameInfo = theGameInfo;
// update symbols
for(std::list<Symbols*>::iterator i = symbols.begin(); i != symbols.end(); ++i)
(*i)->update();
// set agent
if (currentAgent != theBehaviorConfiguration.agent)
{
currentAgent = theBehaviorConfiguration.agent;
if (!setSelectedAgent(currentAgent.c_str()))
{
OUTPUT(idText, text, "BH2009BehaviorControl: Unable to enable selected agent \"" << currentAgent << "\".");
}
}
if (0) {
// execute the engine
executeEngine();
}
readWriteCognitionSharedMemory();
behaviorControlOutput = this->behaviorControlOutput;
TEAM_OUTPUT(idTeamMateBehaviorData, bin, behaviorControlOutput.behaviorData);
}
void BH2009BehaviorControl::update(MotionRequest& motionRequest)
{
motionRequest = theBehaviorControlOutput.motionRequest;
if(Global::getReleaseOptions().motionRequest)
{
TEAM_OUTPUT(idMotionRequest, bin, motionRequest);
}
}
void TeamDataProvider::handleMessages(MessageQueue& teamReceiver)
{
if(theInstance)
{
teamReceiver.handleAllMessages(*theInstance);
TEAM_OUTPUT(idRobot, bin, theInstance->theRobotInfoBH.number);
}
teamReceiver.clear();
}
void ObstacleCombinator::update(ObstacleModel& obstacleModel)
{
MODIFY("parameters:ObstacleCombinator", parameters);
// Perform grid cell clustering and add clusters as obstacles:
memset(clusterAssignment, 0, USObstacleGrid::GRID_SIZE * sizeof(int));
obstacleModel.obstacles.clear();
currentCluster.init();
int currentClusterIndex(1);
for(int y = 1; y < USObstacleGrid::GRID_LENGTH - 1; ++y)
{
for(int x = 1; x < USObstacleGrid::GRID_LENGTH - 1; ++x)
{
const int currentCellIndex = y * USObstacleGrid::GRID_LENGTH + x;
const USObstacleGrid::Cell& c = theUSObstacleGrid.cells[currentCellIndex];
if(clusterAssignment[currentCellIndex] == 0 && c.state >= theUSObstacleGrid.cellOccupiedThreshold)
{
clusterAssignment[currentCellIndex] = currentClusterIndex;
// iterative implementation of floodfill algorithm
cellStack.push(Vector2<int>(x, y));
while(!cellStack.empty())
{
int x = cellStack.top().x;
int y = cellStack.top().y;
cellStack.pop();
currentCluster.cells.push_back(Vector2<int>(x, y));
if(x == 0 || y == 0 || x == USObstacleGrid::GRID_LENGTH - 1 || y == USObstacleGrid::GRID_LENGTH - 1) //ignore border
continue;
// Test all eight neighbors (also stupid check for center cell)
for(int y2 = y - 1; y2 <= y + 1; ++y2)
{
for(int x2 = x - 1; x2 <= x + 1; ++x2)
{
const int neighborCellIndex = y2 * USObstacleGrid::GRID_LENGTH + x2;
const USObstacleGrid::Cell& cn = theUSObstacleGrid.cells[neighborCellIndex];
if(clusterAssignment[neighborCellIndex] == 0 &&
((cn.state >= theUSObstacleGrid.cellOccupiedThreshold && !parameters.clusterNonMaxThresholdCells) ||
(cn.state > 0 && parameters.clusterNonMaxThresholdCells)))
{
clusterAssignment[neighborCellIndex] = currentClusterIndex;
cellStack.push(Vector2<int>(x2, y2));
}
}
}
}
++currentClusterIndex;
if(currentCluster.cells.size() >= (unsigned int)(parameters.minClusterSize))
generateObstacleFromCurrentCluster(obstacleModel.obstacles);
currentCluster.init();
}
}
}
// Robot dimensions:
const float robotHeight = 580;
const float robotWidth = 300; // Guessed value
const float robotDepth = 150; // Guessed value
const int robotSize = 9; // Guessed value
// Add robot obstacles:
for(RobotsModel::RCIt robot = theRobotsModel.robots.begin(); robot != theRobotsModel.robots.end(); robot++)
{
float robotDistance = robot->relPosOnField.abs();
if((!robot->standing && parameters.considerLyingRobots) && (robotDistance < parameters.maxRobotDistance))
{
Vector2<> widthOffset(robotHeight / 2.0f, 0.0f);
Vector2<> robotCenter(robot->relPosOnField);
Vector2<> closestRobotPoint(robotCenter);
closestRobotPoint.normalize(robotCenter.abs() - robotWidth / 2.0f); // Assume that robot lies crosswise
Vector2<> leftCorner(widthOffset);
leftCorner.rotateLeft();
leftCorner += robotCenter;
Vector2<> rightCorner(widthOffset);
rightCorner.rotateRight();
rightCorner += robotCenter;
obstacleModel.obstacles.push_back(ObstacleModel::Obstacle(leftCorner, rightCorner, robotCenter, closestRobotPoint,
robotSize, robot->covariance, ObstacleModel::Obstacle::ROBOT));
}
else if((robot->standing && parameters.considerStandingRobots) && (robotDistance < parameters.maxRobotDistance))
{
Vector2<> widthOffset(robotWidth / 2.0f, 0.0f);
Vector2<> robotCenter(robot->relPosOnField);
Vector2<> closestRobotPoint(robotCenter);
closestRobotPoint.normalize(robotCenter.abs() - robotWidth / 2.0f);
Vector2<> leftCorner(widthOffset);
leftCorner.rotateLeft();
leftCorner += robotCenter;
Vector2<> rightCorner(widthOffset);
rightCorner.rotateRight();
rightCorner += robotCenter;
obstacleModel.obstacles.push_back(ObstacleModel::Obstacle(leftCorner, rightCorner, robotCenter, closestRobotPoint,
robotSize, robot->covariance, ObstacleModel::Obstacle::ROBOT));
}
}
// Add arm obstacles:
Vector2<> robotCenter(0.0f, robotWidth / 2.0f + robotDepth / 2.0f);
float robotDistance = robotCenter.abs();
if(parameters.considerArmCollisions
&& (theArmContactModel.contactLeft || theArmContactModel.contactRight)
&& (robotDistance < parameters.maxRobotDistance))
{
// Assume chest to arm contact
Vector2<> widthOffset(0.0f, robotWidth / 2.0f);
Vector2<> closestRobotPoint(0.0f, robotWidth / 2.0f);
Vector2<> leftCorner(widthOffset);
leftCorner.rotateRight();
leftCorner += robotCenter;
Vector2<> rightCorner(widthOffset);
rightCorner.rotateLeft();
rightCorner += robotCenter;
Matrix2x2<> covariance(robotWidth / 2.0f, 0.0f, 0.0f, robotWidth / 2.0f);
if(theArmContactModel.contactLeft)
obstacleModel.obstacles.push_back(ObstacleModel::Obstacle(leftCorner, rightCorner, robotCenter, closestRobotPoint,
robotSize, covariance, ObstacleModel::Obstacle::ARM));
if(theArmContactModel.contactRight)
obstacleModel.obstacles.push_back(ObstacleModel::Obstacle(-leftCorner, -rightCorner, -robotCenter, -closestRobotPoint,
robotSize, covariance, ObstacleModel::Obstacle::ARM));
}
TEAM_OUTPUT(idTeamMateObstacleModel, bin, obstacleModel);
}
void TeamDataSender::update(TeamDataSenderOutput& teamDataSenderOutput)
{
if(theTeammateData.sendThisFrame)
{
++sendFrames;
TEAM_OUTPUT(idRobot, bin, theRobotInfo.number);
// Own pose information and ball observation:
TEAM_OUTPUT(idRobotPose, bin, RobotPoseCompressed(theRobotPose));
TEAM_OUTPUT(idSideConfidence, bin, theSideConfidence);
TEAM_OUTPUT(idBallModel, bin, BallModelCompressed(theBallModel));
TEAM_OUTPUT(idTeammateBallAge, bin, static_cast<float>(theBallModel.timeWhenLastSeen ? theFrameInfo.getTimeSince(theBallModel.timeWhenLastSeen) / 1000.f : -1.f));
TEAM_OUTPUT(idFieldFeatureOverview, bin, theFieldFeatureOverview);
// Obstacle stuff
TEAM_OUTPUT(idObstacleModelCompressed, bin, ObstacleModelCompressed(theObstacleModel, maxNumberOfObstaclesToSend));
// Information about the behavior (i.e. the robot's state and intentions)
TEAM_OUTPUT(idBehaviorStatus, bin, theBehaviorStatus);
TEAM_OUTPUT(idSPLStandardBehaviorStatus, bin, theSPLStandardBehaviorStatus);
TEAM_OUTPUT(idTeammateRoles, bin, theTeammateRoles);
// Robot status
TEAM_OUTPUT(idTeammateIsPenalized, bin, (theRobotInfo.penalty != PENALTY_NONE));
TEAM_OUTPUT(idTeammateHasGroundContact, bin, (theGroundContactState.contact && theMotionInfo.motion != MotionInfo::getUp && theMotionRequest.motion != MotionRequest::getUp));
TEAM_OUTPUT(idTeammateIsUpright, bin, (theFallDownState.state == theFallDownState.upright));
if(theGroundContactState.contact)
{
TEAM_OUTPUT(idTeammateTimeOfLastGroundContact, bin, theFrameInfo.time);
}
TEAM_OUTPUT(idWhistle, bin, theWhistle);
TEAM_OUTPUT(idTeammateFieldCoverage, bin, FieldCoverageLineCompressed(theFieldCoverage[lineToSendNext++ % FieldCoverage::numOfCellsY]));
if(sendFrames % 20 == 0)
{
TEAM_OUTPUT(idRobotHealth, bin, theRobotHealth);
}
//TEAM_OUTPUT(idNetworkThumbnail, bin, theNetworkThumbnail);
}
}
void RobotHealthProvider::update(RobotHealth& robotHealth)
{
// Transfer information from other process:
robotHealth = theMotionRobotHealth;
// Directly transfer temperature and batteryLevel data from SensorData:
robotHealth.batteryLevel = theFilteredSensorData.data[SensorData::batteryLevel] == SensorData::off ? 1.f : theFilteredSensorData.data[SensorData::batteryLevel];
unsigned char maxTemperature(0);
for(int i=0; i<JointData::numOfJoints; ++i)
if(theFilteredSensorData.temperatures[i] > maxTemperature)
maxTemperature = theFilteredSensorData.temperatures[i];
robotHealth.maxTemperature = maxTemperature;
// Add cpu load, memory load and robot name:
SystemCall::getLoad(robotHealth.memoryLoad, robotHealth.load);
robotHealth.robotName = Global::getSettings().robot;
// Compute frame rate of cognition process:
timeBuffer.add(SystemCall::getCurrentSystemTime());
unsigned timeDiffSum(0);
int numOfTimeDiffs(0);
for(int i=0; i < timeBuffer.getNumberOfEntries() - 1; ++i)
{
timeDiffSum += (timeBuffer[i] - timeBuffer[i+1]);
++numOfTimeDiffs;
}
robotHealth.cognitionFrameRate = timeDiffSum ? 1000.0F / (static_cast<float>(timeDiffSum)/numOfTimeDiffs) : 0.0F;
// Communicate health:
if(Global::getReleaseOptions().robotHealth)
{
TEAM_OUTPUT(idRobotHealth, bin, robotHealth);
}
if(theFilteredSensorData.timeStamp)
{
//battery warning
if(lastBatteryLevel < robotHealth.batteryLevel)
batteryVoltageFalling = false;
else if(lastBatteryLevel > robotHealth.batteryLevel)
batteryVoltageFalling = true;
if(robotHealth.batteryLevel < theSensorCalibration.batteryLow)
{
if(batteryVoltageFalling && theFrameInfo.getTimeSince(startBatteryLow) > 1000)
{
SoundPlayer::play("lowbattery.wav");
//next warning in 90 seconds
//startBatteryLow = theFrameInfo.time + 90000;
batteryVoltageFalling = false;
}
}
else if(startBatteryLow < theFrameInfo.time)
startBatteryLow = theFrameInfo.time;
lastBatteryLevel = robotHealth.batteryLevel;
//temperature warning
if(maxTemperature > theSensorCalibration.temperatureHigh)
{
if(theFrameInfo.getTimeSince(highTemperatureSince) > 1000)
{
SoundPlayer::play("heat.wav");
highTemperatureSince = theFrameInfo.time + 20000;
}
}
else if(highTemperatureSince < theFrameInfo.time)
highTemperatureSince = theFrameInfo.time;
}
}
void TeamDataSender::update(TeamDataSenderOutput& teamDataSenderOutput)
{
if(theTeamMateData.sendThisFrame)
{
++sendFrames;
// Own pose information and ball observation:
TEAM_OUTPUT(idTeamMateRobotPose, bin, RobotPoseCompressed(theRobotPose));
TEAM_OUTPUT(idTeamMateSideConfidence, bin, theSideConfidence);
TEAM_OUTPUT(idTeamMateBallModel, bin, BallModelCompressed(theBallModel));
// Obstacle stuff
TEAM_OUTPUT(idObstacleClusters, bin, ObstacleClustersCompressed(theObstacleClusters, maxNumberOfObstacleClustersToSend));
TEAM_OUTPUT(idTeamMateRobotsModel, bin, RobotsModelCompressed(theRobotsModel, maxNumberOfRobotsToSend));
TEAM_OUTPUT(idTeamMateObstacleModel, bin,ObstacleModelCompressed(theObstacleModel, maxNumberOfObstaclesToSend));
// Robot status
TEAM_OUTPUT(idTeamMateIsPenalized, bin, (theRobotInfo.penalty != PENALTY_NONE));
TEAM_OUTPUT(idTeamMateHasGroundContact, bin, theGroundContactState.contact);
TEAM_OUTPUT(idTeamMateIsUpright, bin, (theFallDownState.state == theFallDownState.upright));
if(theGroundContactState.contact)
TEAM_OUTPUT(idTeamMateTimeSinceLastGroundContact, bin, theFrameInfo.time);
TEAM_OUTPUT(idTeamCameraHeight, bin, theCameraMatrix.translation.z);
if(sendFrames % 20 == 0)
TEAM_OUTPUT(idRobotHealth, bin, theRobotHealth);
}
}
