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

Add SickLaserScanDevice struct and methods to initialize and run the scanner

parent 5ba1509a
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.cpp
+ 125
− 106
View file @ ca18b0b9
......@@ -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()
{
......
source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.h
+ 55
− 30
View file @ ca18b0b9
......@@ -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;
......
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