From 4c43f7c66b78b32df76c054dcae828c128e8822f Mon Sep 17 00:00:00 2001
From: Fabian Paus <fabian.paus@kit.edu>
Date: Wed, 8 Mar 2017 16:43:35 +0100
Subject: [PATCH] Hokuyo: Improved visualisation
---
.../HokuyoLaserUnit/HokuyoLaserUnit.cpp | 6 +++-
.../LaserScannerPluginWidgetController.cpp | 30 +++++++++++++------
2 files changed, 26 insertions(+), 10 deletions(-)
diff --git a/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.cpp b/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.cpp
index fc78cf268..fca068ee6 100644
--- a/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.cpp
+++ b/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;
}
diff --git a/source/RobotAPI/gui-plugins/LaserScannerPlugin/LaserScannerPluginWidgetController.cpp b/source/RobotAPI/gui-plugins/LaserScannerPlugin/LaserScannerPluginWidgetController.cpp
index 233f94be9..2327635b3 100644
--- a/source/RobotAPI/gui-plugins/LaserScannerPlugin/LaserScannerPluginWidgetController.cpp
+++ b/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 ×tamp, 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);
}
--
GitLab