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); } } }