diff --git a/scenarios/SickLaserUnitTest/config/SickLaserUnit.cfg b/scenarios/SickLaserUnitTest/config/SickLaserUnit.cfg
index bac8b18e34e4899622d2b49608a42794af7e9fae..5ff571ebdb58c6f447e14154934e7b4cac213a35 100644
--- a/scenarios/SickLaserUnitTest/config/SickLaserUnit.cfg
+++ b/scenarios/SickLaserUnitTest/config/SickLaserUnit.cfg
@@ -151,26 +151,18 @@
ArmarX.SickLaserUnit.angleOffset = 0.0
-# ArmarX.SickLaserUnit.deviceNumber: number of the LaserScanner Device
+# ArmarX.SickLaserUnit.devices: List of Devices in format frame1,ip1,port1;frame2,ip2,port2
# Attributes:
-# - Default: 0
+# - Default: LaserScannerFront,192.168.8.133,2112
# - Case sensitivity: yes
# - Required: no
-# ArmarX.SickLaserUnit.deviceNumber = 0
-
-
-# ArmarX.SickLaserUnit.devices: No Description
-# Attributes:
-# - Default: Device1; Scanner2
-# - Case sensitivity: no
-# - Required: no
-ArmarX.SickLaserUnit.devices = Device1; Scanner2
+ArmarX.SickLaserUnit.devices = LaserScannerFront,192.168.8.133,2112
-# ArmarX.SickLaserUnit.hostname: Hostname of the LaserScanner
+# ArmarX.SickLaserUnit.hostname: No Description
# Attributes:
# - Default: 192.168.8.133
-# - Case sensitivity: yes
+# - Case sensitivity: no
# - Required: no
ArmarX.SickLaserUnit.hostname = 192.168.8.133
@@ -183,10 +175,10 @@ ArmarX.SickLaserUnit.hostname = 192.168.8.133
ArmarX.SickLaserUnit.laserScannerTopicName = "SickTopic"
-# ArmarX.SickLaserUnit.port: port to use on the LaserScanner
+# ArmarX.SickLaserUnit.port: No Description
# Attributes:
# - Default: 2112
-# - Case sensitivity: yes
+# - Case sensitivity: no
# - Required: no
ArmarX.SickLaserUnit.port = 2112
@@ -222,15 +214,6 @@ ArmarX.SickLaserUnit.rangeMin = 0.01
ArmarX.SickLaserUnit.scannerType = sick_tim_5xx
-# ArmarX.SickLaserUnit.subscribeDatagram: subscribe to Datagram in communication or not
-# Attributes:
-# - Default: false
-# - Case sensitivity: yes
-# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.SickLaserUnit.subscribeDatagram = false
-
-
# ArmarX.SickLaserUnit.timeIncrement: timeIncrement??
# Attributes:
# - Default: 0.10000000000000001
@@ -247,19 +230,20 @@ ArmarX.SickLaserUnit.timeIncrement = 0.1
# ArmarX.SickLaserUnit.timelimit = 5
-# ArmarX.SickLaserUnit.topicName: Name of topic.
+# ArmarX.SickLaserUnit.topicName: Name of the topic
# Attributes:
+# - Default: SICKLaserScanner
# - Case sensitivity: yes
-# - Required: yes
+# - Required: no
ArmarX.SickLaserUnit.topicName = "myLaserTopic1"
# ArmarX.SickLaserUnit.tpc.sub.LaserScannerUnit: Name of the `LaserScannerUnit` topic to subscribe to.
# Attributes:
-# - Default: LaserScannerUnit
+# - Default: SICKLaserScanner
# - Case sensitivity: yes
# - Required: no
-# ArmarX.SickLaserUnit.tpc.sub.LaserScannerUnit = LaserScannerUnit
+# ArmarX.SickLaserUnit.tpc.sub.LaserScannerUnit = SICKLaserScanner
# ArmarX.SickLaserUnit.updatePeriod: No Description
diff --git a/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.cpp b/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.cpp
index 2e852005c897449b67185c19fecb109484c1709f..6c84b806ffc36dca5a6e981da5d19a2a67f329d2 100644
--- a/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.cpp
+++ b/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.cpp
@@ -123,9 +123,7 @@ namespace armarx
def->optional(properties.topicName, "topicName", "Name of the topic");
//Scanner parameters
- def->optional(properties.frameName, "frameName", "Name of scanner frame.");
- def->optional(properties.hostname, "hostname", "Hostname of the LaserScanner");
- def->optional(properties.port, "port", "port to use on the LaserScanner");
+ def->optional(properties.devices, "devices", "List of Devices in format frame1,ip1,port1;frame2,ip2,port2");
def->optional(properties.timelimit, "timelimit", "timelimit for communication");
def->required(properties.scannerType, "scannerType", "Name of the LaserScanner");
def->optional(properties.angleOffset, "angleOffset", "offset to the scanning angle");
@@ -145,38 +143,50 @@ namespace armarx
ARMARX_INFO_S << "initializing SickLaserUnit.";
- scanDevice.scanTopic = topic;
- scanDevice.isSensorInitialized = false;
- if (properties.hostname != "")
+ std::vector<std::string> splitDeviceStrings = Split(properties.devices, ";");
+ scanDevices.clear();
+ scanDevices.reserve(splitDeviceStrings.size());
+ for (std::string const& deviceString : splitDeviceStrings)
{
- scanDevice.useTcp = true;
- scanDevice.ip = properties.hostname;
- }
- scanDevice.port = properties.port;
- scanDevice.frameName = properties.frameName;
- scanDevice.angleOffset = properties.angleOffset;
- scanDevice.timelimit = properties.timelimit;
- scanDevice.scannerType = properties.scannerType;
- scanDevice.rangeMin = properties.rangeMin;
- scanDevice.rangeMax = properties.rangeMax;
- scanDevice.timeIncrement = properties.timeIncrement;
- //scanner Parameters
- try
- {
- scanDevice.parser = new sick_scan::SickGenericParser(scanDevice.scannerType);
- scanDevice.parser->set_range_min(properties.rangeMin);
- scanDevice.parser->set_range_max(properties.rangeMax);
- scanDevice.parser->set_time_increment(properties.timeIncrement);
- }
- catch (std::exception const& e)
- {
- ARMARX_ERROR_S << "Could not create parser. Wrong Scanner name.";
- return;
+ std::vector<std::string> deviceInfo = Split(deviceString, ",");
+ if (deviceInfo.size() != 3)
+ {
+ ARMARX_WARNING << "Unexpected format for laser scanner device: " << deviceString
+ << " (split size: " << deviceInfo.size() << ")";
+ continue;
+ }
+ SickLaserScanDevice& device = scanDevices.emplace_back();
+ device.scanTopic = topic;
+ device.isSensorInitialized = false;
+ device.frameName = deviceInfo[0];
+ if (deviceInfo[1] != "")
+ {
+ device.useTcp = true;
+ device.ip = deviceInfo[1];
+ }
+ device.port = deviceInfo[2];
+ device.angleOffset = properties.angleOffset;
+ device.timelimit = properties.timelimit;
+ device.scannerType = properties.scannerType;
+ device.rangeMin = properties.rangeMin;
+ device.rangeMax = properties.rangeMax;
+ device.timeIncrement = properties.timeIncrement;
+ //scanner Parameters
+ try
+ {
+ device.parser = new sick_scan::SickGenericParser(device.scannerType);
+ device.parser->set_range_min(properties.rangeMin);
+ device.parser->set_range_max(properties.rangeMax);
+ device.parser->set_time_increment(properties.timeIncrement);
+ }
+ catch (std::exception const& e)
+ {
+ ARMARX_ERROR_S << "Could not create parser. Wrong Scanner name.";
+ return;
+ }
+ device.parser->getCurrentParamPtr()->setUseBinaryProtocol(false);
+ device.colaDialectId = 'A';
}
- scanDevice.parser->getCurrentParamPtr()->setUseBinaryProtocol(false);
- scanDevice.colaDialectId = 'A';
- ARMARX_INFO_S << "ASCII protocol activated.";
- ARMARX_INFO_S << "SickLaserUnit initialisation complete.";
//addPlugin(heartbeat);
//configureHeartbeatChannel();
@@ -186,20 +196,20 @@ namespace armarx
{
topic = getTopic<LaserScannerUnitListenerPrx>(properties.topicName);
offeringTopic(properties.topicName);
- scanDevice.scanTopic = topic;
-
- ARMARX_INFO_S << "Connecting.";
- //start the laser scanner
- if (scanDevice.task)
+ for (SickLaserScanDevice& device : scanDevices)
{
- scanDevice.task->stop();
- scanDevice.task = nullptr;
+ device.scanTopic = topic;
+ //start the laser scanner
+ if (device.task)
+ {
+ device.task->stop();
+ device.task = nullptr;
+ }
+ device.runState = RunState::scannerInit;
+ device.task = new RunningTask<SickLaserScanDevice>(&device, &SickLaserScanDevice::run, "SickLaserScanUpdate_" + device.ip);
+ device.task->start();
}
- scanDevice.runState = RunState::scannerInit;
- scanDevice.task = new RunningTask<SickLaserScanDevice>(&scanDevice, &SickLaserScanDevice::run, "SickLaserScanUpdate_" + scanDevice.ip);
- scanDevice.task->start();
-
// Do things after connecting to topics and components.
/* (Requies the armarx::DebugObserverComponentPluginUser.)
@@ -231,18 +241,21 @@ namespace armarx
void SickLaserUnit::onDisconnectComponent()
{
ARMARX_INFO_S << "Disconnecting LaserScanner.";
- if (scanDevice.task)
- {
- scanDevice.task->stop();
- scanDevice.task = nullptr;
- }
- if (scanDevice.scanner)
- {
- delete scanDevice.scanner;
- }
- if (scanDevice.parser)
+ for (SickLaserScanDevice& device : scanDevices)
{
- delete scanDevice.parser;
+ if (device.task)
+ {
+ device.task->stop();
+ device.task = nullptr;
+ }
+ if (device.scanner)
+ {
+ delete device.scanner;
+ }
+ if (device.parser)
+ {
+ delete device.parser;
+ }
}
}
diff --git a/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.h b/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.h
index 388285ae8314bf45829316c1eafa24696a9a0e6e..69d6385c130969a4d18a6d104f91765ec4d9a876 100644
--- a/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.h
+++ b/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.h
@@ -133,9 +133,7 @@ namespace armarx
{
std::string topicName = "SICKLaserScanner";
//scanner parameters
- std::string hostname = "192.168.8.133";
- std::string port = "2112";
- std::string frameName = "LaserScannerFront";
+ std::string devices = "LaserScannerFront,192.168.8.133,2112";
int timelimit = 5;
std::string scannerType = "sick_tim_5xx";
double angleOffset = 0.0;
@@ -144,7 +142,7 @@ namespace armarx
double timeIncrement = 0.1;
};
Properties properties;
- SickLaserScanDevice scanDevice;
+ std::vector<SickLaserScanDevice> scanDevices;
LaserScannerUnitListenerPrx topic;
//HeartbeatComponentPlugin heartbeat;