From 9c8de482efe687eedb0f4059564d25fb308fa36c Mon Sep 17 00:00:00 2001
From: Fabian Paus <fabian.paus@kit.edu>
Date: Wed, 22 Mar 2017 16:55:04 +0100
Subject: [PATCH] Publish connected laser scanners and show info in GUI plugin
---
.../config/HokuyoLaserUnitApp.cfg | 2 +-
.../HokuyoLaserUnit/HokuyoLaserUnit.cpp | 69 ++++++++---------
.../drivers/HokuyoLaserUnit/HokuyoLaserUnit.h | 7 +-
.../LaserScannerPluginWidget.ui | 77 +++++++++++++++++--
.../LaserScannerPluginWidgetController.cpp | 38 +++++++--
.../LaserScannerPluginWidgetController.h | 5 +-
.../interface/units/LaserScannerUnit.ice | 16 +++-
7 files changed, 161 insertions(+), 53 deletions(-)
diff --git a/scenarios/LaserScannerTest/config/HokuyoLaserUnitApp.cfg b/scenarios/LaserScannerTest/config/HokuyoLaserUnitApp.cfg
index 814e0371a..37804992b 100644
--- a/scenarios/LaserScannerTest/config/HokuyoLaserUnitApp.cfg
+++ b/scenarios/LaserScannerTest/config/HokuyoLaserUnitApp.cfg
@@ -87,7 +87,7 @@
# - Default: ""
# - Case sensitivity: no
# - Required: no
-ArmarX.HokuyoLaserUnit.Devices = "192.168.0.11,10940"
+ArmarX.HokuyoLaserUnit.Devices = "192.168.0.11,10940,Laser Scanner A"
# ArmarX.HokuyoLaserUnit.EnableProfiling: enable profiler which is used for logging performance events
diff --git a/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.cpp b/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.cpp
index 765aabf56..a03e962ea 100644
--- a/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.cpp
+++ b/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.cpp
@@ -45,28 +45,28 @@ void HokuyoLaserUnit::onInitComponent()
devices.reserve(splitDeviceStrings.size());
for (std::string const & deviceString : splitDeviceStrings)
{
- std::vector<std::string> ipAndPort;
- boost::split(ipAndPort, deviceString, boost::is_any_of(","));
- if (ipAndPort.size() != 2)
+ std::vector<std::string> deviceInfo;
+ boost::split(deviceInfo, deviceString, boost::is_any_of(","));
+ if (deviceInfo.size() != 3)
{
ARMARX_WARNING << "Unexpected format for laser scanner device: " << deviceString
- << " (split size: " << ipAndPort.size() << ")";
+ << " (split size: " << deviceInfo.size() << ")";
continue;
}
try
{
HokuyoLaserScanDevice device;
- device.ip = ipAndPort[0];
- device.port = std::stoi(ipAndPort[1]);
+ device.ip = deviceInfo[0];
+ device.port = std::stoi(deviceInfo[1]);
+ device.frame = deviceInfo[2];
device.connected = false;
- device.isDummy = boost::starts_with(device.ip, "Dummy");
devices.push_back(device);
}
catch (std::exception const& ex)
{
ARMARX_WARNING << "Could not convert port to integer for laser scanner device " << deviceString
- << " (port is " << ipAndPort[1] << ") : " << ex.what();
+ << " (port is " << deviceInfo[1] << ") : " << ex.what();
continue;
}
}
@@ -82,12 +82,9 @@ void HokuyoLaserUnit::onConnectComponent()
task->stop();
}
+ connectedDevices.clear();
for (HokuyoLaserScanDevice & device : devices)
{
- if (device.isDummy)
- {
- continue;
- }
ARMARX_INFO << "Connecting to " << device.ip << ":" << device.port;
int ret = urg_open(&device.urg, URG_ETHERNET, device.ip.c_str(), device.port);
device.connected = (ret == 0);
@@ -98,8 +95,23 @@ void HokuyoLaserUnit::onConnectComponent()
continue;
}
+ LaserScannerInfo info;
+ info.device = device.ip;
+ info.frame = device.frame;
+ int minStep = 0, maxStep = 0;
+ urg_step_min_max(&device.urg, &minStep, &maxStep);
+ info.minAngle = urg_step2rad(&device.urg, minStep);
+ info.maxAngle = urg_step2rad(&device.urg, maxStep);
+
+
int lengthDataSize = urg_max_data_size(&device.urg);
+ info.stepSize = (info.maxAngle - info.minAngle) / (maxStep - minStep);
device.lengthData.resize(lengthDataSize);
+
+ connectedDevices.push_back(info);
+
+ ARMARX_INFO << "Connected to " << device.ip << ", " << info.frame << ", "
+ << info.minAngle << ", " << info.maxAngle << ", " << info.stepSize;
}
task = new PeriodicTask<HokuyoLaserUnit>(this, &HokuyoLaserUnit::updateScanData, updatePeriod, false, "HokuyoLaserScanUpdate");
@@ -141,33 +153,17 @@ std::string HokuyoLaserUnit::getReportTopicName(const Ice::Current& c) const
return topicName;
}
+LaserScannerInfoSeq HokuyoLaserUnit::getConnectedDevices(const Ice::Current& c) const
+{
+ return connectedDevices;
+}
+
void HokuyoLaserUnit::updateScanData()
{
LaserScan scan;
TimestampVariantPtr now(new TimestampVariant(TimeUtil::GetTime()));
for (HokuyoLaserScanDevice & device : devices)
{
- 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);
- for (int stepIndex = 0; stepIndex < lengthDataSize; ++stepIndex)
- {
- LaserScanStep step;
- step.angle = angleOffset + float(0.25 * M_PI / 180.0 * stepIndex);
- 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;
- }
if (device.connected)
{
int ret = urg_start_measurement(&device.urg, URG_DISTANCE, 1, 0);
@@ -192,17 +188,16 @@ void HokuyoLaserUnit::updateScanData()
{
LaserScanStep step;
long distance = device.lengthData[stepIndex];
- // TODO: Extract the min and max valid value for distance into parameters
+ // TODO: Extract the min and max valid value for distance into parameters?
if (distance >= 21 && distance <= 30000)
{
- step.angle = angleOffset + (float)urg_step2rad(&device.urg, stepIndex); // Convert steps to rad
+ step.angle = angleOffset + (float)urg_index2rad(&device.urg, stepIndex); // Convert steps to rad
step.distance = (float)distance; // Data is already in mm
scan.push_back(step);
}
}
- // TODO: Better names for the devices?
- topic->reportSensorValues(device.ip, device.ip, scan, now);
+ topic->reportSensorValues(device.ip, device.frame, scan, now);
}
}
}
diff --git a/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.h b/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.h
index 2445d3a03..d40262623 100644
--- a/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.h
+++ b/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.h
@@ -47,7 +47,7 @@ namespace armarx
defineOptionalProperty<std::string>("LaserScannerTopicName", "LaserScans", "Name of the laser scan topic.");
defineOptionalProperty<int>("UpdatePeriod", 25, "Update period for laser scans");
defineOptionalProperty<float>("AngleOffset", -2.3561944902, "Offset is applied the raw angles before reporting them");
- defineOptionalProperty<std::string>("Devices", "", "List of devices in form of 'IP1,port1;IP2,port2;...'");
+ defineOptionalProperty<std::string>("Devices", "", "List of devices in form of 'IP1,port1,frame1;IP2,port2,frame2;...'");
}
};
@@ -55,8 +55,8 @@ namespace armarx
{
std::string ip;
int port = 0;
+ std::string frame;
bool connected = false;
- bool isDummy = false;
urg_t urg;
std::vector<long> lengthData;
};
@@ -113,6 +113,8 @@ namespace armarx
std::string getReportTopicName(const Ice::Current& c) const override;
+ LaserScannerInfoSeq getConnectedDevices(const Ice::Current& c) const override;
+
private:
void updateScanData();
@@ -123,6 +125,7 @@ namespace armarx
float angleOffset = 0.0f;
std::vector<HokuyoLaserScanDevice> devices;
PeriodicTask<HokuyoLaserUnit>::pointer_type task;
+ LaserScannerInfoSeq connectedDevices;
};
}
diff --git a/source/RobotAPI/gui-plugins/LaserScannerPlugin/LaserScannerPluginWidget.ui b/source/RobotAPI/gui-plugins/LaserScannerPlugin/LaserScannerPluginWidget.ui
index 097f7d74a..eac4cfc57 100644
--- a/source/RobotAPI/gui-plugins/LaserScannerPlugin/LaserScannerPluginWidget.ui
+++ b/source/RobotAPI/gui-plugins/LaserScannerPlugin/LaserScannerPluginWidget.ui
@@ -18,11 +18,8 @@
<enum>QLayout::SetMinAndMaxSize</enum>
</property>
<item>
- <layout class="QHBoxLayout" name="horizontalLayout" stretch="0,0">
- <property name="spacing">
- <number>6</number>
- </property>
- <item>
+ <layout class="QGridLayout" name="gridLayout">
+ <item row="0" column="0">
<widget class="QLabel" name="label">
<property name="sizePolicy">
<sizepolicy hsizetype="Ignored" vsizetype="Preferred">
@@ -38,9 +35,77 @@
</property>
</widget>
</item>
- <item>
+ <item row="0" column="1">
<widget class="QComboBox" name="deviceComboBox"/>
</item>
+ <item row="1" column="0">
+ <widget class="QLabel" name="label_2">
+ <property name="text">
+ <string>Frame:</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="3" column="0">
+ <widget class="QLabel" name="label_4">
+ <property name="text">
+ <string>Maximum Angle (rad)</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="0">
+ <widget class="QLabel" name="label_3">
+ <property name="text">
+ <string>Minimum Angle (rad):</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="4" column="0">
+ <widget class="QLabel" name="label_5">
+ <property name="text">
+ <string>Step Size (rad):</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="1">
+ <widget class="QLabel" name="frameLabel">
+ <property name="text">
+ <string>-</string>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="1">
+ <widget class="QLabel" name="minAngleLabel">
+ <property name="text">
+ <string>-</string>
+ </property>
+ </widget>
+ </item>
+ <item row="3" column="1">
+ <widget class="QLabel" name="maxAngleLabel">
+ <property name="text">
+ <string>-</string>
+ </property>
+ </widget>
+ </item>
+ <item row="4" column="1">
+ <widget class="QLabel" name="stepSizeLabel">
+ <property name="text">
+ <string>-</string>
+ </property>
+ </widget>
+ </item>
</layout>
</item>
<item>
diff --git a/source/RobotAPI/gui-plugins/LaserScannerPlugin/LaserScannerPluginWidgetController.cpp b/source/RobotAPI/gui-plugins/LaserScannerPlugin/LaserScannerPluginWidgetController.cpp
index cc3030eaf..036f956a4 100644
--- a/source/RobotAPI/gui-plugins/LaserScannerPlugin/LaserScannerPluginWidgetController.cpp
+++ b/source/RobotAPI/gui-plugins/LaserScannerPlugin/LaserScannerPluginWidgetController.cpp
@@ -59,6 +59,7 @@ void LaserScannerPluginWidgetController::onInitComponent()
usingProxy(laserScannerUnitName);
connect(this, SIGNAL(newSensorValuesReported()), this, SLOT(onNewSensorValuesReported()), Qt::QueuedConnection);
+ connect(widget.deviceComboBox, SIGNAL(currentIndexChanged(int)), this, SLOT(onDeviceSelected(int)));
}
@@ -67,6 +68,8 @@ void LaserScannerPluginWidgetController::onConnectComponent()
laserScannerUnit = getProxy<LaserScannerUnitInterfacePrx>(laserScannerUnitName);
std::string topicName = laserScannerUnit->getReportTopicName();
usingTopic(topicName);
+
+ laserScanners = laserScannerUnit->getConnectedDevices();
}
QPointer<QDialog> LaserScannerPluginWidgetController::getConfigDialog(QWidget* parent)
@@ -102,8 +105,6 @@ void LaserScannerPluginWidgetController::reportSensorValues(const std::string& d
void LaserScannerPluginWidgetController::onNewSensorValuesReported()
{
- // TODO: Draw something on the canvas
-
QComboBox* deviceBox = widget.deviceComboBox;
boost::mutex::scoped_lock lock(scanMutex);
@@ -117,6 +118,14 @@ void LaserScannerPluginWidgetController::onNewSensorValuesReported()
}
std::string deviceName(deviceBox->currentText().toUtf8().data());
+ float stepSize = 0.25f;
+ for (LaserScannerInfo const & scanner : laserScanners)
+ {
+ if (scanner.device == deviceName)
+ {
+ stepSize = scanner.stepSize;
+ }
+ }
QGraphicsView* view = widget.graphicsView;
@@ -134,9 +143,8 @@ void LaserScannerPluginWidgetController::onNewSensorValuesReported()
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);
+ item->setStartAngle(std::round(16.0f * angle * 180.0 / M_PI) + 90 * 16);
+ item->setSpanAngle(std::round(stepSize * 16.0f * 180.0 / M_PI));
};
LaserScan& scan = scans[deviceName];
@@ -147,3 +155,23 @@ void LaserScannerPluginWidgetController::onNewSensorValuesReported()
view->fitInView(scene.itemsBoundingRect(), Qt::KeepAspectRatio);
}
+
+void LaserScannerPluginWidgetController::onDeviceSelected(int index)
+{
+ if (index < 0 && index >= widget.deviceComboBox->count())
+ {
+ return;
+ }
+ std::string deviceName = widget.deviceComboBox->itemText(index).toStdString();
+ for (LaserScannerInfo const & scanner : laserScanners)
+ {
+ if (scanner.device == deviceName)
+ {
+ widget.frameLabel->setText(QString::fromStdString(scanner.frame));
+ widget.minAngleLabel->setText(QString::number(scanner.minAngle));
+ widget.maxAngleLabel->setText(QString::number(scanner.minAngle));
+ widget.stepSizeLabel->setText(QString::number(scanner.stepSize));
+ }
+ }
+
+}
diff --git a/source/RobotAPI/gui-plugins/LaserScannerPlugin/LaserScannerPluginWidgetController.h b/source/RobotAPI/gui-plugins/LaserScannerPlugin/LaserScannerPluginWidgetController.h
index 7d35abcbf..a98917b3e 100644
--- a/source/RobotAPI/gui-plugins/LaserScannerPlugin/LaserScannerPluginWidgetController.h
+++ b/source/RobotAPI/gui-plugins/LaserScannerPlugin/LaserScannerPluginWidgetController.h
@@ -23,7 +23,7 @@
#ifndef _ARMARX_RobotAPI_LaserScannerPlugin_WidgetController_H
#define _ARMARX_RobotAPI_LaserScannerPlugin_WidgetController_H
-#include "ui_LaserScannerPluginWidget.h"
+#include <RobotAPI/gui-plugins/LaserScannerPlugin/ui_LaserScannerPluginWidget.h>
#include <ArmarXCore/core/system/ImportExportComponent.h>
#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
@@ -107,6 +107,7 @@ namespace armarx
const Ice::Current& c) override;
public slots:
void onNewSensorValuesReported();
+ void onDeviceSelected(int index);
signals:
void newSensorValuesReported();
@@ -120,10 +121,12 @@ namespace armarx
std::string laserScannerUnitName;
LaserScannerUnitInterfacePrx laserScannerUnit;
+ LaserScannerInfoSeq laserScanners;
Mutex scanMutex;
std::unordered_map<std::string, LaserScan> scans;
+
QGraphicsScene scene;
};
}
diff --git a/source/RobotAPI/interface/units/LaserScannerUnit.ice b/source/RobotAPI/interface/units/LaserScannerUnit.ice
index 113d5331f..f022260d0 100644
--- a/source/RobotAPI/interface/units/LaserScannerUnit.ice
+++ b/source/RobotAPI/interface/units/LaserScannerUnit.ice
@@ -45,17 +45,31 @@ module armarx
* @param angle Angle in which direction a distance was measured [rad].
* @param distance The measured distance [mm].
**/
- struct LaserScanStep {
+ struct LaserScanStep
+ {
float angle;
float distance;
};
+ struct LaserScannerInfo
+ {
+ string device;
+ string frame;
+ float minAngle;
+ float maxAngle;
+ float stepSize;
+ };
+
sequence<LaserScanStep> LaserScan;
+ sequence<LaserScannerInfo> LaserScannerInfoSeq;
interface LaserScannerUnitInterface extends armarx::SensorActorUnitInterface
{
["cpp:const"]
idempotent string getReportTopicName() throws NotInitializedException;
+
+ ["cpp:const"]
+ LaserScannerInfoSeq getConnectedDevices();
};
interface LaserScannerUnitListener
--
GitLab