Esempio n. 1
0
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);
    }
}
Esempio n. 2
0
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));
    }
  });
Esempio n. 3
0
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);
        }
    });
Esempio n. 4
0
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);
  }
}
Esempio n. 5
0
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);
}