void BodyContourProvider::update(BodyContour& bodyContour)
{
DECLARE_DEBUG_DRAWING3D("module:BodyContourProvider:contour", "robot");
bodyContour.cameraResolution.x = theCameraInfo.width;
bodyContour.cameraResolution.y = theCameraInfo.height;
bodyContour.lines.clear();
robotCameraMatrixInverted = theRobotCameraMatrix.invert();
add(Pose3D(), torso, 1, bodyContour);
add(theRobotModel.limbs[MassCalibration::bicepsLeft], shoulder, 1, bodyContour);
add(theRobotModel.limbs[MassCalibration::bicepsRight], shoulder, -1, bodyContour);
add(theRobotModel.limbs[MassCalibration::bicepsLeft], upperArm, 1, bodyContour);
add(theRobotModel.limbs[MassCalibration::bicepsRight], upperArm, -1, bodyContour);
add(theRobotModel.limbs[MassCalibration::foreArmLeft], lowerArm, 1, bodyContour);
add(theRobotModel.limbs[MassCalibration::foreArmRight], lowerArm, -1, bodyContour);
add(theRobotModel.limbs[MassCalibration::foreArmLeft], lowerArm2, 1, bodyContour);
add(theRobotModel.limbs[MassCalibration::foreArmRight], lowerArm2, -1, bodyContour);
add(theRobotModel.limbs[MassCalibration::thighLeft], upperLeg1, 1, bodyContour);
add(theRobotModel.limbs[MassCalibration::thighRight], upperLeg1, -1, bodyContour);
add(theRobotModel.limbs[MassCalibration::thighLeft], upperLeg2, 1, bodyContour);
add(theRobotModel.limbs[MassCalibration::thighRight], upperLeg2, -1, bodyContour);
add(theRobotModel.limbs[MassCalibration::footLeft], foot, 1, bodyContour);
add(theRobotModel.limbs[MassCalibration::footRight], foot, -1, bodyContour);
}
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 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 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 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 CognitionLogDataProvider::update(Image& image)
{
if(SystemCall::getMode() == SystemCall::logfileReplay)
{
CameraInfo& info = (CameraInfo&) Blackboard::getInstance()["CameraInfo"];
if(lowFrameRateImage)
{
if(lowFrameRateImage->imageUpdated)
lastImages[info.camera] = lowFrameRateImage->image;
image = lastImages[info.camera];
}
else if(info.width != image.width || info.height != image.height)
image = Image(true, info.width, info.height);
}
static const float distance = 300.f;
DECLARE_DEBUG_DRAWING3D("representation:Image", "camera");
IMAGE3D("representation:Image", distance, 0, 0, 0, 0, 0,
distance * theCameraInfo.width / theCameraInfo.focalLength,
distance * theCameraInfo.height / theCameraInfo.focalLength,
image);
DEBUG_RESPONSE("representation:JPEGImage") OUTPUT(idJPEGImage, bin, JPEGImage(image));
}
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);
}
