From 5ba1509af2eb03af43cd5efeb4330e4fda6b1a2e Mon Sep 17 00:00:00 2001
From: Johann Mantel <j-mantel@gmx.net>
Date: Fri, 18 Jun 2021 12:53:09 +0200
Subject: [PATCH] add initialization and run methods
---
.../drivers/SickLaserUnit/CMakeLists.txt | 27 +-
.../drivers/SickLaserUnit/SickLaserUnit.cpp | 394 ++++++++++--------
.../drivers/SickLaserUnit/SickLaserUnit.h | 21 +
3 files changed, 246 insertions(+), 196 deletions(-)
diff --git a/source/RobotAPI/drivers/SickLaserUnit/CMakeLists.txt b/source/RobotAPI/drivers/SickLaserUnit/CMakeLists.txt
index 6d65f022c..92c9e15b4 100644
--- a/source/RobotAPI/drivers/SickLaserUnit/CMakeLists.txt
+++ b/source/RobotAPI/drivers/SickLaserUnit/CMakeLists.txt
@@ -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(
diff --git a/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.cpp b/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.cpp
index 4ee9c8b0b..b99a4169a 100644
--- a/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.cpp
+++ b/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.cpp
@@ -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()
{
diff --git a/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.h b/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.h
index b4233d8c9..81f8391e1 100644
--- a/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.h
+++ b/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.h
@@ -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;
--
GitLab