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

add initialization and run methods

parent 6e11930b
Branches fix/obstacle_awareness
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
source/RobotAPI/drivers/SickLaserUnit/CMakeLists.txt
+ 14
− 13
View file @ 5ba1509a
......@@ -17,19 +17,19 @@ armarx_set_target("Library: ${LIB_NAME}")
armarx_add_component(
COMPONENT_LIBS
# ArmarXCore
ArmarXCore
## ArmarXCoreComponentPlugins # For DebugObserver plugin.
# ArmarXGui
## ArmarXGuiComponentPlugins # For RemoteGui plugin.
# RobotAPI
RobotAPICore
## RobotAPIInterfaces
## RobotAPIComponentPlugins # For ArViz and other plugins.
# This project
## ${PROJECT_NAME}Interfaces # For ice interfaces from this package.
# This component
## SickLaserUnitInterfaces # If you defined a component ice interface above.
ArmarXCore
## ArmarXCoreComponentPlugins # For DebugObserver plugin.
# ArmarXGui
## ArmarXGuiComponentPlugins # For RemoteGui plugin.
# RobotAPI
RobotAPICore
## RobotAPIInterfaces
## RobotAPIComponentPlugins # For ArViz and other plugins.
# This project
## ${PROJECT_NAME}Interfaces # For ice interfaces from this package.
# This component
## SickLaserUnitInterfaces # If you defined a component ice interface above.
SOURCES
SickLaserUnit.cpp
......@@ -61,6 +61,7 @@ armarx_add_component(
# Add unit tests
add_subdirectory(test)
add_subdirectory(include/sick_scan_base)
# Generate the application
armarx_generate_and_add_component_executable(
......
source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.cpp
+ 211
− 183
View file @ 5ba1509a
......@@ -33,214 +33,242 @@ namespace armarx
std::string protocolToString(SickLaserUnit::Protocol protocol)
{
std::string protocolStr;
switch (protocol)
{
case SickLaserUnit::Protocol::ASCII:
protocolStr = "ASCII";
break;
case SickLaserUnit::Protocol::Binary:
protocolStr = "Binary";
break;
}
return modeStr;
std::string protocolStr;
switch (protocol)
{
case SickLaserUnit::Protocol::ASCII:
protocolStr = "ASCII";
break;
case SickLaserUnit::Protocol::Binary:
protocolStr = "Binary";
break;
}
return modeStr;
}
armarx::PropertyDefinitionsPtr SickLaserUnit::createPropertyDefinitions()
{
armarx::PropertyDefinitionsPtr def =
new ComponentPropertyDefinitions(getConfigIdentifier());
// Publish to a topic (passing the TopicListenerPrx).
// def->topic(myTopicListener);
// Subscribe to a topic (passing the topic name).
// def->topic<PlatformUnitListener>("MyTopic");
// Use (and depend on) another component (passing the ComponentInterfacePrx).
// def->component(myComponentProxy)
//communication parameters
def->optional(properties.hostname, "hostname", "Hostname of the LaserScanner");
def->optional(properties.newIpAddress, "newIpAddress", "New IP address for the LaserScanner");
def->optional(properties.port, "port", "port to use on the LaserScanner");
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);
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");
def->optional(properties.deviceNumber, "deviceNumber", "number of the LaserScanner Device");
def->required(properties.rangeMin, "rangeMin", "minimum Range of the Scanner");
def->required(properties.rangeMax, "rangeMax", "maximum Range of the Scanner");
def->optional(properties.timeIncrement, "timeIncrement", "timeIncrement??");
//Additional configuration
def->optional(properties.emulSensor, "emulateSensor", "overwrite the default Settings and don't connect to Scanner");
return def;
armarx::PropertyDefinitionsPtr def =
new ComponentPropertyDefinitions(getConfigIdentifier());
// Publish to a topic (passing the TopicListenerPrx).
// def->topic(myTopicListener);
// Subscribe to a topic (passing the topic name).
// def->topic<PlatformUnitListener>("MyTopic");
// Use (and depend on) another component (passing the ComponentInterfacePrx).
// def->component(myComponentProxy)
//communication parameters
def->optional(properties.hostname, "hostname", "Hostname of the LaserScanner");
def->optional(properties.newIpAddress, "newIpAddress", "New IP address for the LaserScanner");
def->optional(properties.port, "port", "port to use on the LaserScanner");
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);
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");
def->optional(properties.deviceNumber, "deviceNumber", "number of the LaserScanner Device");
def->required(properties.rangeMin, "rangeMin", "minimum Range of the Scanner");
def->required(properties.rangeMax, "rangeMax", "maximum Range of the Scanner");
def->optional(properties.timeIncrement, "timeIncrement", "timeIncrement??");
//Additional configuration
def->optional(properties.emulSensor, "emulateSensor", "overwrite the default Settings and don't connect to Scanner");
return def;
}
void SickLaserUnit::onInitComponent()
{
// Topics and properties defined above are automagically registered.
// Keep debug observer data until calling `sendDebugObserverBatch()`.
// (Requies the armarx::DebugObserverComponentPluginUser.)
// setDebugObserverBatchModeEnabled(true);
ARMARX_INFO_S << "SickLaserUnit sagt Hallo Welt!";
bool useTCP = false;
if (properties.hostname != "")
{
useTCP = true;
}
bool changeIP = false;
if (sNewIp != "")
{
changeIP = true;
}
//scanner Parameters
sick_scan::SickGenericParser* 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)
if (properties.emulSensor)
{
ARMARX_INFO("Found paraemter emulSensor overwriting default settings. Emulation: True\n");
}
switch (properties.protocol)
{
case Protocol::ASCII:
if (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;
}
else
{
ARMARX_INFO("ASCII protocol activated");
}
break;
case Protocol::Binary:
ARMARX_INFO("Binary protocol activated");
break;
default:
ARMARX_WARNING("Unknown protocol type. Defaulting to Binary protocol.\n");
properties.protocol = Protocol::Binary;
}
if (properties.protocol == Protocol::ASCII)
{
parser->getCurrentParamPtr()->setUseBinaryProtocol(false);
colaDialectId = 'A';
}
else
{
parser->getCurrentParamPtr()->setUseBinaryProtocol(true);
colaDialectId = 'B';
}
// Topics and properties defined above are automagically registered.
// Keep debug observer data until calling `sendDebugObserverBatch()`.
// (Requies the armarx::DebugObserverComponentPluginUser.)
// setDebugObserverBatchModeEnabled(true);
ARMARX_INFO_S << "SickLaserUnit sagt Hallo Welt!";
if (properties.hostname != "")
{
useTCP = true;
}
if (sNewIp != "")
{
changeIP = true;
}
//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)
if (properties.emulSensor)
{
ARMARX_INFO_S << "Found paraemter emulSensor overwriting default settings. Emulation: True";
}
switch (properties.protocol)
{
case Protocol::ASCII:
if (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;
}
else
{
ARMARX_INFO_S << "ASCII protocol activated";
}
break;
case Protocol::Binary:
ARMARX_INFO_S << "Binary protocol activated";
break;
default:
ARMARX_WARNING_S << "Unknown protocol type. Defaulting to Binary protocol.";
properties.protocol = Protocol::Binary;
}
if (properties.protocol == Protocol::ASCII)
{
parser->getCurrentParamPtr()->setUseBinaryProtocol(false);
colaDialectId = 'A';
}
else
{
parser->getCurrentParamPtr()->setUseBinaryProtocol(true);
colaDialectId = 'B';
}
isSensorInitialized = false;
}
void SickLaserUnit::onConnectComponent()
{
ARMARX_INFO("Start initialising scanner [Ip: %s] [Port: %s]", hostname.c_str(),
port.c_str());
// attempt to connect/reconnect
delete s; // disconnect scanner
if (useTCP)
{
s = new sick_scan::SickScanCommonTcp(hostname, port, timelimit, parser,
colaDialectId);
}
else
{
ROS_ERROR("TCP is not switched on. Probably hostname or port not set. Use "
"roslaunch to start node.");
exit(-1);
}
if (emulSensor)
{
s->setEmulSensor(true);
}
result = s->init();
isInitialized = true;
signal(SIGINT,
SIG_DFL); // change back to standard signal handler after initialising
if (result == sick_scan::ExitSuccess) // OK -> loop again
{
if (changeIP)
{
runState = scanner_finalize;
}
runState = scanner_run; // after initialising switch to run state
}
else
{
runState = scanner_init; // If there was an error, try to restart scanner
}
// Do things after connecting to topics and components.
/* (Requies the armarx::DebugObserverComponentPluginUser.)
// Use the debug observer to log data over time.
// The data can be viewed in the ObserverView and the LivePlotter.
// (Before starting any threads, we don't need to lock mutexes.)
{
setDebugObserverDatafield("numBoxes", properties.numBoxes);
setDebugObserverDatafield("boxLayerName", properties.boxLayerName);
sendDebugObserverBatch();
}
*/
/* (Requires the armarx::ArVizComponentPluginUser.)
// Draw boxes in ArViz.
// (Before starting any threads, we don't need to lock mutexes.)
drawBoxes(properties, arviz);
*/
/* (Requires the armarx::LightweightRemoteGuiComponentPluginUser.)
// Setup the remote GUI.
{
createRemoteGuiTab();
RemoteGui_startRunningTask();
}
*/
initScanner();
// Do things after connecting to topics and components.
/* (Requies the armarx::DebugObserverComponentPluginUser.)
// Use the debug observer to log data over time.
// The data can be viewed in the ObserverView and the LivePlotter.
// (Before starting any threads, we don't need to lock mutexes.)
{
setDebugObserverDatafield("numBoxes", properties.numBoxes);
setDebugObserverDatafield("boxLayerName", properties.boxLayerName);
sendDebugObserverBatch();
}
*/
/* (Requires the armarx::ArVizComponentPluginUser.)
// Draw boxes in ArViz.
// (Before starting any threads, we don't need to lock mutexes.)
drawBoxes(properties, arviz);
*/
/* (Requires the armarx::LightweightRemoteGuiComponentPluginUser.)
// Setup the remote GUI.
{
createRemoteGuiTab();
RemoteGui_startRunningTask();
}
*/
}
void SickLaserUnit::run()
{
while (!task->isStopped())
{
if (result == sick_scan::ExitSuccess) // OK -> loop again
{
ros::spinOnce();
result = s->loopOnce();
}
else
{
runState = scanner_finalize; // interrupt
}
}
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() {}
void SickLaserUnit::onDisconnectComponent()
{
ARMARX_INFO_S << "Disconnecting LaserScanner.";
if (scanner != NULL)
{
delete scanner;
}
if (parser != NULL)
{
delete parser;
}
}
void SickLaserUnit::onExitComponent() {}
std::string SickLaserUnit::getDefaultName() const
{
return "SickLaserUnit";
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
+ 21
− 0
View file @ 5ba1509a
......@@ -36,6 +36,11 @@
#include <vector>
#include 'include/sick_scan_base/include/sick_scan/sick_scan_common_tcp.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'
namespace armarx
{
......@@ -63,6 +68,12 @@ namespace armarx
ASCII,
Binary
};
enum class RunState
{
scannerInit,
scannerRun,
scannerFinalize
};
protected:
/// @see PropertyUser::createPropertyDefinitions()
......@@ -80,6 +91,8 @@ 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.
......@@ -127,9 +140,17 @@ namespace armarx
};
Properties properties;
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