src/robomon/hokuyoscan.cpp

   1 #include "hokuyoscan.h"
   2 #include <QPointF>
   3 #include <hokuyo.h>
   4 #include "PlaygroundScene.h"
   5 #include <robodim.h>
   6 #include <iostream>
   7
   8 HokuyoScan::HokuyoScan() : QGraphicsItem(), showShapeDetect(false)
   9 {
  10     memset(&data, 0, sizeof(data));
  11 }
  12
  13 HokuyoScan::~HokuyoScan()
  14 {
  15 }
  16
  17 QRectF HokuyoScan::boundingRect() const
  18 {
  19     QPointF tl, br;
  20     tl = PlaygroundScene::world2scene(QPointF(-3, -3));
  21     br = PlaygroundScene::world2scene(QPointF(+3, +3));
  22     return QRectF(tl, br);
  23 }
  24
  25 void HokuyoScan::paintShapeDetect(QPainter * painter)
  26 {
  27         Shape_detect sd;
  28
  29         std::vector<Shape_detect::Line> lines;
  30         std::vector<Shape_detect::Arc> arcs;
  31
  32         sd.prepare(data.data);
  33         sd.line_detect(lines);
  34         sd.arc_detect(arcs);
  35
  36         QPen pen(Qt::yellow);
  37         pen.setWidth(20);
  38         painter->setBrush(QBrush(Qt::NoBrush));
  39
  40         painter->setPen(pen);
  41
  42         for (unsigned i = 0; i < lines.size(); i++)
  43                 painter->drawLine(lines[i].a.x, lines[i].a.y,
  44                                   lines[i].b.x, lines[i].b.y);
  45
  46         QPen pen2(Qt::green);
  47         pen2.setWidth(20);
  48
  49         painter->setPen(pen2);
  50
  51         for (unsigned i = 0; i < arcs.size(); i++) {
  52                 Shape_detect::Arc *a = &arcs[i];
  53                 painter->drawPoint(QPoint(a->center.x, a->center.y));
  54                 painter->drawEllipse(QRectF(a->center.x - a->radius, a->center.y - a->radius,
  55                                             2*a->radius, 2*a->radius));
  56         }
  57 }
  58
  59 void HokuyoScan::paint(QPainter * painter, const QStyleOptionGraphicsItem * option, QWidget * widget)
  60 {
  61     int d;
  62     double ang;
  63     QPointF points[HOKUYO_ARRAY_SIZE + 1];
  64     unsigned point_num = 0;
  65     QColor color;
  66
  67     painter->setPen(QPen(Qt::blue));
  68     color = QColor(Qt::red);
  69     color.setAlpha(50);
  70     painter->setBrush(QBrush(color));
  71
  72     for (unsigned i=0; i < HOKUYO_ARRAY_SIZE; i++) {
  73         d = data.data[i];
  74
  75         ang = HOKUYO_INDEX_TO_RAD(i);
  76
  77         if((ang<(-HOKUYO_RANGE_ANGLE_LEFT/180.0*M_PI))||((ang>(HOKUYO_RANGE_ANGLE_RIGHT/180.0*M_PI)))) {
  78                 continue;
  79         }
  80
  81         if (d > 5600)
  82             d = 5600;
  83         if (d > 19) {
  84             float x, y;
  85
  86             if (point_num == 0) {
  87                 points[0] = QPointF(0, 0);
  88                 point_num++;
  89             }
  90
  91             x = d * cos(HOKUYO_INDEX_TO_RAD(i));
  92             y = d * sin(HOKUYO_INDEX_TO_RAD(i));
  93
  94             points[point_num] = QPointF(x, y);
  95             point_num++;
  96         } else {
  97             painter->drawPolygon(points, point_num);
  98             point_num = 0;
  99         }
 100     }
 101     painter->drawPolygon(points, point_num);
 102
 103     if (showShapeDetect)
 104         paintShapeDetect(painter);
 105
 106 }
 107
 108 void HokuyoScan::setPosition(double x, double y, double phi)
 109 {
 110         // Store position to be used when new data arrives
 111         this->x = x;
 112         this->y = y;
 113         this->phi = phi;
 114 }
 115
 116 void HokuyoScan::newScan(struct hokuyo_scan_type *scan)
 117 {
 118     QPointF pos(x, y);
 119     pos = PlaygroundScene::world2scene(pos);
 120     setPos(pos);
 121     setTransform(QTransform().rotateRadians(phi).translate(HOKUYO_CENTER_OFFSET_MM, 0));
 122
 123     data = *scan;
 124
 125     update(boundingRect());
 126 }