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