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