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 PenaltyMarkPerceptor::searchPotentialLineSpots(PenaltyMarkPercept& penaltyPoint) { PenaltyMarkPercept pp; std::vector<Vector2i> spotsInLines; for(const LineSpots::Line& line : theLineSpots.lines) { spotsInLines.insert(spotsInLines.end(), line.spotsInImg.begin(), line.spotsInImg.end()); } for(const PotentialScanlineSpot& spot : thePotentialLineSpots.spots) { //todo use set with lexicographic ordering of elements if(std::find(spotsInLines.begin(), spotsInLines.end(), spot.spotInImg) == spotsInLines.end()) { pp.position = spot.spotInImg; pp.positionOnField = spot.spotInField; pp.timeLastSeen = 0;//invalidate if(isPenaltyMark(pp) && isFarAwayFromFieldBoundary(pp) && !isTooFarAway(pp) && !isInsideBall(pp)) { pp.timeLastSeen = theFrameInfo.time; penaltyPoint = pp; return; } } } DRAWTEXT("module:PenaltyMarkPerceptor:penaltyPointScanLines", 50, 50, 7, ColorRGBA::black, "not found"); }
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); } }
bool PenaltyMarkPerceptor::checkIsNoise(unsigned int x, unsigned int y, std::vector<unsigned char>& yValues) { RECTANGLE("module:PenaltyMarkPerceptor:noiseRect", x - noiseAreaSize, y - noiseAreaSize, x + noiseAreaSize, y + noiseAreaSize, 1, Drawings::solidPen, ColorRGBA::yellow); unsigned int numNotGreenInSurrounding = 0; for(int dx = -noiseAreaSize; dx <= noiseAreaSize; dx++) { for(int dy = -noiseAreaSize; dy <= noiseAreaSize; dy++) { unsigned int xpos = x + dx; unsigned int ypos = y + dy; const Image::Pixel& pixel = theImage[ypos][xpos]; yValues.push_back(pixel.y); if(!theColorTable[pixel].is(ColorClasses::green)) { numNotGreenInSurrounding++; DOT("module:PenaltyMarkPerceptor:penaltyPointScanLines", xpos, ypos, ColorRGBA::yellow, ColorRGBA::yellow); } } } if(numNotGreenInSurrounding > numNotGreenMax) { DRAWTEXT("module:PenaltyMarkPerceptor:penaltyPointScanLines", x, y, 7, ColorRGBA::black, "nonoise " + std::to_string(numNotGreenInSurrounding)); } return numNotGreenInSurrounding <= numNotGreenMax; }
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 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 BorderBar::paintEvent(QPaintEvent *) { QRectF rect(2, 2, width()-4, height()-4); QPainter painter(this); painter.setRenderHint(QPainter::Antialiasing, true); // 边缘平滑.. painter.setBrush(backgroundColor()); painter.setPen(Qt::NoPen); painter.drawEllipse(rect); painter.setBrush(Qt::NoBrush); m_pen.setColor(foregroundColor()); m_pen.setWidth(4); painter.setPen(m_pen); painter.drawArc(rect, 0.25 * RADIAN, -percentage() * RADIAN); // +: 逆时针 -: 顺时针 DRAWTEXT(painter, m_pen, m_font); // 绘画文字.. }
void ArmContactModelProvider::update(ArmContactModel& model) { MODIFY("module:ArmContactModelProvider:parameters", p); DECLARE_PLOT("module:ArmContactModelProvider:errorLeftX"); DECLARE_PLOT("module:ArmContactModelProvider:errorRightX"); DECLARE_PLOT("module:ArmContactModelProvider:errorLeftY"); DECLARE_PLOT("module:ArmContactModelProvider:errorRightY"); DECLARE_PLOT("module:ArmContactModelProvider:errorDurationLeft"); DECLARE_PLOT("module:ArmContactModelProvider:errorDurationRight"); DECLARE_PLOT("module:ArmContactModelProvider:errorYThreshold"); DECLARE_PLOT("module:ArmContactModelProvider:errorXThreshold"); DECLARE_PLOT("module:ArmContactModelProvider:contactLeft"); DECLARE_PLOT("module:ArmContactModelProvider:contactRight"); DECLARE_DEBUG_DRAWING("module:ArmContactModelProvider:armContact", "drawingOnField"); CIRCLE("module:ArmContactModelProvider:armContact", 0, 0, 200, 30, Drawings::ps_solid, ColorClasses::blue, Drawings::ps_null, ColorClasses::blue); CIRCLE("module:ArmContactModelProvider:armContact", 0, 0, 400, 30, Drawings::ps_solid, ColorClasses::blue, Drawings::ps_null, ColorClasses::blue); CIRCLE("module:ArmContactModelProvider:armContact", 0, 0, 600, 30, Drawings::ps_solid, ColorClasses::blue, Drawings::ps_null, ColorClasses::blue); CIRCLE("module:ArmContactModelProvider:armContact", 0, 0, 800, 30, Drawings::ps_solid, ColorClasses::blue, Drawings::ps_null, ColorClasses::blue); CIRCLE("module:ArmContactModelProvider:armContact", 0, 0, 1000, 30, Drawings::ps_solid, ColorClasses::blue, Drawings::ps_null, ColorClasses::blue); CIRCLE("module:ArmContactModelProvider:armContact", 0, 0, 1200, 30, Drawings::ps_solid, ColorClasses::blue, Drawings::ps_null, ColorClasses::blue); /* Buffer arm angles */ struct ArmAngles angles; angles.leftX = theJointRequest.angles[JointData::LShoulderPitch]; angles.leftY = theJointRequest.angles[JointData::LShoulderRoll]; angles.rightX = theJointRequest.angles[JointData::RShoulderPitch]; angles.rightY = theJointRequest.angles[JointData::RShoulderRoll]; angleBuffer.add(angles); /* Reset in case of a fall or penalty */ if(theFallDownState.state == FallDownState::onGround || theRobotInfo.penalty != PENALTY_NONE) { leftErrorBuffer.init(); rightErrorBuffer.init(); } Pose2D odometryOffset = theOdometryData - lastOdometry; lastOdometry = theOdometryData; const Vector3<>& leftHandPos3D = theRobotModel.limbs[MassCalibration::foreArmLeft].translation; Vector2<> leftHandPos(leftHandPos3D.x, leftHandPos3D.y); Vector2<> leftHandSpeed = (odometryOffset + Pose2D(leftHandPos) - Pose2D(lastLeftHandPos)).translation / theFrameInfo.cycleTime; float leftFactor = std::max(0.f, 1.f - leftHandSpeed.abs() / p.speedBasedErrorReduction); lastLeftHandPos = leftHandPos; const Vector3<>& rightHandPos3D = theRobotModel.limbs[MassCalibration::foreArmRight].translation; Vector2<> rightHandPos(rightHandPos3D.x, rightHandPos3D.y); Vector2<> rightHandSpeed = (odometryOffset + Pose2D(rightHandPos) - Pose2D(lastRightHandPos)).translation / theFrameInfo.cycleTime; float rightFactor = std::max(0.f, 1.f - rightHandSpeed.abs() / p.speedBasedErrorReduction); lastRightHandPos = rightHandPos; /* Check for arm contact */ // motion types to take into account: stand, walk (if the robot is upright) if((theMotionInfo.motion == MotionInfo::stand || theMotionInfo.motion == MotionInfo::walk) && (theFallDownState.state == FallDownState::upright || theFallDownState.state == FallDownState::staggering) && (theGameInfo.state == STATE_PLAYING || theGameInfo.state == STATE_READY) && (theRobotInfo.penalty == PENALTY_NONE)) // TICKET 897: ArmContact only if robot is not penalized { checkArm(LEFT, leftFactor); checkArm(RIGHT, rightFactor); //left and right are projections of the 3 dimensional shoulder-joint vector //onto the x-y plane. Vector2f left = leftErrorBuffer.getAverageFloat(); Vector2f right = rightErrorBuffer.getAverageFloat(); //Determine if we are being pushed or not bool leftX = fabs(left.x) > fromDegrees(p.errorXThreshold); bool leftY = fabs(left.y) > fromDegrees(p.errorYThreshold); bool rightX = fabs(right.x)> fromDegrees(p.errorXThreshold); bool rightY = fabs(right.y)> fromDegrees(p.errorYThreshold); // update the model model.contactLeft = leftX || leftY; model.contactRight = rightX || rightY; // The duration of the contact is counted upwards as long as the error //remains. Otherwise it is reseted to 0. model.durationLeft = model.contactLeft ? model.durationLeft + 1 : 0; model.durationRight = model.contactRight ? model.durationRight + 1 : 0; model.contactLeft &= model.durationLeft < p.malfunctionThreshold; model.contactRight &= model.durationRight < p.malfunctionThreshold; if(model.contactLeft) { model.timeOfLastContactLeft = theFrameInfo.time; } if(model.contactRight) { model.timeOfLastContactRight = theFrameInfo.time; } model.pushDirectionLeft = getDirection(LEFT, leftX, leftY, left); model.pushDirectionRight = getDirection(RIGHT, rightX, rightY, right); model.lastPushDirectionLeft = model.pushDirectionLeft != ArmContactModel::NONE ? model.pushDirectionLeft : model.lastPushDirectionLeft; model.lastPushDirectionRight = model.pushDirectionRight != ArmContactModel::NONE ? model.pushDirectionRight : model.lastPushDirectionRight; PLOT("module:ArmContactModelProvider:errorLeftX", toDegrees(left.x)); PLOT("module:ArmContactModelProvider:errorRightX", toDegrees(right.x)); PLOT("module:ArmContactModelProvider:errorLeftY", toDegrees(left.y)); PLOT("module:ArmContactModelProvider:errorRightY", toDegrees(right.y)); PLOT("module:ArmContactModelProvider:errorDurationLeft", model.durationLeft); PLOT("module:ArmContactModelProvider:errorDurationRight", model.durationRight); PLOT("module:ArmContactModelProvider:errorYThreshold", toDegrees(p.errorYThreshold)); PLOT("module:ArmContactModelProvider:errorXThreshold", toDegrees(p.errorXThreshold)); PLOT("module:ArmContactModelProvider:contactLeft", model.contactLeft ? 10.0 : 0.0); PLOT("module:ArmContactModelProvider:contactRight", model.contactRight ? 10.0 : 0.0); ARROW("module:ArmContactModelProvider:armContact", 0, 0, -(toDegrees(left.y) * SCALE), toDegrees(left.x) * SCALE, 20, Drawings::ps_solid, ColorClasses::green); ARROW("module:ArmContactModelProvider:armContact", 0, 0, toDegrees(right.y) * SCALE, toDegrees(right.x) * SCALE, 20, Drawings::ps_solid, ColorClasses::red); COMPLEX_DRAWING("module:ArmContactModelProvider:armContact", { DRAWTEXT("module:ArmContactModelProvider:armContact", -2300, 1300, 20, ColorClasses::black, "LEFT"); DRAWTEXT("module:ArmContactModelProvider:armContact", -2300, 1100, 20, ColorClasses::black, "ErrorX: " << toDegrees(left.x)); DRAWTEXT("module:ArmContactModelProvider:armContact", -2300, 900, 20, ColorClasses::black, "ErrorY: " << toDegrees(left.y)); DRAWTEXT("module:ArmContactModelProvider:armContact", -2300, 500, 20, ColorClasses::black, ArmContactModel::getName(model.pushDirectionLeft)); DRAWTEXT("module:ArmContactModelProvider:armContact", -2300, 300, 20, ColorClasses::black, "Time: " << model.timeOfLastContactLeft); DRAWTEXT("module:ArmContactModelProvider:armContact", 1300, 1300, 20, ColorClasses::black, "RIGHT"); DRAWTEXT("module:ArmContactModelProvider:armContact", 1300, 1100, 20, ColorClasses::black, "ErrorX: " << toDegrees(right.x)); DRAWTEXT("module:ArmContactModelProvider:armContact", 1300, 900, 20, ColorClasses::black, "ErrorY: " << toDegrees(right.y)); DRAWTEXT("module:ArmContactModelProvider:armContact", 1300, 500, 20, ColorClasses::black, ArmContactModel::getName(model.pushDirectionRight)); DRAWTEXT("module:ArmContactModelProvider:armContact", 1300, 300, 20, ColorClasses::black, "Time: " << model.timeOfLastContactRight); if (model.contactLeft) { CROSS("module:ArmContactModelProvider:armContact", -2000, 0, 100, 20, Drawings::ps_solid, ColorClasses::red); } if (model.contactRight) { CROSS("module:ArmContactModelProvider:armContact", 2000, 0, 100, 20, Drawings::ps_solid, ColorClasses::red); } });
void TeammateData::draw() const { DECLARE_DEBUG_DRAWING("representation:TeammateData", "drawingOnField"); for(int i = 1; i < numOfPlayers; ++i) { if(timeStamps[i]) { ColorRGBA posCol; if(isFullyActive[i]) { posCol = ColorRGBA(0, 255, 0); } else if(isActive[i]) { posCol = ColorRGBA(255, 255, 0); } else { posCol = ColorRGBA(255, 0, 0); } const Vector2<>& rPos = robotPoses[i].translation; float radius = robotPoses[i].deviation; if(radius < 50.f) { radius = 50.f; } Vector2<> dirPos = robotPoses[i] * Vector2<>(radius, 0); // Circle arround Player CIRCLE("representation:TeammateData", rPos.x, rPos.y, radius, 20, Drawings::ps_solid, posCol, Drawings::bs_null, ColorRGBA::white); // Direction of the Robot LINE("representation:TeammateData", rPos.x, rPos.y, dirPos.x, dirPos.y, 20, Drawings::ps_solid, posCol); // Player number DRAWTEXT("representation:TeammateData", rPos.x + 100, rPos.y, 100, ColorRGBA::black, i); // Role DRAWTEXT("representation:TeammateData", rPos.x + 100, rPos.y - 150, 100, ColorRGBA::black, Role::getName(behaviorStatus[i].role)); // time to reach ball DRAWTEXT("representation:TeammateData", rPos.x + 100, rPos.y - 300, 100, ColorRGBA::black, "" << behaviorStatus[i].estimatedTimeToReachBall); // intention std::string intentionStr; switch(intention[i]) { case DROPIN_INTENTION_DEFAULT: intentionStr = "default"; break; case DROPIN_INTENTION_KEEPER: intentionStr = "keeper"; break; case DROPIN_INTENTION_DEFENSIVE: intentionStr = "defensive"; break; case DROPIN_INTENTION_KICK: intentionStr = "kick"; break; case DROPIN_INTENTION_LOST: intentionStr = "lost"; break; default: intentionStr = "unknown"; } DRAWTEXT("representation:TeammateData", rPos.x + 100, rPos.y - 450, 100, ColorRGBA::black, "Intention " << intentionStr); Vector2<> bPos = robotPoses[i] * ballModels[i].estimate.position; ColorRGBA ballCol; if(currentTimestamp - ballModels[i].timeWhenLastSeen < networkTimeout / 2) { ballCol = ColorRGBA(0, 255, 0); } else if(currentTimestamp - ballModels[i].timeWhenLastSeen < networkTimeout) { ballCol = ColorRGBA(255, 255, 0); } else { ballCol = ColorRGBA(255, 0, 0); } //Line from Robot to Ball LINE("representation:TeammateData", rPos.x, rPos.y, walkingTo[i].x, walkingTo[i].y, 10, Drawings::ps_dash, ColorRGBA::magenta); // Ball position CIRCLE("representation:TeammateData", bPos.x, bPos.y, 50, 20, Drawings::ps_solid, ballCol, Drawings::bs_solid, ballCol); //Line from Robot to Ball LINE("representation:TeammateData", rPos.x, rPos.y, bPos.x, bPos.y, 20, Drawings::ps_dash, ballCol); } } }
bool PenaltyMarkPerceptor::isPenaltyMark(PenaltyMarkPercept& pp) { const int height = theImage.height - 3; int horizon = std::max(2, static_cast<int>(theImageCoordinateSystem.origin.y())); horizon = std::min(horizon, height); Vector2f relativePosition; if(!Transformation::imageToRobot(pp.position.x(), pp.position.y(), theCameraMatrix, theCameraInfo, relativePosition)) return false; int maxWidth = static_cast<int>(Geometry::getSizeByDistance(theCameraInfo, theFieldDimensions.penaltyMarkSize, relativePosition.norm())); //s DRAWTEXT("module:PenaltyMarkPerceptor:penaltyPointScanLines", 60, 60, 7, ColorRGBA::black, std::to_string(maxWidth)); const int leftLimit = 2; const int whiteSkipping = 5; const int rightLimit = theImage.width - 3; int skipped = 0; int lower, upper; int left = 0; int right = 0; left = right = pp.position.x(); skipped = 0; while(left >= leftLimit && skipped < whiteSkipping) { if(std::abs(right - (left + skipped + 1)) > maxWidth * scaledToleratedSizeDeviation) { return false; } if(theColorTable[theImage[pp.position.y()][left]].is(ColorClasses::white)) skipped = 0; else skipped++; left--; } left += skipped + 1; skipped = 0; while(right <= rightLimit && skipped < whiteSkipping) { if(std::abs(left - (right - skipped)) > maxWidth * scaledToleratedSizeDeviation) { return false; } if(theColorTable[theImage[pp.position.y()][right]].is(ColorClasses::white)) skipped = 0; else skipped++; right++; } right -= skipped; pp.position.x() = (left + right) / 2; // find upper/lower => middle vertical lower = upper = pp.position.y(); skipped = 0; while(lower <= height && skipped < whiteSkipping) { if(std::abs(upper - (lower - skipped)) > maxWidth * scaledToleratedSizeDeviation) { return false; } if(theColorTable[theImage[lower][pp.position.x()]].is(ColorClasses::white)) { skipped = 0; } else { skipped++; } lower++; } lower -= skipped; skipped = 0; while(upper >= horizon && skipped < whiteSkipping) { if(std::abs(lower - (upper + skipped + 1)) > maxWidth * scaledToleratedSizeDeviation) { return false; } if(theColorTable[theImage[upper][pp.position.x()]].is(ColorClasses::white)) skipped = 0; else skipped++; upper--; } upper += skipped + 1; pp.position.y() = (lower + upper) / 2; LINE("module:PenaltyMarkPerceptor:penaltyPointScanLines", pp.position.x(), lower, pp.position.x(), upper, 1, Drawings::solidPen, ColorRGBA::blue); // find left/right => middle horizontal LINE("module:PenaltyMarkPerceptor:penaltyPointScanLines", left, pp.position.y(), right, pp.position.y(), 1, Drawings::solidPen, ColorRGBA::blue); const int minBallSpotRadius = maxWidth / scaledToleratedSizeDeviation; bool minRadiusReached = (right - left) >= minBallSpotRadius && (lower - upper) >= minBallSpotRadius && (right - left) >= minNumPixelVertical && (lower - upper) >= minNumPixelHorizontal; if(!minRadiusReached) { DRAWTEXT("module:PenaltyMarkPerceptor:penaltyPointScanLines", pp.position.x(), pp.position.y(), 7, ColorRGBA::black, "minRadius"); } else { //check if region is sourounded by green which is always the case for the //ball spot and not for line segments //go with a rectangle around the region int yUpper = upper - scaledWhiteSpacing; int xRight = right + scaledWhiteSpacing; int yLower = lower + scaledWhiteSpacing; int xLeft = left - scaledWhiteSpacing; if(yUpper <= 2 || yLower <= 2 || xRight <= 2 || xLeft <= 2 || yUpper >= height - 2 || yLower >= height - 2 || xRight >= rightLimit - 2 || xLeft >= rightLimit - 2) return false; std::vector<unsigned char> yValues; for(int i = xLeft; i <= xRight; i++) { if(!checkIsNoise(i, yUpper, yValues)) { return false; } if(!checkIsNoise(i, yLower, yValues)) { return false; } } for(int i = yUpper; i <= yLower; i++) { if(!checkIsNoise(xRight, i, yValues)) { return false; } if(!checkIsNoise(xLeft, i, yValues)) { return false; } } RECTANGLE("module:PenaltyMarkPerceptor:penaltyPointScanLines", xLeft, yUpper, xRight, yLower, 2, Drawings::solidPen, ColorRGBA::red); DRAWTEXT("module:PenaltyMarkPerceptor:penaltyPointScanLines", pp.position.x(), pp.position.y(), 7, ColorRGBA::black, std::to_string(minBallSpotRadius) + ", " + std::to_string(right - left) + ", " + std::to_string(lower - upper)); const float variance = calcVariance(yValues); DRAWTEXT("module:PenaltyMarkPerceptor:penaltyPointScanLines", pp.position.x(), pp.position.y() + 20, 7, ColorRGBA::black, "Variance: " << variance); if(variance >= maxVariance) { ANNOTATION("PenaltyMarkPerceptor", "Ignored Spot (variance: " << variance); } return variance < maxVariance; } return false; }
bool TeamRobot::main() { { SYNC; OUTPUT(idProcessBegin, bin, 't'); DECLARE_DEBUG_DRAWING("representation:RobotPose", "drawingOnField"); // The robot pose DECLARE_DEBUG_DRAWING("representation:RobotPose:deviation", "drawingOnField"); // The robot pose DECLARE_DEBUG_DRAWING("origin:RobotPose", "drawingOnField"); // Set the origin to the robot's current position DECLARE_DEBUG_DRAWING("representation:BallModel", "drawingOnField"); // drawing of the ball model DECLARE_DEBUG_DRAWING("representation:CombinedWorldModel", "drawingOnField"); // drawing of the combined world model DECLARE_DEBUG_DRAWING("representation:GoalPercept:Field", "drawingOnField"); // drawing of the goal percept DECLARE_DEBUG_DRAWING("representation:MotionRequest", "drawingOnField"); // drawing of a request walk vector DECLARE_DEBUG_DRAWING("representation:ObstacleModel:Center", "drawingOnField"); //drawing center of obstacle model DECLARE_DEBUG_DRAWING("representation:LinePercept:Field", "drawingOnField"); uint8_t teamColor = 0, swapSides = 0; MODIFY("teamColor", ownTeamInfo.teamColor); MODIFY("swapSides", swapSides); if(SystemCall::getTimeSince(robotPoseReceived) < 1000) robotPose.draw(); if(SystemCall::getTimeSince(ballModelReceived) < 1000) ballModel.draw(); if(SystemCall::getTimeSince(combinedWorldModelReceived) < 1000) combinedWorldModel.draw(); if(SystemCall::getTimeSince(goalPerceptReceived) < 1000) goalPercept.draw(); if(SystemCall::getTimeSince(motionRequestReceived) < 1000) motionRequest.draw(); if(SystemCall::getTimeSince(obstacleModelReceived) < 1000) obstacleModel.draw(); if(SystemCall::getTimeSince(linePerceptReceived) < 1000) linePercept.draw(); if(swapSides ^ teamColor) { ORIGIN("field polygons", 0, 0, pi2); // hack: swap sides! } fieldDimensions.draw(); fieldDimensions.drawPolygons(teamColor); DECLARE_DEBUG_DRAWING("robotState", "drawingOnField"); // text decribing the state of the robot int lineY = 3550; DRAWTEXT("robotState", -5100, lineY, 150, ColorRGBA::white, "batteryLevel: " << robotHealth.batteryLevel << " %"); DRAWTEXT("robotState", 3700, lineY, 150, ColorRGBA::white, "role: " << Role::getName(behaviorStatus.role)); lineY -= 180; DRAWTEXT("robotState", -5100, lineY, 150, ColorRGBA::white, "temperatures: joint " << JointData::getName(robotHealth.jointWithMaxTemperature)<< ":" << robotHealth.maxJointTemperature << " C, cpu: " << robotHealth.cpuTemperature << " C"); lineY -= 180; DRAWTEXT("robotState", -5100, lineY, 150, ColorRGBA::white, "rates: cognition: " << (int) std::floor(robotHealth.cognitionFrameRate + 0.5f) << " fps, motion: " << (int) std::floor(robotHealth.motionFrameRate + 0.5f) << " fps"); if(ballModel.timeWhenLastSeen) { DRAWTEXT("robotState", 3700, lineY, 150, ColorRGBA::white, "ballLastSeen: " << SystemCall::getRealTimeSince(ballModel.timeWhenLastSeen) << " ms"); } else { DRAWTEXT("robotState", 3700, lineY, 150, ColorRGBA::white, "ballLastSeen: never"); } //DRAWTEXT("robotState", -5100, lineY, 150, ColorRGBA::white, "ballPercept: " << ballModel.lastPerception.position.x << ", " << ballModel.lastPerception.position.y); lineY -= 180; DRAWTEXT("robotState", -5100, lineY, 150, ColorRGBA::white, "memory usage: " << robotHealth.memoryUsage << " %"); if(goalPercept.timeWhenGoalPostLastSeen) { DRAWTEXT("robotState", 3700, lineY, 150, ColorRGBA::white, "goalLastSeen: " << SystemCall::getRealTimeSince(goalPercept.timeWhenGoalPostLastSeen) << " ms"); } else { DRAWTEXT("robotState", 3700, lineY, 150, ColorRGBA::white, "goalLastSeen: never"); } lineY -= 180; DRAWTEXT("robotState", -5100, lineY, 150, ColorRGBA::white, "load average: " << (float(robotHealth.load[0]) / 10.f) << " " << (float(robotHealth.load[1]) / 10.f) << " " << (float(robotHealth.load[2]) / 10.f)); DRAWTEXT("robotState", -4100, 0, 150, ColorRGBA(255, 255, 255, 50), robotHealth.robotName); DECLARE_DEBUG_DRAWING("robotOffline", "drawingOnField"); // A huge X to display the online/offline state of the robot if(SystemCall::getTimeSince(robotPoseReceived) > 500 || (SystemCall::getTimeSince(isPenalizedReceived) < 1000 && isPenalized) || (SystemCall::getTimeSince(hasGroundContactReceived) < 1000 && !hasGroundContact) || (SystemCall::getTimeSince(isUprightReceived) < 1000 && !isUpright)) { LINE("robotOffline", -5100, 3600, 5100, -3600, 50, Drawings::ps_solid, ColorRGBA(0xff, 0, 0)); LINE("robotOffline", 5100, 3600, -5100, -3600, 50, Drawings::ps_solid, ColorRGBA(0xff, 0, 0)); } if(SystemCall::getTimeSince(isPenalizedReceived) < 1000 && isPenalized) { // Draw a text in red letters to tell that the robot is penalized DRAWTEXT("robotState", -2000, 0, 200, ColorRGBA::red, "PENALIZED"); } if(SystemCall::getTimeSince(hasGroundContactReceived) < 1000 && !hasGroundContact) { // Draw a text in red letters to tell that the robot doesn't have ground contact DRAWTEXT("robotState", 300, 0, 200, ColorRGBA::red, "NO GROUND CONTACT"); } if(SystemCall::getTimeSince(isUprightReceived) < 1000 && !isUpright) { // Draw a text in red letters to tell that the robot is fallen down DRAWTEXT("robotState", 300, 0, 200, ColorRGBA::red, "NOT UPRIGHT"); } DEBUG_RESPONSE_ONCE("automated requests:DrawingManager", OUTPUT(idDrawingManager, bin, Global::getDrawingManager());); DEBUG_RESPONSE_ONCE("automated requests:DrawingManager3D", OUTPUT(idDrawingManager3D, bin, Global::getDrawingManager3D()););
void ExpObstacleModel::draw() const { DECLARE_DEBUG_DRAWING("representation:ExpObstacleModel:center", "drawingOnField"); DECLARE_DEBUG_DRAWING("representation:ExpObstacleModel:feetCircle", "drawingOnField"); DECLARE_DEBUG_DRAWING("representation:ExpObstacleModel:shoulderCircle", "drawingOnField"); DECLARE_DEBUG_DRAWING("representation:ExpObstacleModel:covariance", "drawingOnField"); DECLARE_DEBUG_DRAWING("representation:ExpObstacleModel:velocity", "drawingOnField"); DECLARE_DEBUG_DRAWING("representation:ExpObstacleModel:fallen", "drawingOnField"); ColorRGBA color; for(const auto& obstacle : eobs) { switch(obstacle.type) { case ExpObstacle::GOALPOST: { color = ColorRGBA::yellow; CROSS("representation:ExpObstacleModel:center", obstacle.center.x, obstacle.center.y, 50, 10, Drawings::ps_solid, ColorRGBA::yellow); COVARIANCE2D("representation:ExpObstacleModel:covariance", Matrix2x2<>(obstacle.covariance[0][0], obstacle.covariance[1][0], obstacle.covariance[0][1], obstacle.covariance[1][1]), obstacle.center); continue; } case ExpObstacle::FALLENBLUE: case ExpObstacle::ROBOTBLUE: { color = ColorRGBA::blue; break; } case ExpObstacle::FALLENRED: case ExpObstacle::ROBOTRED: { color = ColorRGBA::red; break; } case ExpObstacle::SOMEFALLENROBOT: case ExpObstacle::SOMEROBOT: { color = ColorRGBA::white; break; } default: { color = ColorRGBA::black; break; } } CROSS("representation:ExpObstacleModel:center", obstacle.center.x, obstacle.center.y, 20, 10, Drawings::ps_solid, color); CIRCLE("representation:ExpObstacleModel:feetCircle", obstacle.center.x, obstacle.center.y, feetRadius, 10, Drawings::ps_solid, color, Drawings::bs_solid, color); CIRCLE("representation:ExpObstacleModel:shoulderCircle", obstacle.center.x, obstacle.center.y, shoulderRadius, 10, Drawings::ps_solid, color, Drawings::bs_solid, color); COVARIANCE2D("representation:ExpObstacleModel:covariance", Matrix2x2<>(obstacle.covariance[0][0], obstacle.covariance[1][0], obstacle.covariance[0][1], obstacle.covariance[1][1]), obstacle.center); if(obstacle.velocity.abs() > 0) ARROW("representation:ExpObstacleModel:velocity", obstacle.center.x, obstacle.center.y, obstacle.center.x + (obstacle.velocity.x * 1000.f), obstacle.center.y + (obstacle.velocity.y * 1000.f), 10, Drawings::ps_solid, color); if(obstacle.type == ExpObstacle::SOMEFALLENROBOT || obstacle.type == ExpObstacle::FALLENBLUE || obstacle.type == ExpObstacle::FALLENRED) { DRAWTEXT("representation:ExpObstacleModel:fallen", obstacle.center.x, obstacle.center.y, 100, color, "FALLEN"); } } }