Hokuyo: Improved visualisation

source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.cpp

@@ -147,6 +147,9 @@ void HokuyoLaserUnit::updateScanData()
     {
         if (device.isDummy)
         {
+            static std::mt19937 engine;
+            std::uniform_real_distribution<float> dist(0.0f, 500.0f);
+
             int lengthDataSize = 1081;
             scan.clear();
             scan.reserve(lengthDataSize);
@@ -154,11 +157,12 @@ void HokuyoLaserUnit::updateScanData()
             {
                 LaserScanStep step;
                 step.angle = angleOffset + float(0.25 * M_PI / 180.0 * stepIndex);
-                step.distance = lengthDataSize * stepIndex / 100.0f + 200.0f;
+                step.distance = stepIndex * 25000.0f / lengthDataSize + dist(engine);
                 scan.push_back(step);
             }
 
             // TODO: Better names for the devices?
+            ARMARX_INFO << deactivateSpam(1.0) << "Reporting new laser scan values";
             topic->reportSensorValues(device.ip, device.ip, scan, now);
             continue;
         }

source/RobotAPI/gui-plugins/LaserScannerPlugin/LaserScannerPluginWidgetController.cpp

@@ -69,7 +69,7 @@ void LaserScannerPluginWidgetController::onConnectComponent()
     usingTopic(topicName);
 }
 
-QPointer<QDialog> LaserScannerPluginWidgetController::getConfigDialog(QWidget *parent)
+QPointer<QDialog> LaserScannerPluginWidgetController::getConfigDialog(QWidget* parent)
 {
     if (!dialog)
     {
@@ -87,7 +87,7 @@ void LaserScannerPluginWidgetController::configured()
     }
 }
 
-void LaserScannerPluginWidgetController::reportSensorValues(const std::string &device, const std::string &name, const LaserScan &newScan, const TimestampBasePtr &timestamp, const Ice::Current &c)
+void LaserScannerPluginWidgetController::reportSensorValues(const std::string& device, const std::string& name, const LaserScan& newScan, const TimestampBasePtr& timestamp, const Ice::Current& c)
 {
     {
         boost::mutex::scoped_lock lock(scanMutex);
@@ -97,6 +97,7 @@ void LaserScannerPluginWidgetController::reportSensorValues(const std::string &d
         scan = newScan;
     }
 
+    ARMARX_INFO << deactivateSpam(1) << "Got new laser scan values";
     emit newSensorValuesReported();
 }
 
@@ -107,7 +108,7 @@ void LaserScannerPluginWidgetController::onNewSensorValuesReported()
     QComboBox* deviceBox = widget.deviceComboBox;
 
     boost::mutex::scoped_lock lock(scanMutex);
-    for (auto& pair : scans)
+    for (auto & pair : scans)
     {
         QString deviceName(QString::fromStdString(pair.first.c_str()));
         if (deviceBox->findText(deviceName) < 0)
@@ -119,17 +120,28 @@ void LaserScannerPluginWidgetController::onNewSensorValuesReported()
     std::string deviceName(deviceBox->currentText().toUtf8().data());
 
     QGraphicsView* view = widget.graphicsView;
-    int outerR =std::min(view->width() / 2, view->height() / 2);
-    int r = outerR - 10;
 
 
     scene.clear();
-    scene.addEllipse(-outerR, -outerR, 2*outerR,2*outerR, QPen(QColor(255,255,255)));
-    scene.addEllipse(-r, -r, 2*r,2*r, QPen(QColor(200,200,200)));
-    auto line = [&](float angle, float d){ scene.addLine(0, 0, std::cos(angle) * d * r, std::sin(angle) * d * r); };
+    int outerR = std::min(view->width() / 2, view->height() / 2);
+    scene.addEllipse(-outerR, -outerR, 2 * outerR, 2 * outerR, QPen(QColor(255, 255, 255)));
+    int r = outerR - 10;
+    scene.addEllipse(-r, -r, 2 * r, 2 * r, QPen(QColor(200, 200, 200)));
+    QColor stepColor(QColor::fromRgb(100, 100, 255));
+    QPen stepPen(stepColor);
+    QBrush stepBrush(stepColor);
+    auto line = [&](float angle, float d)
+    {
+        float di = d * r;
+        QGraphicsEllipseItem* item = scene.addEllipse(-di, -di, 2 * di, 2 * di, stepPen, stepBrush);
+        // Angles for Qt ellipse are in 16th of degree (who thought that would be a great idea?)
+        item->setStartAngle(std::round(16.0f * angle * 180.0 / M_PI));
+        item->setSpanAngle(std::round(0.25f * 16.0f));
+        //scene.addLine(0, 0, std::sin(angle) * d * r, std::cos(angle) * d * r);
+    };
 
     LaserScan& scan = scans[deviceName];
-    for (LaserScanStep& step : scan)
+    for (LaserScanStep & step : scan)
     {
         line(step.angle, step.distance / 30000.0f);
     }