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<int> input(HOKUYO_ARRAY_SIZE);
  30
  31         for (unsigned i = 0; i < HOKUYO_ARRAY_SIZE; i++)
  32                 input[i] = (int) data.data[i];
  33
  34         std::vector<Shape_detect::Line> output;
  35         sd.shape_detect(input, output);
  36
  37         QPen pen(Qt::yellow);
  38         pen.setWidth(20);
  39
  40         painter->setPen(pen);
  41
  42         for (unsigned i = 0; i < output.size(); i++)
  43                 painter->drawLine(output[i].a.x, output[i].a.y,
  44                                   output[i].b.x, output[i].b.y);
  45 }
  46
  47 void HokuyoScan::paint(QPainter * painter, const QStyleOptionGraphicsItem * option, QWidget * widget)
  48 {
  49     int d;
  50     QPointF points[HOKUYO_ARRAY_SIZE + 1];
  51     unsigned point_num = 0;
  52     QColor color;
  53
  54     painter->setPen(QPen(Qt::blue));
  55     color = QColor(Qt::red);
  56     color.setAlpha(50);
  57     painter->setBrush(QBrush(color));
  58
  59     for (unsigned i=0; i < HOKUYO_ARRAY_SIZE; i++) {
  60         d = data.data[i];
  61         if (d > 5600)
  62             d = 5600;
  63         if (d > 19) {
  64             float x, y;
  65
  66             if (point_num == 0) {
  67                 points[0] = QPointF(0, 0);
  68                 point_num++;
  69             }
  70
  71             x = d * cos(HOKUYO_INDEX_TO_RAD(i));
  72             y = d * sin(HOKUYO_INDEX_TO_RAD(i));
  73
  74             points[point_num] = QPointF(x, y);
  75             point_num++;
  76         } else {
  77             painter->drawPolygon(points, point_num);
  78             point_num = 0;
  79         }
  80     }
  81     painter->drawPolygon(points, point_num);
  82
  83     if (showShapeDetect)
  84         paintShapeDetect(painter);
  85
  86 }
  87
  88 void HokuyoScan::setPosition(double x, double y, double phi)
  89 {
  90         // Store position to be used when new data arrives
  91         this->x = x;
  92         this->y = y;
  93         this->phi = phi;
  94 }
  95
  96 void HokuyoScan::newScan(struct hokuyo_scan_type *scan)
  97 {
  98     QPointF pos(x, y);
  99     pos = PlaygroundScene::world2scene(pos);
 100     setPos(pos);
 101     setTransform(QTransform().rotateRadians(phi).translate(HOKUYO_CENTER_OFFSET_MM, 0));
 102
 103     data = *scan;
 104
 105     update(boundingRect());
 106 }