From ca18b0b91507c43b2f5892cf0fdead473e0c0468 Mon Sep 17 00:00:00 2001
From: Johann Mantel <j-mantel@gmx.net>
Date: Fri, 25 Jun 2021 12:42:14 +0200
Subject: [PATCH] Add SickLaserScanDevice struct and methods to initialize and
run the scanner
---
.../drivers/SickLaserUnit/SickLaserUnit.cpp | 231 ++++++++++--------
.../drivers/SickLaserUnit/SickLaserUnit.h | 85 ++++---
2 files changed, 180 insertions(+), 136 deletions(-)
diff --git a/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.cpp b/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.cpp
index b99a4169a..184e7f23d 100644
--- a/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.cpp
+++ b/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.cpp
@@ -31,22 +31,93 @@
namespace armarx
{
- std::string protocolToString(SickLaserUnit::Protocol protocol)
+ std::string protocolToString(ScanProtocol protocol)
{
std::string protocolStr;
-
switch (protocol)
{
- case SickLaserUnit::Protocol::ASCII:
+ case ScanProtocol::ASCII:
protocolStr = "ASCII";
break;
- case SickLaserUnit::Protocol::Binary:
+ case ScanProtocol::Binary:
protocolStr = "Binary";
break;
}
- return modeStr;
+ return protocolStr;
+ }
+
+
+ void SickLaserScanDevice::run()
+ {
+ while (!task->isStopped())
+ {
+ switch (runState)
+ {
+ case RunState::scannerInit:
+ initScanner();
+ break;
+ case RunState::scannerRun:
+ if (result == sick_scan::ExitSuccess) // OK -> loop again
+ {
+ result = scanner->loopOnce();
+ }
+ else
+ {
+ runState = RunState::scannerFinalize;
+ }
+ break;
+ case RunState::scannerFinalize:
+ break;
+ default:
+ ARMARX_ERROR_S << "Invalid run state in task loop";
+ break;
+ }
+ }
}
+ void SickLaserScanDevice::initScanner()
+ {
+ this->isSensorInitialized = false;
+ ARMARX_INFO_S << "Start initialising scanner [Ip: " << this->ip
+ << "] [Port: " << this->port << "]";
+ // attempt to connect/reconnect
+ delete this->scanner; // disconnect scanner
+ if (this->useTcp)
+ {
+ this->scanner = new sick_scan::SickScanCommonTcp(this->ip, this->port, this->timelimit, this->parser, this->colaDialectId);
+ }
+ else
+ {
+ ARMARX_ERROR_S << "TCP is not switched on. Probably hostname or port not set.\n";
+ return;
+ }
+
+ if (this->emulSensor)
+ {
+ this->scanner->setEmulSensor(true);
+ }
+ result = this->scanner->init();
+ this->isSensorInitialized = true;
+
+ if (result == sick_scan::ExitSuccess) // OK -> loop again
+ {
+ if (this->changeIP)
+ {
+ this->runState = RunState::scannerFinalize;
+ }
+ else
+ {
+ this->runState = RunState::scannerRun; // after initialising switch to run state
+ }
+
+ }
+ else
+ {
+ this->runState = RunState::scannerInit; // If there was an error, try to restart scanner
+ }
+ }
+
+
armarx::PropertyDefinitionsPtr SickLaserUnit::createPropertyDefinitions()
{
armarx::PropertyDefinitionsPtr def =
@@ -67,8 +138,8 @@ namespace armarx
def->optional(properties.timelimit, "timelimit", "timelimit for communication");
def->optional(properties.subscribeDatagram, "subscribeDatagram", "subscribe to Datagram in communication or not");
def->optional(properties.protocol, "protocol", "Either use ASCII or Binary protocol")
- .map(protocolToString(Protocol::ASCII), Protocol::ASCII)
- .map(protocolToString(Protocol::Binary), Protocol::Binary);
+ .map(protocolToString(ScanProtocol::ASCII), ScanProtocol::ASCII)
+ .map(protocolToString(ScanProtocol::Binary), ScanProtocol::Binary);
def->optional(properties.sopasProtocolType, "sopasProtocolType", "Automatically set to true if the Scanner does not support ASCII communication");
//Scanner parameters
def->required(properties.scannerType, "scannerType", "Name of the LaserScanner");
@@ -92,65 +163,78 @@ namespace armarx
ARMARX_INFO_S << "SickLaserUnit sagt Hallo Welt!";
+ scanDevice.isSensorInitialized = false;
if (properties.hostname != "")
{
- useTCP = true;
+ scanDevice.useTcp = true;
+ scanDevice.ip = properties.hostname;
}
- if (sNewIp != "")
+ if (properties.newIpAddress != "")
{
- changeIP = true;
+ scanDevice.changeIP = true;
+ scanDevice.newIpAddress = properties.newIpAddress;
}
+ scanDevice.scannerType = properties.scannerType;
//scanner Parameters
- parser = new sick_scan::SickGenericParser(properties.scannerType);
- parser->set_range_min(properties.rangeMin);
- parser->set_range_max(properties.rangeMax);
- parser->set_time_increment(properties.timeIncrement);
- char colaDialectId = 'A'; // A or B (Ascii or Binary)
+ 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);
if (properties.emulSensor)
{
ARMARX_INFO_S << "Found paraemter emulSensor overwriting default settings. Emulation: True";
+ scanDevice.emulSensor = true;
}
switch (properties.protocol)
{
- case Protocol::ASCII:
- if (parser->getCurrentParamPtr()->getNumberOfLayers() > 4)
+ case ScanProtocol::ASCII:
+ if (scanDevice.parser->getCurrentParamPtr()->getNumberOfLayers() > 4)
{
- ARMARX_WARNING(
- "This scanner type does not support ASCII communication.\n"
- "Binary communication has been activated.\n"
- "The parameter \"sopasProtocolType\" has been set to \"true\".");
- properties.sopasProtocolType = true;
- properties.protocol = Protocol::Binary;
+ ARMARX_WARNING_S << "This scanner type does not support ASCII communication.\n"
+ << "Binary communication has been activated.\n"
+ << "The parameter \"sopasProtocolType\" has been set to \"true\".";
+ scanDevice.sopasProtocolType = true;
+ scanDevice.protocol = ScanProtocol::Binary;
}
else
{
ARMARX_INFO_S << "ASCII protocol activated";
+ scanDevice.protocol = ScanProtocol::ASCII;
}
break;
- case Protocol::Binary:
+ case ScanProtocol::Binary:
ARMARX_INFO_S << "Binary protocol activated";
+ scanDevice.protocol = ScanProtocol::Binary;
break;
default:
ARMARX_WARNING_S << "Unknown protocol type. Defaulting to Binary protocol.";
- properties.protocol = Protocol::Binary;
+ scanDevice.protocol = ScanProtocol::Binary;
}
- if (properties.protocol == Protocol::ASCII)
+ if (scanDevice.protocol == ScanProtocol::ASCII)
{
- parser->getCurrentParamPtr()->setUseBinaryProtocol(false);
- colaDialectId = 'A';
+ scanDevice.parser->getCurrentParamPtr()->setUseBinaryProtocol(false);
+ scanDevice.colaDialectId = 'A';
}
else
{
- parser->getCurrentParamPtr()->setUseBinaryProtocol(true);
- colaDialectId = 'B';
+ scanDevice.parser->getCurrentParamPtr()->setUseBinaryProtocol(true);
+ scanDevice.colaDialectId = 'B';
}
- isSensorInitialized = false;
}
void SickLaserUnit::onConnectComponent()
{
- initScanner();
+ //start the laser scanner
+ if (scanDevice.task)
+ {
+ scanDevice.task->stop();
+ scanDevice.task = nullptr;
+ }
+ 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.)
@@ -179,44 +263,21 @@ namespace armarx
*/
}
- void SickLaserUnit::run()
- {
- while (!task->isStopped())
- {
- switch (runState)
- {
- case RunState::scannerInit:
- initScanner();
- break;
- case RunState::scannerRun:
- if (result == sick_scan::ExitSuccess) // OK -> loop again
- {
- result = scanner->loopOnce();
- }
- else
- {
- runState = RunState::scannerFinalize;
- }
- break;
- case RunState::scannerFinalize:
- break;
- default:
- ARMARX_ERROR_S << "Invalid run state in mail loop";
- break;
- }
- }
- }
-
void SickLaserUnit::onDisconnectComponent()
{
ARMARX_INFO_S << "Disconnecting LaserScanner.";
- if (scanner != NULL)
+ if (scanDevice.task)
+ {
+ scanDevice.task->stop();
+ scanDevice.task = nullptr;
+ }
+ if (scanDevice.scanner)
{
- delete scanner;
+ delete scanDevice.scanner;
}
- if (parser != NULL)
+ if (scanDevice.parser)
{
- delete parser;
+ delete scanDevice.parser;
}
}
@@ -227,48 +288,6 @@ namespace armarx
return "SickLaserUnit";
}
- void SickLaserUnit::initScanner()
- {
- isSensorInitialized = false;
- ARMARX_INFO("Start initialising scanner [Ip: %s] [Port: %s]", properties.hostname.c_str(),
- properties.port.c_str());
- // attempt to connect/reconnect
- delete scanner; // disconnect scanner
- if (useTCP)
- {
- scanner = new sick_scan::SickScanCommonTcp(properties.hostname, properties.port, properties.timelimit, parser,
- colaDialectId);
- }
- else
- {
- ARMARX_ERROR("TCP is not switched on. Probably hostname or port not set.\n");
- exit();
- }
-
- if (emulSensor)
- {
- scanner->setEmulSensor(true);
- }
- result = scanner->init();
- isSensorInitialized = true;
-
- if (result == sick_scan::ExitSuccess) // OK -> loop again
- {
- if (changeIP)
- {
- runState = RunState::scannerFinalize;
- }
- else
- {
- runState = RunState::scannerRun; // after initialising switch to run state
- }
-
- }
- else
- {
- runState = RunState::scannerInit; // If there was an error, try to restart scanner
- }
- }
/* (Requires the armarx::LightweightRemoteGuiComponentPluginUser.)
void SickLaserUnit::createRemoteGuiTab()
{
diff --git a/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.h b/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.h
index 81f8391e1..4c447b11e 100644
--- a/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.h
+++ b/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.h
@@ -25,7 +25,7 @@
// #include <mutex>
#include <ArmarXCore/core/Component.h>
-
+#include <ArmarXCore/core/services/tasks/RunningTask.h>
// #include
// <ArmarXCore/libraries/ArmarXCoreComponentPlugins/DebugObserverComponentPlugin.h>
@@ -36,14 +36,62 @@
#include <vector>
-#include 'include/sick_scan_base/include/sick_scan/sick_scan_common_tcp.h'
+//#include "thirdparty/sick_scan_base/include/sick_scan/sick_scan_common.h"
-#include 'include/sick_scan_base/include/sick_scan/sick_generic_parser.h'
-#include 'include/sick_scan_base/include/sick_scan/sick_generic_laser.h'
+#include <sick_scan/sick_scan_common_tcp.h>
+#include <sick_scan/sick_scan_common.h>
+#include <sick_scan/sick_generic_laser.h>
namespace armarx
{
+ enum class ScanProtocol
+ {
+ ASCII,
+ Binary
+ };
+
+ enum class RunState
+ {
+ scannerInit,
+ scannerRun,
+ scannerFinalize
+ };
+
+ struct SickLaserScanDevice
+ {
+ //scanner parameters
+ std::string scannerType;
+ int deviceNumber = 0;
+ double rangeMin;
+ double rangeMax;
+ double timeIncrement;
+ //communication parameters
+ std::string ip;
+ std::string newIpAddress = "";
+ std::string port;
+ float angleOffset = 0.0f;
+ int timelimit = 5;
+ bool subscribeDatagram = false;
+ ScanProtocol protocol = ScanProtocol::ASCII;
+ bool sopasProtocolType = false;
+ bool useTcp = false;
+ bool changeIP = false;
+ bool emulSensor = false;
+ char colaDialectId = 'B';
+ //data and task pointers
+ std::vector<long> scanData;
+ RunState runState = RunState::scannerFinalize;
+ RunningTask<SickLaserScanDevice>::pointer_type task;
+ sick_scan::SickScanConfig cfg;
+ sick_scan::SickScanCommonTcp* scanner;
+ sick_scan::SickGenericParser* parser;
+ int result = sick_scan::ExitError;
+ bool isSensorInitialized = false;
+
+ void initScanner();
+ void run();
+ };
/**
* @defgroup Component-SickLaserUnit SickLaserUnit
* @ingroup RobotAPI-Components
@@ -63,17 +111,6 @@ namespace armarx
public:
/// @see armarx::ManagedIceObject::getDefaultName()
std::string getDefaultName() const override;
- enum class Protocol
- {
- ASCII,
- Binary
- };
- enum class RunState
- {
- scannerInit,
- scannerRun,
- scannerFinalize
- };
protected:
/// @see PropertyUser::createPropertyDefinitions()
@@ -91,8 +128,6 @@ namespace armarx
/// @see armarx::ManagedIceObject::onExitComponent()
void onExitComponent() override;
- void initScanner();
-
/* (Requires armarx::LightweightRemoteGuiComponentPluginUser.)
/// This function should be called once in onConnect() or when you
/// need to re-create the Remote GUI tab.
@@ -122,12 +157,12 @@ namespace armarx
struct Properties
{
//communication parameters
- std::string hostname = "";
+ std::string hostname = "192.168.8.129";
std::string newIpAddress = "";
std::string port = "2112";
int timelimit = 5;
bool subscribeDatagram = false;
- Protocol protocol = Protocol::Binary;
+ ScanProtocol protocol = ScanProtocol::Binary;
bool sopasProtocolType = false;
//scanner parameters
std::string scannerType;
@@ -139,18 +174,8 @@ namespace armarx
bool emulSensor = false;
};
Properties properties;
+ SickLaserScanDevice scanDevice;
- RunState runState;
-
- bool useTcp = false;
- bool changeIP = false;
- char colaDialectId = 'B';
-
- int result = sick_scan::ExitError;
- sick_scan::SickScanConfig cfg;
- sick_scan::SickScanCommonTcp* scanner;
- sick_scan::SickGenericParser* parser;
- bool isSensorInitialized = false;
/* Use a mutex if you access variables from different threads
* (e.g. ice functions and RemoteGui_update()).
std::mutex propertiesMutex;
--
GitLab