robomon: Use both LIDARs using liblidar

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

diff --git a/src/robomon/hokuyoscan.cpp b/src/robomon/LidarScan.cpp
similarity index 54%
rename from src/robomon/hokuyoscan.cpp
rename to src/robomon/LidarScan.cpp
index 12b3e3ed4ae411a6778ef89c5fe39623cb3073a2..7af54dca434ecfc16935afca3c251d890cda67be 100644 (file)
--- a/src/robomon/hokuyoscan.cpp
+++ b/src/robomon/LidarScan.cpp
@@ -1,22 +1,19 @@
-#include "hokuyoscan.h"
+#include "LidarScan.h"
 #include <QPointF>
-#include <hokuyo.h>
-#include <sick.h>
 #include "PlaygroundScene.h"
-#include <robodim.h>
 #include <iostream>
 
-HokuyoScan::HokuyoScan() : QGraphicsItem(), showShapeDetect(false)
+LidarScan::LidarScan(const struct lidar_params arg_lidar) : QGraphicsItem(), showShapeDetect(false)
 {
+    lidar = arg_lidar;
     memset(&data, 0, sizeof(data));
-    memset(&sick_data, 0, sizeof(sick_data));
 }
 
-HokuyoScan::~HokuyoScan()
+LidarScan::~LidarScan()
 {
 }
 
-QRectF HokuyoScan::boundingRect() const
+QRectF LidarScan::boundingRect() const
 {
     QPointF tl, br;
     tl = PlaygroundScene::world2scene(QPointF(-3, -3));
@@ -24,7 +21,7 @@ QRectF HokuyoScan::boundingRect() const
     return QRectF(tl, br);
 }
 
-void HokuyoScan::paintShapeDetect(QPainter * painter)
+void LidarScan::paintShapeDetect(QPainter * painter)
 {
        Shape_detect sd;
 
@@ -58,25 +55,38 @@ void HokuyoScan::paintShapeDetect(QPainter * painter)
        }
 }
 
-void HokuyoScan::paint(QPainter * painter, const QStyleOptionGraphicsItem * option, QWidget * widget)
+void LidarScan::paint(QPainter * painter, const QStyleOptionGraphicsItem * option, QWidget * widget)
 {
     int d;
     double ang;
-    QPointF points[SICK_ARRAY_SIZE + 1];
+    QPointF points[lidar.data_lenght + 1];
     unsigned point_num = 0;
     QColor color;
 
-    painter->setPen(QPen(Qt::blue));
-    color = QColor(Qt::red);
-    color.setAlpha(50);
-    painter->setBrush(QBrush(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 < SICK_ARRAY_SIZE; i++) {
-        d = sick_data.data[i];
+    for (unsigned i = 0; i < lidar.data_lenght; i++) {
+        d = data.data[i];
 
-        ang = SICK_INDEX_TO_RAD(i);
+        ang = index2rad(lidar, i);
 
-        if((ang<(-SICK_RANGE_ANGLE_LEFT/180.0*M_PI))||((ang>(SICK_RANGE_ANGLE_RIGHT/180.0*M_PI)))) {
+        if((ang<(-lidar.range_angle_left/180.0*M_PI))||((ang>(lidar.range_angle_right/180.0*M_PI)))) {
                 continue;
         }
 
@@ -90,8 +100,8 @@ void HokuyoScan::paint(QPainter * painter, const QStyleOptionGraphicsItem * opti
                point_num++;
            }
 
-            x = d * cos(SICK_INDEX_TO_RAD(i));
-            y = d * sin(SICK_INDEX_TO_RAD(i));
+            x = d * cos(index2rad(lidar, i));
+            y = d * sin(index2rad(lidar, i));
 
            points[point_num] = QPointF(x, y);
            point_num++;
@@ -103,11 +113,11 @@ void HokuyoScan::paint(QPainter * painter, const QStyleOptionGraphicsItem * opti
     painter->drawPolygon(points, point_num);
 
     if (showShapeDetect)
-       paintShapeDetect(painter);
+          paintShapeDetect(painter);
 
 }
 
-void HokuyoScan::setPosition(double x, double y, double phi)
+void LidarScan::setPosition(double x, double y, double phi)
 {
        // Store position to be used when new data arrives
        this->x = x;
@@ -115,14 +125,14 @@ void HokuyoScan::setPosition(double x, double y, double phi)
        this->phi = phi;
 }
 
-void HokuyoScan::newScan(struct sick_scan_type *scan)
+void LidarScan::newScan(struct lidar_scan_type *scan)
 {
     QPointF pos(x, y);
     pos = PlaygroundScene::world2scene(pos);
     setPos(pos);
-    setTransform(QTransform().rotateRadians(phi).translate(SICK_CENTER_OFFSET_MM, 0));
+    setTransform(QTransform().rotateRadians(phi).translate(lidar.center_offset_m * 1000, 0));
 
-    sick_data = *scan;
+    data = *scan;
 
     update(boundingRect());
 }