static void drawDigit(int digit, const Vector3f& pos, float size, const ColorRGBA& color)
{
static const Vector3f points[8] =
{
Vector3f(1, 0, 1),
Vector3f(1, 0, 0),
Vector3f(0, 0, 0),
Vector3f(0, 0, 1),
Vector3f(0, 0, 2),
Vector3f(1, 0, 2),
Vector3f(1, 0, 1),
Vector3f(0, 0, 1)
};
static const unsigned char digits[10] =
{
0x3f,
0x0c,
0x76,
0x5e,
0x4d,
0x5b,
0x7b,
0x0e,
0x7f,
0x5f
};
digit = digits[std::abs(digit)];
for(int i = 0; i < 7; ++i)
if(digit & (1 << i))
{
Vector3f from = pos - points[i] * size;
Vector3f to = pos - points[i + 1] * size;
LINE3D("representation:GameInfo", from.x(), from.y(), from.z(), to.x(), to.y(), to.z(), 2, color);
}
}
void RobotModelBH::draw() const
{
DECLARE_DEBUG_DRAWING3D("representation:RobotModelBH", "origin");
COMPLEX_DRAWING3D("representation:RobotModelBH",
{
for(int i = 0; i < MassCalibrationBH::numOfLimbs; ++i)
{
const Pose3DBH& p = limbs[i];
const Vector3BH<>& v = p.translation;
const Vector3BH<> v1 = p * Vector3BH<>(50, 0, 0);
const Vector3BH<> v2 = p * Vector3BH<>(0, 50, 0);
const Vector3BH<> v3 = p * Vector3BH<>(0, 0, 50);
LINE3D("representation:RobotModelBH", v.x, v.y, v.z, v1.x, v1.y, v1.z, 1, ColorRGBA(255, 0, 0));
LINE3D("representation:RobotModelBH", v.x, v.y, v.z, v2.x, v2.y, v2.z, 1, ColorRGBA(0, 255, 0));
LINE3D("representation:RobotModelBH", v.x, v.y, v.z, v3.x, v3.y, v3.z, 1, ColorRGBA(0, 0, 255));
}
});
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 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 BodyContourProvider::add(const Pose3D& origin, const std::vector<Vector3<> >& c, float sign,
BodyContour& bodyContour)
{
Vector2<int> q1,
q2;
Vector3<> p1 = origin * Vector3<>(c[0].x, c[0].y * sign, c[0].z);
bool valid1 = calculatePointInImage(p1, q1);
if(valid1)
{
Vector2<> v = theImageCoordinateSystem.fromCorrectedApprox(q1);
q1 = Vector2<int>(int(floor(v.x)), int(floor(v.y)));
}
for(unsigned i = 1; i < c.size(); ++i)
{
Vector3<> p2 = origin * Vector3<>(c[i].x, c[i].y * sign, c[i].z);
bool valid2 = calculatePointInImage(p2, q2);
if(valid2)
{
Vector2<> v = theImageCoordinateSystem.fromCorrectedApprox(q2);
q2 = Vector2<int>(int(floor(v.x)), int(floor(v.y)));
}
if(valid1 && valid2 &&
(q1.y < theCameraInfo.height || q2.y < theCameraInfo.height) &&
(q1.x >= 0 || q2.x >= 0) &&
(q1.x < theCameraInfo.width || q2.x < theCameraInfo.width))
bodyContour.lines.push_back(BodyContour::Line(q1, q2));
q1 = q2;
valid1 = valid2;
COMPLEX_DRAWING3D("module:BodyContourProvider:contour",
{
LINE3D("module:BodyContourProvider:contour", p1.x, p1.y, p1.z, p2.x, p2.y, p2.z, 1, ColorRGBA(255, 0, 0));
p1 = p2;
});
void JointDataPrediction::draw() const
{
DECLARE_PLOT("representation:JointDataPrediction:position:lHipYawPitch");
DECLARE_PLOT("representation:JointDataPrediction:position:lHipRoll");
DECLARE_PLOT("representation:JointDataPrediction:position:lHipPitch");
DECLARE_PLOT("representation:JointDataPrediction:position:lKneePitch");
DECLARE_PLOT("representation:JointDataPrediction:position:lAnklePitch");
DECLARE_PLOT("representation:JointDataPrediction:position:lAnkleRoll");
DECLARE_PLOT("representation:JointDataPrediction:position:rHipYawPitch");
DECLARE_PLOT("representation:JointDataPrediction:position:rHipRoll");
DECLARE_PLOT("representation:JointDataPrediction:position:rHipPitch");
DECLARE_PLOT("representation:JointDataPrediction:position:rKneePitch");
DECLARE_PLOT("representation:JointDataPrediction:position:rAnklePitch");
DECLARE_PLOT("representation:JointDataPrediction:position:rAnkleRoll");
DECLARE_PLOT("representation:JointDataPrediction:velocity:lHipYawPitch");
DECLARE_PLOT("representation:JointDataPrediction:velocity:lHipRoll");
DECLARE_PLOT("representation:JointDataPrediction:velocity:lHipPitch");
DECLARE_PLOT("representation:JointDataPrediction:velocity:lKneePitch");
DECLARE_PLOT("representation:JointDataPrediction:velocity:lAnklePitch");
DECLARE_PLOT("representation:JointDataPrediction:velocity:lAnkleRoll");
DECLARE_PLOT("representation:JointDataPrediction:velocity:rHipYawPitch");
DECLARE_PLOT("representation:JointDataPrediction:velocity:rHipRoll");
DECLARE_PLOT("representation:JointDataPrediction:velocity:rHipPitch");
DECLARE_PLOT("representation:JointDataPrediction:velocity:rKneePitch");
DECLARE_PLOT("representation:JointDataPrediction:velocity:rAnklePitch");
DECLARE_PLOT("representation:JointDataPrediction:velocity:rAnkleRoll");
DECLARE_PLOT("representation:JointDataPrediction:acceleration:lHipYawPitch");
DECLARE_PLOT("representation:JointDataPrediction:acceleration:lHipRoll");
DECLARE_PLOT("representation:JointDataPrediction:acceleration:lHipPitch");
DECLARE_PLOT("representation:JointDataPrediction:acceleration:lKneePitch");
DECLARE_PLOT("representation:JointDataPrediction:acceleration:lAnklePitch");
DECLARE_PLOT("representation:JointDataPrediction:acceleration:lAnkleRoll");
DECLARE_PLOT("representation:JointDataPrediction:acceleration:rHipYawPitch");
DECLARE_PLOT("representation:JointDataPrediction:acceleration:rHipRoll");
DECLARE_PLOT("representation:JointDataPrediction:acceleration:rHipPitch");
DECLARE_PLOT("representation:JointDataPrediction:acceleration:rKneePitch");
DECLARE_PLOT("representation:JointDataPrediction:acceleration:rAnklePitch");
DECLARE_PLOT("representation:JointDataPrediction:acceleration:rAnkleRoll");
PLOT("representation:JointDataPrediction:velocity:lHipYawPitch",
velocities[Joints::lHipYawPitch]);
PLOT("representation:JointDataPrediction:velocity:lHipRoll",
velocities[Joints::lHipRoll]);
PLOT("representation:JointDataPrediction:velocity:lHipPitch",
velocities[Joints::lHipPitch]);
PLOT("representation:JointDataPrediction:velocity:lKneePitch",
velocities[Joints::lKneePitch]);
PLOT("representation:JointDataPrediction:velocity:lAnklePitch",
velocities[Joints::lAnklePitch]);
PLOT("representation:JointDataPrediction:velocity:lAnkleRoll",
velocities[Joints::lAnkleRoll]);
PLOT("representation:JointDataPrediction:velocity:rHipYawPitch",
velocities[Joints::rHipYawPitch]);
PLOT("representation:JointDataPrediction:velocity:rHipRoll",
velocities[Joints::rHipRoll]);
PLOT("representation:JointDataPrediction:velocity:rHipPitch",
velocities[Joints::rHipPitch]);
PLOT("representation:JointDataPrediction:velocity:rKneePitch",
velocities[Joints::rKneePitch]);
PLOT("representation:JointDataPrediction:velocity:rAnklePitch",
velocities[Joints::rAnklePitch]);
PLOT("representation:JointDataPrediction:velocity:rAnkleRoll",
velocities[Joints::rAnkleRoll]);
PLOT("representation:JointDataPrediction:position:lHipYawPitch",
angles[Joints::lHipYawPitch]);
PLOT("representation:JointDataPrediction:position:lHipRoll",
angles[Joints::lHipRoll]);
PLOT("representation:JointDataPrediction:position:lHipPitch",
angles[Joints::lHipPitch]);
PLOT("representation:JointDataPrediction:position:lKneePitch",
angles[Joints::lKneePitch]);
PLOT("representation:JointDataPrediction:position:lAnklePitch",
angles[Joints::lAnklePitch]);
PLOT("representation:JointDataPrediction:position:lAnkleRoll",
angles[Joints::lAnkleRoll]);
PLOT("representation:JointDataPrediction:position:rHipYawPitch",
angles[Joints::rHipYawPitch]);
PLOT("representation:JointDataPrediction:position:rHipRoll",
angles[Joints::rHipRoll]);
PLOT("representation:JointDataPrediction:position:rHipPitch",
angles[Joints::rHipPitch]);
PLOT("representation:JointDataPrediction:position:rKneePitch",
angles[Joints::rKneePitch]);
PLOT("representation:JointDataPrediction:position:rAnklePitch",
angles[Joints::rAnklePitch]);
PLOT("representation:JointDataPrediction:position:rAnkleRoll",
angles[Joints::rAnkleRoll]);
PLOT("representation:JointDataPrediction:acceleration:lHipYawPitch",
accelerations[Joints::lHipYawPitch]);
PLOT("representation:JointDataPrediction:acceleration:lHipRoll",
accelerations[Joints::lHipRoll]);
PLOT("representation:JointDataPrediction:acceleration:lHipPitch",
accelerations[Joints::lHipPitch]);
PLOT("representation:JointDataPrediction:acceleration:lKneePitch",
accelerations[Joints::lKneePitch]);
PLOT("representation:JointDataPrediction:acceleration:lAnklePitch",
accelerations[Joints::lAnklePitch]);
PLOT("representation:JointDataPrediction:acceleration:lAnkleRoll",
accelerations[Joints::lAnkleRoll]);
PLOT("representation:JointDataPrediction:acceleration:rHipYawPitch",
accelerations[Joints::rHipYawPitch]);
PLOT("representation:JointDataPrediction:acceleration:rHipRoll",
accelerations[Joints::rHipRoll]);
PLOT("representation:JointDataPrediction:acceleration:rHipPitch",
accelerations[Joints::rHipPitch]);
PLOT("representation:JointDataPrediction:acceleration:rKneePitch",
accelerations[Joints::rKneePitch]);
PLOT("representation:JointDataPrediction:acceleration:rAnklePitch",
accelerations[Joints::rAnklePitch]);
PLOT("representation:JointDataPrediction:acceleration:rAnkleRoll",
accelerations[Joints::rAnkleRoll]);
DECLARE_DEBUG_DRAWING3D("representation:JointDataPrediction:com", "robot");
CROSS3D("representation:JointDataPrediction:com", com.x(), com.y(), com.z(),
50, 50, ColorRGBA::red);
LINE3D("representation:JointDataPrediction:com", com.x(), com.y(), com.z(),
com.x() + comAcceleration.x(), com.y() + comAcceleration.y(),
com.z() + comAcceleration.z(), 10, ColorRGBA::red);
DECLARE_PLOT("representation:JointDataPrediction:comAccX");
DECLARE_PLOT("representation:JointDataPrediction:comAccY");
DECLARE_PLOT("representation:JointDataPrediction:comAccZ");
DECLARE_PLOT("representation:JointDataPrediction:comX");
DECLARE_PLOT("representation:JointDataPrediction:comY");
DECLARE_PLOT("representation:JointDataPrediction:comAtan2");
DECLARE_PLOT("representation:JointDataPrediction:comAccAtan2");
PLOT("representation:JointDataPrediction:comAccX", comAcceleration.x());
PLOT("representation:JointDataPrediction:comAccY", comAcceleration.y());
PLOT("representation:JointDataPrediction:comAccZ", comAcceleration.z());
PLOT("representation:JointDataPrediction:comAccAtan2", atan2(comAcceleration.y(), comAcceleration.x()));
PLOT("representation:JointDataPrediction:comX", com.x());
PLOT("representation:JointDataPrediction:comY", com.y());
PLOT("representation:JointDataPrediction:comAtan2", atan2(com.y(), com.x()));
DECLARE_PLOT("representation:JointDataPrediction:angleSum");
float sum = 0.0f;
for(int i = 0; i < Joints::numOfJoints; ++i)
{
sum += angles[i];
}
PLOT("representation:JointDataPrediction:angleSum", sum);
DECLARE_PLOT("representation:JointDataPrediction:squareVelSum");
float velSum = 0.0f;
for(int i = 0; i < Joints::numOfJoints; ++i)
{
velSum += velocities[i] * velocities[i];
}
PLOT("representation:JointDataPrediction:squareVelSum", velSum);
}