robomon: Display hokuyo scan data

diff --git a/src/hokuyo/hokuyo.h b/src/hokuyo/hokuyo.h
index 5525d9a8478f2cfc09b10a5616fe8da4a408e47b..0b10a28f687f2b63f408c3a9967c2bbb01dc3474 100644 (file)
--- a/src/hokuyo/hokuyo.h
+++ b/src/hokuyo/hokuyo.h
@@ -10,7 +10,7 @@
 #define HOKUYO_INITIAL_MEASUREMENT 44
 #define HOKUYO_FINAL_MEASUREMENT 725
 
-#define HOKUYO_INDEX_TO_RAD(x) (-113.0+(x)*360.0/HOKUYO_SPLIT_DIVISION)
-#define HOKUYO_CLUSTER_TO_RAD(x) (HOKUYO_CLUSTER_TO_DEG(x)/180.0*M_PI)
+#define HOKUYO_INDEX_TO_DEG(x) (-113.0+(x)*360.0/HOKUYO_SPLIT_DIVISION)
+#define HOKUYO_INDEX_TO_RAD(x) (HOKUYO_INDEX_TO_DEG(x)/180.0*M_PI)
 
 #endif //HOKUYO_H

diff --git a/src/robomon/RobomonAtlantis.cpp b/src/robomon/RobomonAtlantis.cpp
index e39bfbccd527626b03239f47def9dc1561ffdf4b..c23bb65a15e8aa99f526e8c7ecb392cb92ccda33 100644 (file)
--- a/src/robomon/RobomonAtlantis.cpp
+++ b/src/robomon/RobomonAtlantis.cpp
@@ -336,6 +336,11 @@ void RobomonAtlantis::createRobots()
        playgroundScene->addItem(trailRefPos);
        playgroundScene->addItem(trailPosIndepOdo);
        playgroundScene->addItem(trailOdoPos);
+
+       hokuyoScan = new HokuyoScan();
+       hokuyoScan->setZValue(10);
+       playgroundScene->addItem(hokuyoScan);
+       
 }
 
 /**********************************************************************
@@ -568,6 +573,9 @@ bool RobomonAtlantis::event(QEvent *event)
                case QEVENT(QEV_MOTION_STATUS):
                        emit motionStatusReceivedSignal();
                        break;
+               case QEVENT(QEV_HOKUYO_SCAN):
+                       hokuyoScan->newScan(&orte.hokuyo_scan);
+                       break;
                case QEVENT(QEV_REFERENCE_POSITION):
                        emit actualPositionReceivedSignal();
                        break;
@@ -581,6 +589,10 @@ bool RobomonAtlantis::event(QEvent *event)
                                orte.est_pos_indep_odo.y, orte.est_pos_indep_odo.phi);
                        trailPosIndepOdo->addPoint(QPointF(orte.est_pos_indep_odo.x, 
                                              orte.est_pos_indep_odo.y));
+
+//                     hokuyoScan->setPosition(orte.est_pos_indep_odo.x, 
+//                                             orte.est_pos_indep_odo.y,
+//                                             orte.est_pos_indep_odo.phi); 
                        break;
                case QEVENT(QEV_ESTIMATED_POSITION_ODO):
                        robotEstPosOdo->moveRobot(orte.est_pos_odo.x, 
@@ -761,6 +773,8 @@ void RobomonAtlantis::createOrte()
                        generic_rcv_cb, new OrteCallbackInfo(this, QEV_ESTIMATED_POSITION_ODO));
        robottype_subscriber_est_pos_indep_odo_create(&orte, 
                        generic_rcv_cb, new OrteCallbackInfo(this, QEV_ESTIMATED_POSITION_INDEP_ODO));
+       robottype_subscriber_hokuyo_scan_create(&orte, 
+                       generic_rcv_cb, new OrteCallbackInfo(this, QEV_HOKUYO_SCAN));
        robottype_subscriber_fsm_main_create(&orte, 
                                             rcv_fsm_main_cb, this);
        robottype_subscriber_fsm_motion_create(&orte, 

diff --git a/src/robomon/RobomonAtlantis.h b/src/robomon/RobomonAtlantis.h
index da1c8508563f2dca7c3cb19b0f146d91beb56ae8..88eea467ab01f96380c59fa24a393f460ccce301 100644 (file)
--- a/src/robomon/RobomonAtlantis.h
+++ b/src/robomon/RobomonAtlantis.h
@@ -22,6 +22,7 @@
 #include "Robot.h"
 #include <roboorte_robottype.h>
 #include "trail.h"
+#include "hokuyoscan.h"
 
 class QHBoxLayout;
 class QVBoxLayout;
@@ -166,6 +167,8 @@ private:
        Trail *trailPosIndepOdo;
        Trail *trailOdoPos;
 
+        HokuyoScan *hokuyoScan;
+
        /* keypad */
        double leftMotorValue;
        double rightMotorValue;

diff --git a/src/robomon/hokuyoscan.cpp b/src/robomon/hokuyoscan.cpp
new file mode 100644 (file)
index 0000000..2528c75
--- /dev/null
+++ b/src/robomon/hokuyoscan.cpp
@@ -0,0 +1,76 @@
+#include "hokuyoscan.h"
+#include <QPointF>
+#include <hokuyo.h>
+#include "PlaygroundScene.h"
+#include <robodim.h>
+#include <iostream>
+
+HokuyoScan::HokuyoScan() : QGraphicsItem()
+{
+    memset(&data, 0, sizeof(data));
+}
+
+HokuyoScan::~HokuyoScan()
+{
+}
+
+QRectF HokuyoScan::boundingRect() const
+{
+    QPointF tl, br;
+    tl = PlaygroundScene::world2scene(QPointF(-3, -3));
+    br = PlaygroundScene::world2scene(QPointF(+3, +3));
+    return QRectF(tl, br);
+}
+
+void HokuyoScan::paint(QPainter * painter, const QStyleOptionGraphicsItem * option, QWidget * widget)
+{
+    int d;
+    QPointF points[HOKUYO_ARRAY_SIZE + 1];
+    unsigned point_num = 0;
+    QColor color;
+
+    painter->setPen(QPen(Qt::blue));
+    color = QColor(Qt::red);
+    color.setAlpha(50);
+    painter->setBrush(QBrush(color));
+
+    for (unsigned i=0; i < HOKUYO_ARRAY_SIZE; i++) {
+        d = data.data[i];
+       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(HOKUYO_INDEX_TO_RAD(i));
+            y = d * sin(HOKUYO_INDEX_TO_RAD(i));
+
+           points[point_num] = QPointF(x, y);
+           point_num++;
+        } else {
+           painter->drawPolygon(points, point_num);
+           point_num = 0;
+       }
+    }
+    painter->drawPolygon(points, point_num);
+}
+
+void HokuyoScan::setPosition(double x, double y, double phi)
+{
+    QPointF pos(x, y);
+
+    pos = PlaygroundScene::world2scene(pos);
+
+    setPos(pos);
+    setTransform(QTransform().rotateRadians(phi).translate(HOKUYO_CENTER_OFFSET_MM, 0));
+}
+
+void HokuyoScan::newScan(struct hokuyo_scan_type *scan)
+{
+    data = *scan;
+    update(boundingRect());
+}

diff --git a/src/robomon/hokuyoscan.h b/src/robomon/hokuyoscan.h
new file mode 100644 (file)
index 0000000..8e26662
--- /dev/null
+++ b/src/robomon/hokuyoscan.h
@@ -0,0 +1,24 @@
+#ifndef HOKUYOSCAN_H
+#define HOKUYOSCAN_H
+
+#include <robottype.h>
+#include <QRectF>
+#include <QPainter>
+#include <QStyleOptionGraphicsItem>
+#include <QWidget>
+#include <QGraphicsItem>
+
+class HokuyoScan : public QGraphicsItem
+{
+public:
+    struct hokuyo_scan_type data;
+    HokuyoScan();
+    QRectF boundingRect() const;
+    void paint(QPainter *painter, const QStyleOptionGraphicsItem *option, QWidget *widget);
+    void setPosition(double x, double y, double phi);
+    void newScan(struct hokuyo_scan_type *scan);
+
+    ~HokuyoScan();
+};
+
+#endif // HOKUYOSCAN_H

diff --git a/src/robomon/robomon.pro b/src/robomon/robomon.pro
index 8208cb971ea7b4cbebe968618258926d84432381..9192ae76ef11f662c710a2b5cf03c55bc704e965 100644 (file)
--- a/src/robomon/robomon.pro
+++ b/src/robomon/robomon.pro
@@ -11,7 +11,8 @@ SOURCES += main.cpp \
            MiscGui.cpp \
            robomon_orte.cpp \
  playgroundview.cpp \
- trail.cpp
+ trail.cpp \
+ hokuyoscan.cpp
 
 TEMPLATE = app
 CONFIG += warn_on \
@@ -33,7 +34,8 @@ HEADERS += MainWindow.h \
            MiscGui.h \
            robomon_orte.h \
  playgroundview.h \
- trail.h
+ trail.h \
+ hokuyoscan.h
 
 LIBS += -lm -lpthread -lmcl -llaser-nav -lrobodim -lrobomath -lroboorte \
                -lrobottype -lorte  -lsharp -lmap -lactlib

diff --git a/src/robomon/robomon_orte.h b/src/robomon/robomon_orte.h
index 4a3aaa1a02433c7596e15566eb7dc9ed386ed80c..1337092b3fc8b4817a8aaa856af2536f2c77d301 100644 (file)
--- a/src/robomon/robomon_orte.h
+++ b/src/robomon/robomon_orte.h
@@ -36,6 +36,7 @@ enum robomon_qev {
        QEV_FSM_MAIN,
        QEV_FSM_ACT,
        QEV_FSM_MOTION,
+       QEV_HOKUYO_SCAN,
 };
 
 class OrteCallbackInfo {