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 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 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 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 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());
}
});
CIRCLE CPlane::GetCircle()
{
double centerX=_x+_cx/2;
double centerY=_y+_cy/2;
double radius=_cx/2;
return CIRCLE(centerX,centerY,radius);
}
// Portão de teleport
void block::GATE() {
e[0] = e[6] = true;
CIRCLE(9, 0);
DOT(LT, 11, 9, 22);
DOT(NR, 11, 0, 23);
DOT(RT, 11, 9, 24);
}
// Super fire item (fogo level 9)
void block::SFIREIT() {
e[0] = e[3] = true;
item = 'F';
CIRCLE(12, 14);
DOT(NR, 14, 0, 11, 15);
DOT(DR, 12, 14, 12, 14);
}
// Personagem, carinha da Hudson Soft
void block::BOMBERBOY(short int color, short int backcolor, char LastMove) {
CIRCLE(color, backcolor);
switch(LastMove) {
case KEY_DOWN: DOT(VL, 0, 6, 22, 24); DOT(NR, 6, 0, 23); break;
case KEY_LEFT: DOT(VL, 0, 6, 22); DOT(NR, 6, 0, 23, 24); break;
case KEY_RIGHT: DOT(NR, 6, 0, 22, 23); DOT(VL, 0, 6, 24);
}
}
void DummyActiveGoalLocator::execute()
{
// reset
getLocalGoalModel().opponentGoalIsValid = false;
getLocalGoalModel().ownGoalIsValid = false;
getLocalGoalModel().someGoalWasSeen = getGoalPercept().getNumberOfSeenPosts() > 0; //getSensingGoalModel().someGoalWasSeen;
// opp goal is in front of me
const GoalModel::Goal& oppGoal = getSelfLocGoalModel().getOppGoal(getCompassDirection(), getFieldInfo());
if(((oppGoal.leftPost+oppGoal.leftPost)*0.5).x > 0)
getLocalGoalModel().opponentGoalIsValid = true;
else
getLocalGoalModel().ownGoalIsValid = true;
// copy the self loc goal
getLocalGoalModel().goal = getSelfLocGoalModel().goal;
//frame Info when goal was seen not useful! New: some_goal_was seen
if(getGoalPercept().getNumberOfSeenPosts() > 0)
{
getLocalGoalModel().goal.frameInfoWhenGoalLastSeen = getFrameInfo();
if(getLocalGoalModel().opponentGoalIsValid)
{
getLocalGoalModel().frameWhenOpponentGoalWasSeen = getFrameInfo();
}
else
{
getLocalGoalModel().frameWhenOwnGoalWasSeen = getFrameInfo();
}
}
DEBUG_REQUEST("DummyActiveGoalLocator:draw_goal_model",
FIELD_DRAWING_CONTEXT;
if(getLocalGoalModel().opponentGoalIsValid)
PEN("000000", 50);
else
PEN("FFFFFF", 50);
CIRCLE(getLocalGoalModel().goal.leftPost.x, getLocalGoalModel().goal.leftPost.y, 50);
CIRCLE(getLocalGoalModel().goal.rightPost.x, getLocalGoalModel().goal.rightPost.y, 50);
LINE(getLocalGoalModel().goal.rightPost.x, getLocalGoalModel().goal.rightPost.y, getLocalGoalModel().goal.leftPost.x, getLocalGoalModel().goal.leftPost.y);
);
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 block::MONSTER1() {
ZERO();
e[0] = e[5] = true;
monster = '1';
CIRCLE(12, 0);
HLINE(UT, 12, 0, 1);
DOT(E2, 0, 12, 22);
DOT(E3, 0, 12, 24);
DOT(UT, 0, 12, 33);
}
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 GridClustering::draw_grid(const FieldInfo& fieldInfo)
{
FIELD_DRAWING_CONTEXT;
PEN("000000", 20);
for (int x = 0; x <= fieldInfo.xLength; x+=CountGrid2D::GRID_STEP)
{
LINE(x - fieldInfo.xPosOpponentGroundline,
0 - fieldInfo.yPosLeftSideline,
x - fieldInfo.xPosOpponentGroundline,
0 + fieldInfo.yPosLeftSideline);
}
for (int y = 0; y <= fieldInfo.yLength; y+=CountGrid2D::GRID_STEP)
{
LINE(0 - fieldInfo.xPosOpponentGroundline,
y - fieldInfo.yPosLeftSideline,
0 + fieldInfo.xPosOpponentGroundline,
y - fieldInfo.yPosLeftSideline);
}
DEBUG_REQUEST("MCSL:draw_best_cells",
for (int j = 0; j < CountGrid2D::GRID_X_PRECISION; j++)
{
for (int i = 0; i < CountGrid2D::GRID_Y_PRECISION; i++)
{
if (fieldGrid.table[j][i] > (int)(sampleSet.numberOfParticles*0.03))
{
FIELD_DRAWING_CONTEXT;
//Draw best grid cells
if (fieldGrid.table[j][i] >= (int)(sampleSet.numberOfParticles*0.09))
{
PEN("FF0000", 20);
}
else if (fieldGrid.table[j][i] > (int)(sampleSet.numberOfParticles*0.06))
{
PEN("FF5555", 20);
}
else if (fieldGrid.table[j][i] > (int)(sampleSet.numberOfParticles*0.03))
{
PEN("FFAAAA", 20);
}
CIRCLE(j*CountGrid2D::GRID_STEP-fieldInfo.xPosOpponentGroundline + CountGrid2D::GRID_STEP/2,
i*CountGrid2D::GRID_STEP-fieldInfo.yPosLeftSideline + CountGrid2D::GRID_STEP/2,
CountGrid2D::GRID_STEP/2);
}//end if
}//end for
}//end for
);
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 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 block::BOSS(short int color) {
ZERO();
e[0] = e[5] = true;
if (color == 12) {
monster = '5';
} else if (color == 4) {
monster = '6';
} else {
monster = '7';
}
CIRCLE(color, 0);
DOT(RT, 12, 0, 22);
DOT(NR, 0, 12, 23);
DOT(LT, 12, 0, 24);
}
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 CameraMatrixBH::draw() const
{
DECLARE_DEBUG_DRAWING("representation:CameraMatrixBH:ImageBH", "drawingOnImage"); // Shows the robot coordinate system
DECLARE_DEBUG_DRAWING("representation:CameraMatrixBH:Field", "drawingOnField"); // Shows the robot coordinate system
COMPLEX_DRAWING("representation:CameraMatrixBH:Field",
{
CameraInfoBH cameraInfo; // HACK!
Vector2BH<int> pointOnField[6];
// calculate the projection of the four image corners to the ground
Geometry::calculatePointOnField(0, 0, *this, cameraInfo, pointOnField[0]);
Geometry::calculatePointOnField(cameraInfo.width, 0, *this, cameraInfo, pointOnField[1]);
Geometry::calculatePointOnField(cameraInfo.width, cameraInfo.height, *this, cameraInfo, pointOnField[2]);
Geometry::calculatePointOnField(0, cameraInfo.height, *this, cameraInfo, pointOnField[3]);
// calculate a line 15 pixels below the horizon in the image
Geometry::Line horizon = Geometry::calculateHorizon(*this, cameraInfo);
Geometry::Line lineBelowHorizon;
Vector2BH<> vertLineDirection(-horizon.direction.y, horizon.direction.x);
lineBelowHorizon.direction = horizon.direction;
lineBelowHorizon.base = horizon.base;
lineBelowHorizon.base += vertLineDirection * 15.0f;
// calculate the projection to the ground of the intersection points of the line parallel to the horizon and the image borders
Vector2BH<int> beginPoint;
Vector2BH<int> endPoint;
if(Geometry::getIntersectionPointsOfLineAndRectangle(
Vector2BH<int>(0, 0), Vector2BH<int>(cameraInfo.width - 1, cameraInfo.height - 1), lineBelowHorizon, beginPoint, endPoint))
{
Geometry::calculatePointOnField(beginPoint.x, beginPoint.y, *this, cameraInfo, pointOnField[4]);
Geometry::calculatePointOnField(endPoint.x, endPoint.y, *this, cameraInfo, pointOnField[5]);
LINE("representation:CameraMatrixBH:Field", pointOnField[4].x, pointOnField[4].y, pointOnField[5].x, pointOnField[5].y, 30, Drawings::ps_solid, ColorClasses::yellow);
}
// determine the boundary of all the points that were projected to the ground
Boundary<int> boundary(-10000, +10000);
if(pointOnField[0].x != 0 || pointOnField[0].y != 0) {
boundary.add(pointOnField[0]);
CIRCLE("representation:CameraMatrixBH:Field", pointOnField[0].x, pointOnField[0].y, 100, 50, Drawings::ps_solid, ColorClasses::white, Drawings::bs_null, ColorClasses::white);
}
if(pointOnField[1].x != 0 || pointOnField[1].y != 0) {
boundary.add(pointOnField[1]);
CIRCLE("representation:CameraMatrixBH:Field", pointOnField[1].x, pointOnField[1].y, 100, 50, Drawings::ps_solid, ColorClasses::white, Drawings::bs_null, ColorClasses::white);
}
if(pointOnField[2].x != 0 || pointOnField[2].y != 0) {
boundary.add(pointOnField[2]);
CIRCLE("representation:CameraMatrixBH:Field", pointOnField[2].x, pointOnField[2].y, 100, 50, Drawings::ps_solid, ColorClasses::white, Drawings::bs_null, ColorClasses::white);
}
if(pointOnField[3].x != 0 || pointOnField[3].y != 0) {
boundary.add(pointOnField[3]);
CIRCLE("representation:CameraMatrixBH:Field", pointOnField[3].x, pointOnField[3].y, 100, 50, Drawings::ps_solid, ColorClasses::white, Drawings::bs_null, ColorClasses::white);
}
if(pointOnField[4].x != 0 || pointOnField[4].y != 0) {
boundary.add(pointOnField[4]);
CIRCLE("representation:CameraMatrixBH:Field", pointOnField[4].x, pointOnField[4].y, 100, 50, Drawings::ps_solid, ColorClasses::yellow, Drawings::bs_null, ColorClasses::yellow);
}
if(pointOnField[5].x != 0 || pointOnField[5].y != 0) {
boundary.add(pointOnField[5]);
CIRCLE("representation:CameraMatrixBH:Field", pointOnField[5].x, pointOnField[5].y, 100, 50, Drawings::ps_solid, ColorClasses::yellow, Drawings::bs_null, ColorClasses::yellow);
}
LINE("representation:CameraMatrixBH:Field", boundary.x.min, boundary.y.min, boundary.x.max, boundary.y.min, 30, Drawings::ps_solid, ColorRGBA(255, 0, 0));
LINE("representation:CameraMatrixBH:Field", boundary.x.max, boundary.y.min, boundary.x.max, boundary.y.max, 30, Drawings::ps_solid, ColorClasses::yellow);
LINE("representation:CameraMatrixBH:Field", boundary.x.max, boundary.y.max, boundary.x.min, boundary.y.max, 30, Drawings::ps_solid, ColorClasses::blue);
LINE("representation:CameraMatrixBH:Field", boundary.x.min, boundary.y.max, boundary.x.min, boundary.y.min, 30, Drawings::ps_solid, ColorClasses::white);
// fill the bounding rectangle with coordinate system lines (and reproject it to the image)
int spacing = 100;
for(int xx = boundary.x.min - boundary.x.min % spacing + spacing ; xx <= boundary.x.max; xx += spacing)
{
LINE("representation:CameraMatrixBH:Field", xx, boundary.y.min, xx, boundary.y.max, 5, Drawings::ps_solid, ColorClasses::white);
}
for(int yy = boundary.y.min - boundary.y.min % spacing + spacing ; yy <= boundary.y.max; yy += spacing)
{
LINE("representation:CameraMatrixBH:Field", boundary.x.min, yy, boundary.x.max, yy, 5, Drawings::ps_solid, ColorClasses::white);
}
});// end complex drawing
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");
}
}
}
// Personagem, morte
void block::BOMBERDIE() {
CIRCLE(12, 0);
DOT('x', 0, 15, 22, 24);
DOT(NR, 15, 0, 23);
}
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);
}
}
}
int main() {
printf("周长是%.2f\n", CIRCLE(2 + 3));
printf("周长是%.2f\n", CIRCLE(5));
return 0;
}
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);
}
});
// Bomba grande
void block::BOMB1(short int backcolor) {
CIRCLE(1, backcolor);
DOT(201, 8, 1, 13);
DOT(B2, 8, 1, 14);
DOT(UR, 8, 1, 23);
}
