void ExtendedBallPercept::draw() const
{
DECLARE_DEBUG_DRAWING("representation:ExtendedBallPercepts:Image", "drawingOnImage"); // drawing of representation BallPercept
DECLARE_DEBUG_DRAWING("representation:ExtendedballPercepts:Field", "drawingOnField"); // drawing of representation BallPercept
if(isValidBallPercept)
{
CIRCLE("representation:ExtendedBallPercepts:Image",
positionInImage.x,
positionInImage.y,
radiusInImage,
2, // pen width
Drawings::ps_solid,
ColorClasses::black,
Drawings::bs_solid,
ColorRGBA(255,128,64,100));
CIRCLE("representation:ExtendedBallPercepts:Field",
relativePositionOnField.x,
relativePositionOnField.y,
45,
1, // pen width
Drawings::ps_solid,
ColorClasses::orange,
Drawings::bs_null,
ColorClasses::orange);
}
}
void BallPercept::draw() const
{
DECLARE_DEBUG_DRAWING("representation:BallPercept:Image", "drawingOnImage");
DECLARE_DEBUG_DRAWING("representation:BallPercept:Field", "drawingOnField");
DECLARE_DEBUG_DRAWING3D("representation:BallPercept", "origin");
TRANSLATE3D("representation:BallPercept", 0, 0, -230);
if(ballWasSeen)
{
CIRCLE("representation:BallPercept:Image",
positionInImage.x,
positionInImage.y,
radiusInImage,
1, // pen width
Drawings::ps_solid,
ColorClasses::black,
Drawings::bs_solid,
ColorRGBA(255, 128, 64, 100));
CIRCLE("representation:BallPercept:Field",
relativePositionOnField.x,
relativePositionOnField.y,
35,
0, // pen width
Drawings::ps_solid,
ColorClasses::orange,
Drawings::bs_null,
ColorClasses::orange);
// Sorry, no access to field dimensions here, so ball radius is hard coded
SPHERE3D("representation:BallPercept", relativePositionOnField.x, relativePositionOnField.y, 35, 35, ColorClasses::orange);
}
}
void RealisticBallPercepts::draw() const
{
DECLARE_DEBUG_DRAWING("representation:RealisticBallPercepts:image", "drawingOnImage");
DECLARE_DEBUG_DRAWING("representation:RealisticBallPercepts:field", "drawingOnField");
for(const RealisticBallPercept& ball : balls)
{
CIRCLE("representation:RealisticBallPercepts:image", ball.positionInImage.x(), ball.positionInImage.y(), ball.radiusInImage, 2, Drawings::solidPen, ColorRGBA::red, Drawings::noBrush, ColorRGBA::red);
CIRCLE("representation:RealisticBallPercepts:field", ball.relativePositionOnField.x(), ball.relativePositionOnField.y(), ball.radiusOnField, 0, Drawings::solidPen, ColorRGBA::red, Drawings::solidBrush, ColorRGBA::red);
}
}
void TeamMarkerPerceptor::update(TeamMarkerSpots& teamMarkerSpots)
{
DECLARE_DEBUG_DRAWING("module:TeamMarkerPerceptor:grid", "drawingOnImage");
DECLARE_DEBUG_DRAWING("module:TeamMarkerPerceptor:polygon", "drawingOnImage");
DECLARE_DEBUG_DRAWING("module:TeamMarkerPerceptor:rejected", "drawingOnImage");
DECLARE_DEBUG_DRAWING("module:TeamMarkerPerceptor:ellipse", "drawingOnImage");
DECLARE_DEBUG_DRAWING("module:TeamMarkerPerceptor:bb", "drawingOnImage");
MODIFY("parameters:TeamMarkerPerceptor", params);
teamMarkerSpots.teamMarkers.clear();
extractTeamMarkers(teamMarkerSpots);
}
void GlobalFieldCoverageProvider::update(GlobalFieldCoverage& globalFieldCoverage)
{
DECLARE_DEBUG_DRAWING("module:GlobalFieldCoverageProvider:dropInPosition", "drawingOnField");
DECLARE_DEBUG_DRAWING("module:GlobalFieldCoverageProvider:ballOutPosition", "drawingOnField");
if(!initDone)
init(globalFieldCoverage);
if(theCognitionStateChanges.lastGameState != STATE_SET && theGameInfo.state == STATE_SET)
{
const int min = -static_cast<int>(Vector2f(theFieldDimensions.xPosOpponentGroundline, theFieldDimensions.yPosLeftSideline).squaredNorm()) / 1000;
for(size_t i = 0; i < globalFieldCoverage.grid.size(); ++i)
{
globalFieldCoverage.grid[i].timestamp = theFrameInfo.time;
globalFieldCoverage.grid[i].coverage = min + static_cast<int>(globalFieldCoverage.grid[i].positionOnField.squaredNorm()) / 1000;
}
}
auto addLine = [&](const FieldCoverage::GridLine& line)
{
const size_t base = line.y * line.timestamps.size();
for(size_t x = 0; x < line.timestamps.size(); ++x)
{
if(line.timestamps[x] > globalFieldCoverage.grid[base + x].timestamp)
{
globalFieldCoverage.grid[base + x].timestamp = line.timestamps[x];
globalFieldCoverage.grid[base + x].coverage = line.timestamps[x];
}
}
};
for(size_t y = 0; y < theFieldCoverage.lines.size(); ++y)
addLine(theFieldCoverage.lines[y]);
for(const auto teammate : theTeamData.teammates)
if(!teammate.theFieldCoverage.lines.empty() && teammate.mateType == Teammate::TeamOrigin::BHumanRobot)
addLine(teammate.theFieldCoverage.lines.back());
if(theTeamBallModel.isValid)
setCoverageAtFieldPosition(globalFieldCoverage, theTeamBallModel.position, 0);
if(theFrameInfo.getTimeSince(theBallModel.timeWhenLastSeen) < 4000 && theBallModel.timeWhenDisappeared == theFrameInfo.time)
setCoverageAtFieldPosition(globalFieldCoverage, theRobotPose * theBallModel.estimate.position, 0);
setCoverageAtFieldPosition(globalFieldCoverage, theRobotPose.translation, theFrameInfo.time);
accountForBallDropIn(globalFieldCoverage);
globalFieldCoverage.meanCoverage = 0;
for(size_t i = 0; i < globalFieldCoverage.grid.size(); ++i)
globalFieldCoverage.meanCoverage += globalFieldCoverage.grid[i].coverage;
globalFieldCoverage.meanCoverage /= static_cast<int>(globalFieldCoverage.grid.size());
}
void PenaltyMarkPerceptor::update(PenaltyMarkPercept& penaltyPoint)
{
//scale parameters according to resolution
const float scale = theCameraInfo.width / 320.0f;
scaledToleratedSizeDeviation = int(toleratedSizeDeviation * scale);
scaledWhiteSpacing = int(whiteSpacing * scale);
DECLARE_DEBUG_DRAWING("module:PenaltyMarkPerceptor:penaltyPointScanLines", "drawingOnImage");
DECLARE_DEBUG_DRAWING("module:PenaltyMarkPerceptor:noiseRect", "drawingOnImage");
//searchScanLines(penaltyPoint);
searchPotentialLineSpots(penaltyPoint);
}
void PenaltyMarkPercept::draw() const
{
DECLARE_DEBUG_DRAWING("representation:PenaltyMarkPercept:image", "drawingOnImage");
DECLARE_DEBUG_DRAWING("representation:PenaltyMarkPercept:field", "drawingOnField");
if(Blackboard::getInstance().exists("FrameInfo"))
{
const FrameInfo& frameInfo = static_cast<const FrameInfo&>(Blackboard::getInstance()["FrameInfo"]);
if(timeLastSeen == frameInfo.time)
{
CROSS("representation:PenaltyMarkPercept:image", position.x(), position.y(), 5, 5, Drawings::solidPen, ColorRGBA::blue);
CROSS("representation:PenaltyMarkPercept:field", positionOnField.x(), positionOnField.y(), 40, 40, Drawings::solidPen, ColorRGBA::blue);
}
}
}
void BallLocator::update(BallModel& ballModel)
{
DECLARE_DEBUG_DRAWING("module:BallLocator:Field", "drawingOnField");
DECLARE_DEBUG_DRAWING("module:BallLocator:Image", "drawingOnImage");
#ifdef TARGET_SIM
if(theFrameInfo.time <= lastFrameTime)
{
if(theFrameInfo.time < lastFrameTime)
init();
else
return;
}
#endif
// set extra members for seen ball to achieve the following behavior:
// if the ball has been seen in the last lower camera image, it will be ignored in
// the upper image to exclude any clutter in the robot's environment
ballWasSeenInThisFrame = theBallPercept.ballWasSeen;
if(ballWasSeenInThisFrame && theCameraInfo.camera == CameraInfo::upper && ballWasBeenSeenInLastLowerCameraImage)
ballWasSeenInThisFrame = false;
else if(theCameraInfo.camera == CameraInfo::lower)
ballWasBeenSeenInLastLowerCameraImage = ballWasSeenInThisFrame;
// perform prediction step for each filter
motionUpdate(ballModel);
// detect and handle collision with feet
Pose3D leftFoot = theTorsoMatrix;
Pose3D rightFoot = theTorsoMatrix;
leftFoot.conc(theRobotModel.limbs[MassCalibration::footLeft]).translate(footOffset.x, footOffset.y, -theRobotDimensions.heightLeg5Joint);
rightFoot.conc(theRobotModel.limbs[MassCalibration::footRight]).translate(footOffset.x, -footOffset.y, -theRobotDimensions.heightLeg5Joint);
Vector2<> leftFootCenter(leftFoot.translation.x, leftFoot.translation.y);
Vector2<> rightFootCenter(rightFoot.translation.x, rightFoot.translation.y);
COMPLEX_DRAWING("module:BallLocator:Image",
{
Vector2<int> leftCenterImage, leftTopImage, rightCenterImage, rightTopImage;
if(Geometry::calculatePointInImage(Vector3<>(leftFootCenter.x, leftFootCenter.y, 0), theCameraMatrix, theCameraInfo, leftCenterImage) &&
Geometry::calculatePointInImage(Vector3<>(leftFootCenter.x + footRadius, leftFootCenter.y, 0), theCameraMatrix, theCameraInfo, leftTopImage))
{
CIRCLE("module:BallLocator:Image", leftCenterImage.x, leftCenterImage.y, (leftCenterImage - leftTopImage).abs(), 0, Drawings::ps_solid, ColorRGBA(0, 0, 0), Drawings::bs_null, ColorRGBA());
}
if(Geometry::calculatePointInImage(Vector3<>(rightFootCenter.x, rightFootCenter.y, 0), theCameraMatrix, theCameraInfo, rightCenterImage) &&
Geometry::calculatePointInImage(Vector3<>(rightFootCenter.x + footRadius, rightFootCenter.y, 0), theCameraMatrix, theCameraInfo, rightTopImage))
{
CIRCLE("module:BallLocator:Image", rightCenterImage.x, rightCenterImage.y, (rightCenterImage - rightTopImage).abs(), 0, Drawings::ps_solid, ColorRGBA(0, 0, 0), Drawings::bs_null, ColorRGBA());
}
});
void TeamData::draw() const
{
DECLARE_DEBUG_DRAWING("representation:TeamData", "drawingOnField");
for(auto const& teammate : teammates)
{
ColorRGBA posCol;
if(teammate.status == Teammate::PLAYING)
posCol = ColorRGBA::green;
else if(teammate.status == Teammate::FALLEN)
posCol = ColorRGBA::yellow;
else
posCol = ColorRGBA::red;
const Vector2f& rPos = teammate.theRobotPose.translation;
const float radius = std::max(50.f, teammate.theRobotPose.deviation);
Vector2f dirPos = teammate.theRobotPose * Vector2f(radius, 0.f);
// Circle around Player
CIRCLE("representation:TeamData", rPos.x(), rPos.y(), radius, 20, Drawings::solidPen,
posCol, Drawings::noBrush, ColorRGBA::white);
// Direction of the Robot
LINE("representation:TeamData", rPos.x(), rPos.y(), dirPos.x(), dirPos.y(), 20,
Drawings::solidPen, posCol);
// Player number
DRAWTEXT("representation:TeamData", rPos.x() + 100, rPos.y(), 100, ColorRGBA::black, teammate.number);
// Ball position
const Vector2f ballPos = teammate.theRobotPose * teammate.theBallModel.estimate.position;
CIRCLE("representation:TeamData", ballPos.x(), ballPos.y(), 50, 20, Drawings::solidPen, ColorRGBA::yellow, Drawings::solidBrush, ColorRGBA::yellow);
}
}
void MotionRequest::draw() const
{
DECLARE_DEBUG_DRAWING("representation:MotionRequest", "drawingOnField"); // drawing of a request walk vector
if(motion == walk)
{
switch(walkRequest.mode)
{
case WalkRequest::targetMode:
{
LINE("representation:MotionRequest", 0, 0, walkRequest.target.translation.x, walkRequest.target.translation.y, 0, Drawings::ps_solid, ColorRGBA(0xcd, 0, 0));
CROSS("representation:MotionRequest", walkRequest.target.translation.x, walkRequest.target.translation.y, 50, 0, Drawings::ps_solid, ColorRGBA(0xcd, 0, 0));
Vector2<> rotation(500.f, 0.f);
rotation.rotate(walkRequest.target.rotation);
ARROW("representation:MotionRequest", walkRequest.target.translation.x, walkRequest.target.translation.y, walkRequest.target.translation.x + rotation.x, walkRequest.target.translation.y + rotation.y, 0, Drawings::ps_solid, ColorRGBA(0xcd, 0, 0, 127));
}
break;
case WalkRequest::speedMode:
case WalkRequest::percentageSpeedMode:
{
Vector2<> translation = walkRequest.mode == WalkRequest::speedMode ? walkRequest.speed.translation * 10.f : walkRequest.speed.translation * 1000.f;
ARROW("representation:MotionRequest", 0, 0, translation.x, translation.y, 0, Drawings::ps_solid, ColorRGBA(0xcd, 0, 0));
if(walkRequest.target.rotation != 0.0f)
{
translation.x = translation.abs();
translation.y = 0;
translation.rotate(walkRequest.speed.rotation);
ARROW("representation:MotionRequest", 0, 0, translation.x, translation.y, 0, Drawings::ps_solid, ColorRGBA(0xcd, 0, 0, 127));
}
}
break;
default:
break;
}
}
}
void BSWalkTo::update()
{
#ifndef TARGET_TOOL
DECLARE_DEBUG_DRAWING("module:BH2011BehaviorControl:BSWalkTo:Field", "drawingOnField");
MODIFY("module:BH2011BehaviorControl:BSWalkTo:parameters", p);
#endif
}
void FieldModel::draw()
{
DECLARE_DEBUG_DRAWING("module:SelfLocator:fieldModel", "drawingOnField"); // Draws the closest points tables
int index = 0;
MODIFY("module:SelfLocator:fieldModel", index);
COMPLEX_DRAWING("module:SelfLocator:fieldModel",
if(index < 4)
linePointsTables[index >> 1][index & 1].draw();
else if(index < 8)
void BodyContour::draw() const
{
DECLARE_DEBUG_DRAWING("representation:BodyContour", "drawingOnImage");
DECLARE_DEBUG_DRAWING("representation:BodyContour:maxY", "drawingOnImage");
COMPLEX_DRAWING("representation:BodyContour",
{
for(std::vector<Line>::const_iterator i = lines.begin(); i != lines.end(); ++i)
LINE("representation:BodyContour", i->p1.x, i->p1.y, i->p2.x, i->p2.y, 1,
Drawings::ps_solid, ColorRGBA(255, 0, 255));
for(int x = 0; x < cameraResolution.x; x += 10)
{
int y = cameraResolution.y;
clipBottom(x, y);
LINE("representation:BodyContour", x, y, x, cameraResolution.y, 1,
Drawings::ps_solid, ColorRGBA(255, 0, 255));
}
});
void BallPercept::draw() const
{
DECLARE_DEBUG_DRAWING("representation:BallPercept:image", "drawingOnImage");
DECLARE_DEBUG_DRAWING("representation:BallPercept:field", "drawingOnField");
DECLARE_DEBUG_DRAWING3D("representation:BallPercept", "robot");
TRANSLATE3D("representation:BallPercept", 0, 0, -230);
if(status == seen)
{
CIRCLE("representation:BallPercept:image", positionInImage.x(), positionInImage.y(), radiusInImage, 1, // pen width
Drawings::solidPen, ColorRGBA::black, Drawings::solidBrush, ColorRGBA(255, 128, 64, 100));
CIRCLE("representation:BallPercept:field", positionOnField.x(), positionOnField.y(), radiusOnField, 0, // pen width
Drawings::solidPen, ColorRGBA::orange, Drawings::noBrush, ColorRGBA::orange);
SPHERE3D("representation:BallPercept", positionOnField.x(), positionOnField.y(), radiusOnField, radiusOnField, ColorRGBA::orange);
}
else if(status == guessed)
CIRCLE("representation:BallPercept:image", positionInImage.x(), positionInImage.y(), radiusInImage, 1, // pen width
Drawings::solidPen, ColorRGBA::black, Drawings::solidBrush, ColorRGBA(64, 128, 255, 90));
}
void GoalPercept::draw() const
{
const ColorRGBA goalPerceptColor = ColorRGBA::yellow;
DECLARE_DEBUG_DRAWING("representation:GoalPercept:Image", "drawingOnImage");
DECLARE_DEBUG_DRAWING("representation:GoalPercept:Field", "drawingOnField");
DECLARE_DEBUG_DRAWING3D("representation:GoalPercept", "robot");
TRANSLATE3D("representation:GoalPercept", 0, 0, -230);
ColorRGBA color = ColorRGBA(220, 220, 220);
for(unsigned int i = 0; i < goalPosts.size(); ++i)
{
const GoalPost& p = goalPosts.at(i);
if(p.position != GoalPost::IS_UNKNOWN)
{
CIRCLE("representation:GoalPercept:Field", p.positionOnField.x(), p.positionOnField.y(),
50, 0, Drawings::solidPen, goalPerceptColor, Drawings::solidBrush, color);
LINE("representation:GoalPercept:Field", p.positionOnField.x(), p.positionOnField.y(),
p.positionOnField.x(), p.positionOnField.y() +
(p.position == GoalPost::IS_LEFT ? -700 : 700),
60, Drawings::solidPen, goalPerceptColor);
MID_DOT("representation:GoalPercept:Image", p.positionInImage.x(), p.positionInImage.y(),
goalPerceptColor, color);
LINE("representation:GoalPercept:Image", p.positionInImage.x(), p.positionInImage.y(), p.positionInImage.x(), 0,
5, Drawings::solidPen, goalPerceptColor);
if(p.position == GoalPost::IS_LEFT)
DRAWTEXT("representation:GoalPercept:Image", p.positionInImage.x() + 10, 40, 36, goalPerceptColor, "L");
else if(p.position == GoalPost::IS_RIGHT)
DRAWTEXT("representation:GoalPercept:Image", p.positionInImage.x() - 30, 40, 36, goalPerceptColor, "R");
}
else
{
CIRCLE("representation:GoalPercept:Field", p.positionOnField.x(), p.positionOnField.y(),
50, 0, Drawings::solidPen, ColorRGBA(255, 0, 0), Drawings::solidBrush, goalPerceptColor);
MID_DOT("representation:GoalPercept:Image", p.positionInImage.x(), p.positionInImage.y(),
ColorRGBA(255, 0, 0), goalPerceptColor);
LINE("representation:GoalPercept:Image", p.positionInImage.x(), p.positionInImage.y(), p.positionInImage.x(), 0,
5, Drawings::dottedPen, goalPerceptColor);
}
// Sorry, no access to field dimensions here, so the dimensions are hard coded
CYLINDER3D("representation:GoalPercept", p.positionOnField.x(), p.positionOnField.y(), 400, 0, 0, 0, 50, 800, goalPerceptColor);
LINE3D("representation:GoalPercept", 0, 0, 1.f, p.positionOnField.x(), p.positionOnField.y(), 1.f, 2.f, goalPerceptColor);
}
}
void CameraMatrixProvider::update(CameraMatrix& cameraMatrix)
{
cameraMatrix.computeCameraMatrix(theTorsoMatrix, theRobotCameraMatrix, theCameraCalibration);
cameraMatrix.isValid = theTorsoMatrix.isValid && theMotionInfo.isMotionStable &&
theFallDownState.state == FallDownState::upright &&
theFrameInfo.getTimeSince(theFilteredJointData.timeStamp) < 500 &&
theRobotInfo.penalty == PENALTY_NONE;
DECLARE_DEBUG_DRAWING("module:CameraMatrixProvider:calibrationHelper", "drawingOnImage");
COMPLEX_DRAWING("module:CameraMatrixProvider:calibrationHelper", drawFieldLines(cameraMatrix););
void PointExplorer::initFrame(const Image* image, const ColorReference* colRef, int exploreStepSize, int gridStepSize, int skipOffset, int* minSegLength)
{
theImage = image;
theColRef = colRef;
parameters.exploreStepSize = exploreStepSize;
parameters.gridStepSize = gridStepSize;
parameters.skipOffset = skipOffset;
parameters.minSegSize = minSegLength;
DECLARE_DEBUG_DRAWING("module:PointExplorer:runs", "drawingOnImage");
}
void USObstacleGrid::draw()
{
DECLARE_DEBUG_DRAWING("representation:USObstacleGrid", "drawingOnField");
DECLARE_DEBUG_DRAWING("origin:USObstacleGrid", "drawingOnField");
ORIGIN("origin:USObstacleGrid", drawingOrigin.translation.x,
drawingOrigin.translation.y, drawingOrigin.rotation);
COMPLEX_DRAWING("representation:USObstacleGrid",
{
unsigned char colorOccupiedStep(255 / cellMaxOccupancy);
ColorRGBA baseColor(200, 200, 255, 128);
unsigned char cellsForDrawing[GRID_SIZE];
for(int i = 0; i < GRID_SIZE; ++i)
cellsForDrawing[i] = colorOccupiedStep* cells[i].state;
GRID_MONO("representation:USObstacleGrid", CELL_SIZE, GRID_LENGTH, GRID_LENGTH, baseColor, cellsForDrawing);
const int gridWidth(GRID_LENGTH* CELL_SIZE);
const int gridHeight(GRID_LENGTH* CELL_SIZE);
RECTANGLE("representation:USObstacleGrid", -gridWidth / 2, -gridHeight / 2, gridWidth / 2, gridHeight / 2,
20, Drawings::ps_solid, ColorRGBA(0, 0, 100));
});
void RobotsModel::draw() const
{
DECLARE_DEBUG_DRAWING("representation:RobotsModel:robots", "drawingOnField");
DECLARE_DEBUG_DRAWING("representation:RobotsModel:covariance", "drawingOnField");
COMPLEX_DRAWING("representation:RobotsModel:covariance",
{
for(RCIt r(robots.begin()); r != robots.end(); r++)
{
float s1 = 0.0, s2 = 0.0, theta = 0.0;
Covariance::errorEllipse(r->covariance, s1, s2, theta);
ELLIPSE("representation:RobotsModel:covariance", r->relPosOnField, sqrt(3.0f)*s1, sqrt(3.0f)*s2, theta,
10, Drawings::ps_solid, ColorRGBA(100, 100, 255, 100), Drawings::bs_solid, ColorRGBA(100, 100, 255, 100));
ELLIPSE("representation:RobotsModel:covariance", r->relPosOnField, sqrt(2.0f)*s1, sqrt(2.0f)*s2, theta,
10, Drawings::ps_solid, ColorRGBA(150, 150, 100, 100), Drawings::bs_solid, ColorRGBA(150, 150, 100, 100));
ELLIPSE("representation:RobotsModel:covariance", r->relPosOnField, s1, s2, theta,
10, Drawings::ps_solid, ColorRGBA(255, 100, 100, 100), Drawings::bs_solid, ColorRGBA(255, 100, 100, 100));
}
});
void CoordinateSystemProvider::update(ImageCoordinateSystem& imageCoordinateSystem)
{
Geometry::Line horizon = Geometry::calculateHorizon(theCameraMatrix, theCameraInfo);
imageCoordinateSystem.origin = horizon.base;
imageCoordinateSystem.rotation.c[0] = horizon.direction;
imageCoordinateSystem.rotation.c[1] = Vector2<double>(-horizon.direction.y, horizon.direction.x);
imageCoordinateSystem.invRotation = imageCoordinateSystem.rotation.transpose();
RotationMatrix r(theCameraMatrix.rotation.transpose() * prevCameraMatrix.rotation);
imageCoordinateSystem.offset = Vector2<double>(r.getZAngle(), r.getYAngle());
calcScaleFactors(imageCoordinateSystem.a, imageCoordinateSystem.b);
imageCoordinateSystem.offsetInt = Vector2<int>(int(imageCoordinateSystem.offset.x * 1024 + 0.5),
int(imageCoordinateSystem.offset.y * 1024 + 0.5));
imageCoordinateSystem.aInt = int(imageCoordinateSystem.a * 1024 + 0.5);
imageCoordinateSystem.bInt = int(imageCoordinateSystem.b * 1024 + 0.5);
imageCoordinateSystem.cameraInfo = theCameraInfo;
prevCameraMatrix = theCameraMatrix;
prevTime = theFilteredJointData.timeStamp;
DECLARE_DEBUG_DRAWING("horizon", "drawingOnImage"); // displays the horizon
ARROW("horizon",
imageCoordinateSystem.origin.x,
imageCoordinateSystem.origin.y,
imageCoordinateSystem.origin.x + imageCoordinateSystem.rotation.c[0].x * 50,
imageCoordinateSystem.origin.y + imageCoordinateSystem.rotation.c[0].y * 50,
1, Drawings::ps_solid, ColorRGBA(255,0,0));
ARROW("horizon",
imageCoordinateSystem.origin.x,
imageCoordinateSystem.origin.y,
imageCoordinateSystem.origin.x + imageCoordinateSystem.rotation.c[1].x * 50,
imageCoordinateSystem.origin.y + imageCoordinateSystem.rotation.c[1].y * 50,
1, Drawings::ps_solid, ColorRGBA(255,0,0));
GENERATE_DEBUG_IMAGE(corrected,
INIT_DEBUG_IMAGE_BLACK(corrected);
int yDest = -imageCoordinateSystem.toCorrectedCenteredNeg(0, 0).y;
for(int ySrc = 0; ySrc < theImage.cameraInfo.resolutionHeight; ++ySrc)
for(int yDest2 = -imageCoordinateSystem.toCorrectedCenteredNeg(0, ySrc).y; yDest <= yDest2; ++yDest)
{
int xDest = -imageCoordinateSystem.toCorrectedCenteredNeg(0, ySrc).x;
for(int xSrc = 0; xSrc < theImage.cameraInfo.resolutionWidth; ++xSrc)
{
for(int xDest2 = -imageCoordinateSystem.toCorrectedCenteredNeg(xSrc, ySrc).x; xDest <= xDest2; ++xDest)
{
DEBUG_IMAGE_SET_PIXEL_YUV(corrected, xDest + int(theCameraInfo.opticalCenter.x + 0.5),
yDest + int(theCameraInfo.opticalCenter.y + 0.5),
theImage.image[ySrc][xSrc].y,
theImage.image[ySrc][xSrc].cb,
theImage.image[ySrc][xSrc].cr);
}
}
}
SEND_DEBUG_IMAGE(corrected);
);
void BH2011GoalSymbols::update()
{
timeSinceOppGoalWasSeen = (float) frameInfo.getTimeSince(goalPercept.timeWhenOppGoalLastSeen);
timeSinceOwnGoalWasSeen = (float) frameInfo.getTimeSince(goalPercept.timeWhenOwnGoalLastSeen);
timeSinceAnyGoalWasSeen = timeSinceOppGoalWasSeen < timeSinceOwnGoalWasSeen ? timeSinceOppGoalWasSeen : timeSinceOwnGoalWasSeen;
oppGoalWasSeen = timeSinceOppGoalWasSeen < 500;
goalWasSeen = timeSinceAnyGoalWasSeen < 500;
/* Calculate seen goals in ready state*/
if(gameInfo.state == STATE_READY)
{
if(frameInfo.getTimeSince(goalPercept.timeWhenOppGoalLastSeen) == 0 ||
frameInfo.getTimeSince(goalPercept.timeWhenOwnGoalLastSeen) == 0)
goalsSeenInReadyState += 1.0;
}
else
{
goalsSeenInReadyState = 0;
}
/* Calculate seen goals since last pickup */
if((groundContactState.noContactSafe && damageConfiguration.useGroundContactDetectionForSafeStates)
&& (fallDownState.state == FallDownState::upright || fallDownState.state == FallDownState::staggering))
{
goalsSeenSinceLastPickup = 0.0;
}
else
{
if(frameInfo.getTimeSince(goalPercept.timeWhenOppGoalLastSeen) == 0 ||
frameInfo.getTimeSince(goalPercept.timeWhenOwnGoalLastSeen) == 0)
goalsSeenSinceLastPickup += 1.0;
}
PLOT("Behavior:goalsSeenSinceLastPickup", goalsSeenSinceLastPickup);
/* determine angles to sides of free part of opponent goal */
Vector2<> centerOfOppGoalAbs = Vector2<>((float) fieldDimensions.xPosOpponentGoalpost, (float)(fieldDimensions.yPosLeftGoal + fieldDimensions.yPosRightGoal) / 2.0f);
Vector2<> centerOfOppGoalRel = Geometry::fieldCoord2Relative(robotPose, centerOfOppGoalAbs);
float leftDistance = abs(centerOfOppGoalAbs.y - freePartOfOpponentGoalModel.leftEnd.y);
float rightDistance = abs(centerOfOppGoalAbs.y - freePartOfOpponentGoalModel.rightEnd.y);
if(leftDistance < rightDistance)
{
innerSide = freePartOfOpponentGoalModel.leftEnd;
outerSide = freePartOfOpponentGoalModel.rightEnd;
}
else
{
innerSide = freePartOfOpponentGoalModel.rightEnd;
outerSide = freePartOfOpponentGoalModel.leftEnd;
}
/* draw angles to free part into worldstate, see draw()-function below */
DECLARE_DEBUG_DRAWING("behavior:GoalSymbols:FreePartAnglesOnField", "drawingOnField");
COMPLEX_DRAWING("behavior:GoalSymbols:FreePartAnglesOnField", drawingOnField());
}
void ObstacleWheelBH::draw() const
{
DECLARE_DEBUG_DRAWING("representation:ObstacleWheelBH:wheel", "drawingOnField");
for(const Cone& cone : cones)
{
drawCone(cone);
}
CIRCLE("representation:ObstacleWheelBH:wheel", 0, 0, wheelRadius, 1,
Drawings::ps_solid, ColorClasses::black, Drawings::bs_null, ColorClasses::black);
}
void TeamData::draw() const
{
DECLARE_DEBUG_DRAWING("representation:TeamData", "drawingOnField");
for(auto const& teammate : teammates)
{
ColorRGBA posCol;
if(teammate.status == Teammate::PLAYING)
posCol = ColorRGBA::green;
else if(teammate.status == Teammate::FALLEN)
posCol = ColorRGBA::yellow;
else
posCol = ColorRGBA::red;
const Vector2f& rPos = teammate.theRobotPose.translation;
const float radius = std::max(50.f, teammate.theRobotPose.deviation);
Vector2f dirPos = teammate.theRobotPose * Vector2f(radius, 0.f);
// Circle around Player
CIRCLE("representation:TeamData", rPos.x(), rPos.y(), radius, 20, Drawings::solidPen,
posCol, Drawings::noBrush, ColorRGBA::white);
// Direction of the Robot
LINE("representation:TeamData", rPos.x(), rPos.y(), dirPos.x(), dirPos.y(), 20,
Drawings::solidPen, posCol);
// Player number
DRAWTEXT("representation:TeamData", rPos.x() + 100, rPos.y(), 100, ColorRGBA::black, teammate.number);
// Role
DRAWTEXT("representation:TeamData", rPos.x() + 100, rPos.y() - 150, 100,
ColorRGBA::black, Role::getName(teammate.theBehaviorStatus.role));
// Time to reach ball
int ttrb = teammate.theBehaviorStatus.role == Role::striker
? static_cast<int>(teammate.theBehaviorStatus.timeToReachBall.timeWhenReachBallStriker)
: static_cast<int>(teammate.theBehaviorStatus.timeToReachBall.timeWhenReachBall);
if(Blackboard::getInstance().exists("FrameInfo"))
{
const FrameInfo& theFrameInfo = (const FrameInfo&) Blackboard::getInstance()["FrameInfo"];
ttrb = theFrameInfo.getTimeSince(ttrb);
}
DRAWTEXT("representation:TeamData", rPos.x() + 100, rPos.y() - 300, 100, ColorRGBA::black, "TTRB: " << ttrb);
//Line from Robot to WalkTarget
LINE("representation:TeamData", rPos.x(), rPos.y(),
teammate.theSPLStandardBehaviorStatus.walkingTo.x(),
teammate.theSPLStandardBehaviorStatus.walkingTo.y(),
10, Drawings::dashedPen, ColorRGBA::magenta);
// Ball position
const Vector2f ballPos = teammate.theRobotPose * teammate.theBallModel.estimate.position;
CIRCLE("representation:TeamData", ballPos.x(), ballPos.y(), 50, 20, Drawings::solidPen, ColorRGBA::yellow, Drawings::solidBrush, ColorRGBA::yellow);
}
}
void GoalPercept::draw() const
{
DECLARE_DEBUG_DRAWING("representation:GoalPercept:Image", "drawingOnImage");
DECLARE_DEBUG_DRAWING("representation:GoalPercept:Field", "drawingOnField");
DECLARE_DEBUG_DRAWING3D("representation:GoalPercept", "origin");
TRANSLATE3D("representation:GoalPercept", 0, 0, -230);
ColorRGBA color = ColorClasses::yellow;
for(unsigned int i = 0; i < goalPosts.size(); ++i)
{
const GoalPost& p = goalPosts.at(i);
if(p.position != GoalPost::IS_UNKNOWN)
{
CIRCLE("representation:GoalPercept:Field", p.positionOnField.x, p.positionOnField.y,
50, 0, Drawings::ps_solid, ColorClasses::white, Drawings::bs_solid, color);
LINE("representation:GoalPercept:Field", p.positionOnField.x, p.positionOnField.y,
p.positionOnField.x, p.positionOnField.y +
(p.position == GoalPost::IS_LEFT ? -700 : 700),
60, Drawings::ps_solid, color);
MID_DOT("representation:GoalPercept:Image", p.positionInImage.x, p.positionInImage.y,
ColorClasses::white, color);
LINE("representation:GoalPercept:Image", p.positionInImage.x, p.positionInImage.y, p.positionInImage.x, 0,
5, Drawings::ps_solid, color);
// Sorry, no access to field dimensions here, so the dimensions are hard coded
CYLINDER3D("representation:GoalPercept", p.positionOnField.x, p.positionOnField.y, 400, 0, 0, 0, 50, 800, color);
}
else
{
CIRCLE("representation:GoalPercept:Field", p.positionOnField.x, p.positionOnField.y,
50, 0, Drawings::ps_solid, ColorRGBA(255, 0, 0), Drawings::bs_solid, color);
MID_DOT("representation:GoalPercept:Image", p.positionInImage.x, p.positionInImage.y,
ColorRGBA(255, 0, 0), color);
LINE("representation:GoalPercept:Image", p.positionInImage.x, p.positionInImage.y, p.positionInImage.x, 0,
5, Drawings::ps_dot, color);
// Sorry, no access to field dimensions here, so the dimensions are hard coded
CYLINDER3D("representation:GoalPercept", p.positionOnField.x, p.positionOnField.y, 400, 0, 0, 0, 50, 800, color);
}
}
}
void RobotPose::draw(bool teamRed)
{
DECLARE_DEBUG_DRAWING("representation:RobotPose", "drawingOnField");
Vector2<> bodyPoints[4] = {Vector2<>(55, 90),
Vector2<>(-55, 90),
Vector2<>(-55, -90),
Vector2<>(55, -90)
};
for(int i = 0; i < 4; i++)
bodyPoints[i] = *this * bodyPoints[i];
Vector2<> dirVec(200, 0);
dirVec = *this * dirVec;
const ColorRGBA ownTeamColorForDrawing = teamRed ? ColorRGBA(255, 0, 0) : ColorRGBA(0, 0, 255);
LINE("representation:RobotPose", translation.x, translation.y, dirVec.x, dirVec.y,
20, Drawings::ps_solid, ColorClasses::white);
POLYGON("representation:RobotPose", 4, bodyPoints, 20, Drawings::ps_solid,
ownTeamColorForDrawing, Drawings::bs_solid, ColorClasses::white);
CIRCLE("representation:RobotPose", translation.x, translation.y, 42, 0,
Drawings::ps_solid, ownTeamColorForDrawing, Drawings::bs_solid, ownTeamColorForDrawing);
DECLARE_DEBUG_DRAWING("representation:RobotPose:deviation", "drawingOnField");
if(deviation < 100000.f)
DRAWTEXT("representation:RobotPose:deviation", -3000, -2300, 10, ColorRGBA(0xff, 0xff, 0xff), "pose deviation: " << deviation);
else
DRAWTEXT("representation:RobotPose:deviation", -3000, -2300, 10, ColorRGBA(0xff, 0xff, 0xff), "pose deviation: unknown");
DECLARE_DEBUG_DRAWING3D("representation:RobotPose", "field",
{
LINE3D("representation:RobotPose", translation.x, translation.y, 10,
dirVec.x, dirVec.y, 10, 1, ownTeamColorForDrawing);
for(int i = 0; i < 4; ++i)
{
const Vector2<> p1 = bodyPoints[i];
const Vector2<> p2 = bodyPoints[(i + 1) & 3];
LINE3D("representation:RobotPose", p1.x, p1.y, 10,
p2.x, p2.y, 10, 1, ownTeamColorForDrawing);
}
});
void SideConfidenceProvider::draw()
{
DECLARE_DEBUG_DRAWING("module:SideConfidenceProvider:ballAgreements", "drawingOnField");
ColorRGBA agreeColor(100, 255, 100, 240);
ColorRGBA mirrorColor(255, 50, 50, 240);
ColorRGBA unknownColor(200, 200, 200, 240);
for(auto& agreemate : agreemates)
{
const Vector2f& b = agreemate.lastGlobalBallPosition;
const ColorRGBA& c = agreemate.ballCompatibility == Agree ? agreeColor : agreemate.ballCompatibility == Mirror ? mirrorColor : unknownColor;
CIRCLE("module:SideConfidenceProvider:ballAgreements",
b.x(), b.y(), 80, 20, Drawings::solidPen, c, Drawings::solidBrush, c);
}
}
void FieldModel::draw()
{
DECLARE_DEBUG_DRAWING("module:SelfLocator:fieldModel", "drawingOnField"); // Draws the closest points tables
#ifndef RELEASE
int index = 0;
#endif
MODIFY("module:SelfLocator:fieldModel", index);
COMPLEX_DRAWING("module:SelfLocator:fieldModel",
{
if(index < 4)
linePointsTables[index >> 1][index & 1].draw();
else if(index < 8)
tCornersTables[index - 4].draw();
else
lCornersTables[index - 8].draw();
});
void ImageCoordinateSystem::draw() const
{
DECLARE_DEBUG_DRAWING("horizon", "drawingOnImage"); // displays the horizon
ARROW("horizon",
origin.x,
origin.y,
origin.x + rotation.c[0].x * 50,
origin.y + rotation.c[0].y * 50,
0, Drawings::ps_solid, ColorRGBA(255, 0, 0));
ARROW("horizon",
origin.x,
origin.y,
origin.x + rotation.c[1].x * 50,
origin.y + rotation.c[1].y * 50,
0, Drawings::ps_solid, ColorRGBA(255, 0, 0));
}
void CNSImageProvider::update(CNSImage& cnsImage)
{
DECLARE_DEBUG_DRAWING("module:CNSImageProvider:expectedRadius", "drawingOnImage");
cnsImage.setResolution(theECImage.grayscaled.width, theECImage.grayscaled.height);
if(fullImage)
cnsResponse(theECImage.grayscaled[0], theECImage.grayscaled.width,
theECImage.grayscaled.height, theECImage.grayscaled.width,
reinterpret_cast<short*>(cnsImage[0]), sqr(minContrast));
else
for(const Boundaryi& region : theCNSRegions.regions)
cnsResponse(&theECImage.grayscaled[region.y.min][region.x.min], region.x.getSize(),
region.y.getSize(), theECImage.grayscaled.width,
reinterpret_cast<short*>(&cnsImage[region.y.min][region.x.min]), sqr(minContrast));
}
void CognitionLogDataProvider::update(ImageCoordinateSystem& imageCoordinateSystem)
{
imageCoordinateSystem.setCameraInfo(theCameraInfo);
DECLARE_DEBUG_DRAWING("loggedHorizon", "drawingOnImage"); // displays the horizon
ARROW("loggedHorizon",
imageCoordinateSystem.origin.x(),
imageCoordinateSystem.origin.y(),
imageCoordinateSystem.origin.x() + imageCoordinateSystem.rotation(0, 0) * 50,
imageCoordinateSystem.origin.y() + imageCoordinateSystem.rotation(1, 0) * 50,
0, Drawings::solidPen, ColorRGBA(255, 0, 0));
ARROW("loggedHorizon",
imageCoordinateSystem.origin.x(),
imageCoordinateSystem.origin.y(),
imageCoordinateSystem.origin.x() + imageCoordinateSystem.rotation(0, 1) * 50,
imageCoordinateSystem.origin.y() + imageCoordinateSystem.rotation(1, 1) * 50,
0, Drawings::solidPen, ColorRGBA(255, 0, 0));
COMPLEX_IMAGE(corrected)
{
if(Blackboard::getInstance().exists("Image"))
{
const Image& image = (const Image&) Blackboard::getInstance()["Image"];
INIT_DEBUG_IMAGE_BLACK(corrected, theCameraInfo.width, theCameraInfo.height);
int yDest = -imageCoordinateSystem.toCorrectedCenteredNeg(0, 0).y();
for(int ySrc = 0; ySrc < theCameraInfo.height; ++ySrc)
for(int yDest2 = -imageCoordinateSystem.toCorrectedCenteredNeg(0, ySrc).y(); yDest <= yDest2; ++yDest)
{
int xDest = -imageCoordinateSystem.toCorrectedCenteredNeg(0, ySrc).x();
for(int xSrc = 0; xSrc < theCameraInfo.width; ++xSrc)
{
for(int xDest2 = -imageCoordinateSystem.toCorrectedCenteredNeg(xSrc, ySrc).x(); xDest <= xDest2; ++xDest)
{
DEBUG_IMAGE_SET_PIXEL_YUV(corrected, xDest + int(theCameraInfo.opticalCenter.x() + 0.5f),
yDest + int(theCameraInfo.opticalCenter.y() + 0.5f),
image[ySrc][xSrc].y,
image[ySrc][xSrc].cb,
image[ySrc][xSrc].cr);
}
}
}
SEND_DEBUG_IMAGE(corrected);
}
}
}
