Skip to content
Snippets Groups Projects
Commit 09348bde authored by Johann Mantel's avatar Johann Mantel
Browse files

use multiple LaserScanner

parent 05bb94b0
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
scenarios/SickLaserUnitTest/config/SickLaserUnit.cfg
+ 12
− 28
View file @ 09348bde
......@@ -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
+ 68
− 55
View file @ 09348bde
......@@ -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
+ 2
− 4
View file @ 09348bde
......@@ -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;
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment