robomon: Add support for Sick Tim 551 and fix some indentation issues

[eurobot/public.git] / src / robomon / LidarScan.cpp

diff --git a/src/robomon/LidarScan.cpp b/src/robomon/LidarScan.cpp
index 7af54dca434ecfc16935afca3c251d890cda67be..5d4585712b56daff02e86eef4d8b0cbe348adf95 100644 (file)
--- a/src/robomon/LidarScan.cpp
+++ b/src/robomon/LidarScan.cpp
@@ -5,8 +5,8 @@
 
 LidarScan::LidarScan(const struct lidar_params arg_lidar) : QGraphicsItem(), showShapeDetect(false)
 {
-    lidar = arg_lidar;
-    memset(&data, 0, sizeof(data));
+       lidar = arg_lidar;
+       memset(&data, 0, sizeof(data));
 }
 
 LidarScan::~LidarScan()
@@ -15,10 +15,10 @@ LidarScan::~LidarScan()
 
 QRectF LidarScan::boundingRect() const
 {
-    QPointF tl, br;
-    tl = PlaygroundScene::world2scene(QPointF(-3, -3));
-    br = PlaygroundScene::world2scene(QPointF(+3, +3));
-    return QRectF(tl, br);
+       QPointF tl, br;
+       tl = PlaygroundScene::world2scene(QPointF(-3, -3));
+       br = PlaygroundScene::world2scene(QPointF(+3, +3));
+       return QRectF(tl, br);
 }
 
 void LidarScan::paintShapeDetect(QPainter * painter)
@@ -51,68 +51,73 @@ void LidarScan::paintShapeDetect(QPainter * painter)
                Shape_detect::Arc *a = &arcs[i];
                painter->drawPoint(QPoint(a->center.x, a->center.y));
                painter->drawEllipse(QRectF(a->center.x - a->radius, a->center.y - a->radius,
-                                           2*a->radius, 2*a->radius));
+                                               2*a->radius, 2*a->radius));
        }
 }
 
 void LidarScan::paint(QPainter * painter, const QStyleOptionGraphicsItem * option, QWidget * widget)
 {
-    int d;
-    double ang;
-    QPointF points[lidar.data_lenght + 1];
-    unsigned point_num = 0;
-    QColor color;
-
-    switch (lidar.type) {
-        case HOKUYO:
-            painter->setPen(QPen(Qt::blue));
-            color = QColor(Qt::red);
-            color.setAlpha(50);
-            painter->setBrush(QBrush(color));
-            break;
-        case SICK_TIM3XX:
-            painter->setPen(QPen(Qt::green));
-            color = QColor(Qt::yellow);
-            color.setAlpha(75);
-            painter->setBrush(QBrush(color));
-            break;
-        default:
-            return;
-    }
-
-
-    for (unsigned i = 0; i < lidar.data_lenght; i++) {
-        d = data.data[i];
-
-        ang = index2rad(lidar, i);
-
-        if((ang<(-lidar.range_angle_left/180.0*M_PI))||((ang>(lidar.range_angle_right/180.0*M_PI)))) {
-                continue;
-        }
-
-        if (d > 5600)
-            d = 5600;
-        if (d > 19) {
-            float x, y;
-
-           if (point_num == 0) {
+       int d;
+       double ang;
+       QPointF points[lidar.data_lenght + 1];
+       unsigned point_num = 0;
+       QColor color;
+
+       switch (lidar.type) {
+               case HOKUYO:
+                       painter->setPen(QPen(Qt::darkCyan));
+                       color = QColor(Qt::cyan);
+                       color.setAlpha(40);
+                       painter->setBrush(QBrush(color));
+                       break;
+               case SICK_TIM3XX:
+                       painter->setPen(QPen(Qt::darkMagenta));
+                       color = QColor(Qt::magenta);
+                       color.setAlpha(20);
+                       painter->setBrush(QBrush(color));
+                       break;
+               case SICK_TIM551:
+                       painter->setPen(QPen(Qt::darkYellow));
+                       color = QColor(Qt::yellow);
+                       color.setAlpha(60);
+                       painter->setBrush(QBrush(color));
+                       break;
+               default:
+                       return;
+       }
+
+       for (unsigned i = 0; i < lidar.data_lenght; i++) {
+               d = data.data[i];
+
+               ang = index2rad(lidar, i);
+
+               if((ang<(-lidar.range_angle_left/180.0*M_PI))||((ang>(lidar.range_angle_right/180.0*M_PI)))) {
+                               continue;
+               }
+
+               if (d > 5600)
+                       d = 5600;
+               if (d > 19) {
+                       float x, y;
+
+               if (point_num == 0) {
                points[0] = QPointF(0, 0);
                point_num++;
-           }
+               }
 
-            x = d * cos(index2rad(lidar, i));
-            y = d * sin(index2rad(lidar, i));
+                       x = d * cos(index2rad(lidar, i));
+                       y = d * sin(index2rad(lidar, i));
 
-           points[point_num] = QPointF(x, y);
-           point_num++;
-        } else {
-           painter->drawPolygon(points, point_num);
-           point_num = 0;
+               points[point_num] = QPointF(x, y);
+               point_num++;
+               } else {
+               painter->drawPolygon(points, point_num);
+               point_num = 0;
+       }
        }
-    }
-    painter->drawPolygon(points, point_num);
+       painter->drawPolygon(points, point_num);
 
-    if (showShapeDetect)
+       if (showShapeDetect)
           paintShapeDetect(painter);
 
 }
@@ -127,12 +132,12 @@ void LidarScan::setPosition(double x, double y, double phi)
 
 void LidarScan::newScan(struct lidar_scan_type *scan)
 {
-    QPointF pos(x, y);
-    pos = PlaygroundScene::world2scene(pos);
-    setPos(pos);
-    setTransform(QTransform().rotateRadians(phi).translate(lidar.center_offset_m * 1000, 0));
+       QPointF pos(x, y);
+       pos = PlaygroundScene::world2scene(pos);
+       setPos(pos);
+       setTransform(QTransform().rotateRadians(phi).translate(lidar.center_offset_m * 1000, 0));
 
-    data = *scan;
+       data = *scan;
 
-    update(boundingRect());
+       update(boundingRect());
 }