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->drawEllipse(QRectF(a->center.x - a->radius, a->center.y - a->radius,
  54                                             2*a->radius, 2*a->radius));
  55         }
  56 }
  57
  58 void HokuyoScan::paint(QPainter * painter, const QStyleOptionGraphicsItem * option, QWidget * widget)
  59 {
  60     int d;
  61     double ang;
  62     QPointF points[HOKUYO_ARRAY_SIZE + 1];
  63     unsigned point_num = 0;
  64     QColor color;
  65
  66     painter->setPen(QPen(Qt::blue));
  67     color = QColor(Qt::red);
  68     color.setAlpha(50);
  69     painter->setBrush(QBrush(color));
  70
  71     for (unsigned i=0; i < HOKUYO_ARRAY_SIZE; i++) {
  72         d = data.data[i];
  73
  74         ang = HOKUYO_INDEX_TO_RAD(i);
  75
  76         if((ang<(-HOKUYO_RANGE_ANGLE_LEFT/180.0*M_PI))||((ang>(HOKUYO_RANGE_ANGLE_RIGHT/180.0*M_PI)))) {
  77                 continue;
  78         }
  79
  80         if (d > 5600)
  81             d = 5600;
  82         if (d > 19) {
  83             float x, y;
  84
  85             if (point_num == 0) {
  86                 points[0] = QPointF(0, 0);
  87                 point_num++;
  88             }
  89
  90             x = d * cos(HOKUYO_INDEX_TO_RAD(i));
  91             y = d * sin(HOKUYO_INDEX_TO_RAD(i));
  92
  93             points[point_num] = QPointF(x, y);
  94             point_num++;
  95         } else {
  96             painter->drawPolygon(points, point_num);
  97             point_num = 0;
  98         }
  99     }
 100     painter->drawPolygon(points, point_num);
 101
 102     if (showShapeDetect)
 103         paintShapeDetect(painter);
 104
 105 }
 106
 107 void HokuyoScan::setPosition(double x, double y, double phi)
 108 {
 109         // Store position to be used when new data arrives
 110         this->x = x;
 111         this->y = y;
 112         this->phi = phi;
 113 }
 114
 115 void HokuyoScan::newScan(struct hokuyo_scan_type *scan)
 116 {
 117     QPointF pos(x, y);
 118     pos = PlaygroundScene::world2scene(pos);
 119     setPos(pos);
 120     setTransform(QTransform().rotateRadians(phi).translate(HOKUYO_CENTER_OFFSET_MM, 0));
 121
 122     data = *scan;
 123
 124     update(boundingRect());
 125 }