src/robomon/hokuyoscan.cpp

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