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