use multiple LaserScanner

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

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;
+            }
         }
     }
 

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;