diff --git a/scenarios/SickLaserUnitTest/config/SickLaserUnit.cfg b/scenarios/SickLaserUnitTest/config/SickLaserUnit.cfg
index 5d6d74f9763c6dc8270c818e00de7901ad33d399..bac8b18e34e4899622d2b49608a42794af7e9fae 100644
--- a/scenarios/SickLaserUnitTest/config/SickLaserUnit.cfg
+++ b/scenarios/SickLaserUnitTest/config/SickLaserUnit.cfg
@@ -247,6 +247,21 @@ ArmarX.SickLaserUnit.timeIncrement = 0.1
# ArmarX.SickLaserUnit.timelimit = 5
+# ArmarX.SickLaserUnit.topicName: Name of topic.
+# Attributes:
+# - Case sensitivity: yes
+# - Required: yes
+ArmarX.SickLaserUnit.topicName = "myLaserTopic1"
+
+
+# ArmarX.SickLaserUnit.tpc.sub.LaserScannerUnit: Name of the `LaserScannerUnit` topic to subscribe to.
+# Attributes:
+# - Default: LaserScannerUnit
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.SickLaserUnit.tpc.sub.LaserScannerUnit = LaserScannerUnit
+
+
# ArmarX.SickLaserUnit.updatePeriod: No Description
# Attributes:
# - Default: 1
diff --git a/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.cpp b/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.cpp
index 7ac3f9a052e4f27dc7aea487da40071befa9e762..2e852005c897449b67185c19fecb109484c1709f 100644
--- a/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.cpp
+++ b/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.cpp
@@ -34,209 +34,223 @@ namespace armarx
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(scanData);
- }
- else
- {
- runState = RunState::scannerFinalize;
- }
- break;
- case RunState::scannerFinalize:
- break;
- default:
- ARMARX_ERROR_S << "Invalid run state in task loop";
- break;
- }
- }
+ while (!task->isStopped())
+ {
+ switch (runState)
+ {
+ case RunState::scannerInit:
+ initScanner();
+ //read the scanner parameters for initialization
+ result = scanner->loopOnce(scanData, scanTime, scanInfo, true);
+ break;
+ case RunState::scannerRun:
+ if (result == sick_scan::ExitSuccess) // OK -> loop again
+ {
+ result = scanner->loopOnce(scanData, scanTime, scanInfo, false);
+ if (scanTopic)
+ {
+ TimestampVariantPtr scanT(new TimestampVariant(scanTime));
+ scanTopic->reportSensorValues(ip, frameName, scanData, scanT);
+ scanData.clear();
+ //trigger heartbeat
+ //heartbeat.heartbeat(channel);
+ }
+ else
+ {
+ ARMARX_WARNING << "No scan topic available: IP: " << ip << ", Port: " << port;
+ }
+ }
+ else
+ {
+ runState = RunState::scannerInit;
+ }
+ break;
+ case RunState::scannerFinalize:
+ ARMARX_WARNING << "scanner offline";
+ break;
+ default:
+ ARMARX_WARNING << "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 SickScanAdapter(this->ip, this->port, this->timelimit, this->parser, 'A');
- //this->scanner = new sick_scan::SickScanCommonTcp(this->ip, this->port, this->timelimit, this->parser, 'A');
- }
- else
- {
- ARMARX_ERROR_S << "TCP is not switched on. Probably hostname or port not set.\n";
- return;
- }
- result = this->scanner->init();
-
-
- if (result == sick_scan::ExitSuccess) // OK -> loop again
- {
- this->isSensorInitialized = true;
- ARMARX_INFO_S << "Scanner initialized.";
- this->runState = RunState::scannerRun; // after initialising switch to run state
- }
- else
- {
- ARMARX_INFO_S << "Reinitializing scanner.";
- this->runState = RunState::scannerInit; // If there was an error, try to restart scanner
- }
+ 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 SickScanAdapter(this->ip, this->port, this->timelimit, this->parser, 'A');
+ }
+ else
+ {
+ ARMARX_ERROR_S << "TCP is not switched on. Probably hostname or port not set.\n";
+ return;
+ }
+ result = this->scanner->init();
+
+
+ if (result == sick_scan::ExitSuccess) // OK -> loop again
+ {
+ this->isSensorInitialized = true;
+ ARMARX_INFO_S << "Scanner initialized.";
+ this->runState = RunState::scannerRun; // after initialising switch to run state
+ }
+ else
+ {
+ ARMARX_INFO_S << "Reinitializing scanner.";
+ this->runState = RunState::scannerInit; // If there was an error, try to restart scanner
+ }
}
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<LaserScannerUnitListenerPrx>(properties.topicName);
-
- // Use (and depend on) another component (passing the ComponentInterfacePrx).
- // def->component(myComponentProxy)
-
- def->required(properties.topicName, "topicName", "Name of topic.");
- //communication parameters
- def->optional(properties.hostname, "hostname", "Hostname of 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");
- //Scanner parameters
- def->required(properties.scannerType, "scannerType", "Name of the LaserScanner");
- def->optional(properties.deviceNumber, "deviceNumber", "number of the LaserScanner Device");
- def->optional(properties.angleOffset, "angleOffset", "offset to the scanning angle");
- def->optional(properties.rangeMin, "rangeMin", "minimum Range of the Scanner");
- def->optional(properties.rangeMax, "rangeMax", "maximum Range of the Scanner");
- def->optional(properties.timeIncrement, "timeIncrement", "timeIncrement??");
- 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<LaserScannerUnitListenerPrx>(properties.topicName);
+
+ // Use (and depend on) another component (passing the ComponentInterfacePrx).
+ // def->component(myComponentProxy)
+
+ 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.timelimit, "timelimit", "timelimit for communication");
+ def->required(properties.scannerType, "scannerType", "Name of the LaserScanner");
+ def->optional(properties.angleOffset, "angleOffset", "offset to the scanning angle");
+ def->optional(properties.rangeMin, "rangeMin", "minimum Range of the Scanner");
+ def->optional(properties.rangeMax, "rangeMax", "maximum Range of the Scanner");
+ def->optional(properties.timeIncrement, "timeIncrement", "timeIncrement??");
+ return def;
}
void SickLaserUnit::onInitComponent()
{
- // Topics and properties defined above are automagically registered.
- //addPlugin(heartbeat);
-
- topic = getTopic<LaserScannerUnitListenerPrx>(properties.topicName);
- offeringTopic(properties.topicName);
- // Keep debug observer data until calling `sendDebugObserverBatch()`.
- // (Requies the armarx::DebugObserverComponentPluginUser.)
- // setDebugObserverBatchModeEnabled(true);
-
- ARMARX_INFO_S << "initializing SickLaserUnit.";
-
- scanDevice.scanTopic = topic;
- scanDevice.isSensorInitialized = false;
- if (properties.hostname != "")
- {
- scanDevice.useTcp = true;
- scanDevice.ip = properties.hostname;
- }
- scanDevice.port = properties.port;
- scanDevice.angleOffset = properties.angleOffset;
- scanDevice.timelimit = properties.timelimit;
- scanDevice.subscribeDatagram = properties.subscribeDatagram;
- scanDevice.scannerType = properties.scannerType;
- scanDevice.deviceNumber = properties.deviceNumber;
- 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;
- }
- scanDevice.parser->getCurrentParamPtr()->setUseBinaryProtocol(false);
- scanDevice.colaDialectId = 'A';
- ARMARX_INFO_S << "ASCII protocol activated.";
-
- ARMARX_INFO_S << "SickLaserUnit initialisation complete.";
+ // Topics and properties defined above are automagically registered.
+
+ // Keep debug observer data until calling `sendDebugObserverBatch()`.
+ // (Requies the armarx::DebugObserverComponentPluginUser.)
+ // setDebugObserverBatchModeEnabled(true);
+
+ ARMARX_INFO_S << "initializing SickLaserUnit.";
+
+ scanDevice.scanTopic = topic;
+ scanDevice.isSensorInitialized = false;
+ if (properties.hostname != "")
+ {
+ 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;
+ }
+ scanDevice.parser->getCurrentParamPtr()->setUseBinaryProtocol(false);
+ scanDevice.colaDialectId = 'A';
+ ARMARX_INFO_S << "ASCII protocol activated.";
+ ARMARX_INFO_S << "SickLaserUnit initialisation complete.";
+
+ //addPlugin(heartbeat);
+ //configureHeartbeatChannel();
}
void SickLaserUnit::onConnectComponent()
{
- ARMARX_INFO_S << "Connecting.";
-
- //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.)
- // 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();
- }
- */
+ topic = getTopic<LaserScannerUnitListenerPrx>(properties.topicName);
+ offeringTopic(properties.topicName);
+ scanDevice.scanTopic = topic;
+
+ ARMARX_INFO_S << "Connecting.";
+
+ //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.)
+ // 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::onDisconnectComponent()
{
- ARMARX_INFO_S << "Disconnecting LaserScanner.";
- if (scanDevice.task)
- {
- scanDevice.task->stop();
- scanDevice.task = nullptr;
- }
- if (scanDevice.scanner)
- {
- delete scanDevice.scanner;
- }
- if (scanDevice.parser)
- {
- delete scanDevice.parser;
- }
+ ARMARX_INFO_S << "Disconnecting LaserScanner.";
+ if (scanDevice.task)
+ {
+ scanDevice.task->stop();
+ scanDevice.task = nullptr;
+ }
+ if (scanDevice.scanner)
+ {
+ delete scanDevice.scanner;
+ }
+ if (scanDevice.parser)
+ {
+ delete scanDevice.parser;
+ }
}
void SickLaserUnit::onExitComponent() {}
std::string SickLaserUnit::getDefaultName() const
{
- return "SickLaserUnit";
+ return "SickLaserUnit";
}
/* (Requires the armarx::LightweightRemoteGuiComponentPluginUser.)
diff --git a/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.h b/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.h
index a25ee498266b940297af45ee648e57e5960dea81..388285ae8314bf45829316c1eafa24696a9a0e6e 100644
--- a/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.h
+++ b/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.h
@@ -29,10 +29,6 @@
#include <RobotAPI/interface/units/LaserScannerUnit.h>
// #include <ArmarXCore/libraries/ArmarXCoreComponentPlugins/DebugObserverComponentPlugin.h>
-// #include <ArmarXGui/libraries/ArmarXGuiComponentPlugins/LightweightRemoteGuiComponentPlugin.h>
-
-// #include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
-
#include <vector>
#include "SickScanAdapter.h"
@@ -45,49 +41,48 @@ namespace armarx
enum class ScanProtocol
{
- ASCII,
- Binary
+ ASCII,
+ Binary
};
enum class RunState
{
- scannerInit,
- scannerRun,
- scannerFinalize
+ scannerInit,
+ scannerRun,
+ scannerFinalize
};
struct SickLaserScanDevice
{
- //scanner parameters
- std::string scannerType = "sick_tim_5xx";
- int deviceNumber = 0;
- double angleOffset = 0.0;
- double rangeMin;
- double rangeMax;
- double timeIncrement;
- //communication parameters
- std::string ip;
- std::string port;
- int timelimit = 5;
- bool subscribeDatagram = false;
- bool useTcp = false;
- char colaDialectId = 'A';
- //data and task pointers
- std::vector<long> lengthData;
- LaserScan scanData;
- RunState runState = RunState::scannerFinalize;
- RunningTask<SickLaserScanDevice>::pointer_type task;
- LaserScannerUnitListenerPrx scanTopic;
-
- sick_scan::SickScanConfig cfg;
- SickScanAdapter* scanner;
- //sick_scan::SickScanCommonTcp* scanner;
- sick_scan::SickGenericParser* parser;
- int result = sick_scan::ExitError;
- bool isSensorInitialized = false;
-
- void initScanner();
- void run();
+ //scanner parameters
+ std::string scannerType = "sick_tim_5xx";
+ double angleOffset = 0.0;
+ double rangeMin;
+ double rangeMax;
+ double timeIncrement;
+ //communication parameters
+ std::string ip;
+ std::string port;
+ int timelimit = 5;
+ bool useTcp = false;
+ char colaDialectId = 'A';
+ //data and task pointers
+ IceUtil::Time scanTime;
+ LaserScan scanData;
+ LaserScannerInfo scanInfo;
+ RunState runState = RunState::scannerFinalize;
+ RunningTask<SickLaserScanDevice>::pointer_type task;
+ std::string frameName = "LaserScannerFront";
+ LaserScannerUnitListenerPrx scanTopic;
+
+ sick_scan::SickScanConfig cfg;
+ sick_scan::SickGenericParser* parser;
+ SickScanAdapter* scanner;
+ int result = sick_scan::ExitError;
+ bool isSensorInitialized = false;
+
+ void initScanner();
+ void run();
};
/**
* @defgroup Component-SickLaserUnit SickLaserUnit
@@ -101,84 +96,79 @@ namespace armarx
* Detailed description of class SickLaserUnit.
*/
class SickLaserUnit :
- virtual public armarx::LaserScannerUnitInterface,
- virtual public armarx::Component
+ //virtual public armarx::LaserScannerUnitInterface,
+ virtual public armarx::Component
// , virtual public armarx::DebugObserverComponentPluginUser
// , virtual public armarx::LightweightRemoteGuiComponentPluginUser
// , virtual public armarx::ArVizComponentPluginUser
{
public:
- /// @see armarx::ManagedIceObject::getDefaultName()
- std::string getDefaultName() const override;
+ /// @see armarx::ManagedIceObject::getDefaultName()
+ std::string getDefaultName() const override;
protected:
- /// @see PropertyUser::createPropertyDefinitions()
- armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
+ /// @see PropertyUser::createPropertyDefinitions()
+ armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
- /// @see armarx::ManagedIceObject::onInitComponent()
- void onInitComponent() override;
+ /// @see armarx::ManagedIceObject::onInitComponent()
+ void onInitComponent() override;
- /// @see armarx::ManagedIceObject::onConnectComponent()
- void onConnectComponent() override;
+ /// @see armarx::ManagedIceObject::onConnectComponent()
+ void onConnectComponent() override;
- /// @see armarx::ManagedIceObject::onDisconnectComponent()
- void onDisconnectComponent() override;
+ /// @see armarx::ManagedIceObject::onDisconnectComponent()
+ void onDisconnectComponent() override;
- /// @see armarx::ManagedIceObject::onExitComponent()
- void onExitComponent() override;
+ /// @see armarx::ManagedIceObject::onExitComponent()
+ void onExitComponent() override;
private:
- // Private methods go here.
+ // Private methods go here.
private:
- // Private member variables go here.
-
- /// Properties shown in the Scenario GUI.
- struct Properties
- {
- std::string topicName;
- //communication parameters
- std::string hostname = "192.168.8.133";
- std::string port = "2112";
- int timelimit = 5;
- bool subscribeDatagram = false;
- //scanner parameters
- std::string scannerType = "sick_tim_5xx";
- int deviceNumber = 0;
- double angleOffset = 0.0;
- double rangeMin = 0.0;
- double rangeMax = 10.0;
- double timeIncrement = 0.1;
- //additional parameters
- bool emulSensor = false;
- };
- Properties properties;
- SickLaserScanDevice scanDevice;
- std::string topicName;
- LaserScannerUnitListenerPrx topic;
- //HeartbeatComponentPlugin heartbeat;
-
- /* Use a mutex if you access variables from different threads
- * (e.g. ice functions and RemoteGui_update()).
- std::mutex propertiesMutex;
- */
-
- /* (Requires the armarx::LightweightRemoteGuiComponentPluginUser.)
- /// Tab shown in the Remote GUI.
- struct RemoteGuiTab : armarx::RemoteGui::Client::Tab
- {
- armarx::RemoteGui::Client::LineEdit boxLayerName;
- armarx::RemoteGui::Client::IntSpinBox numBoxes;
-
- armarx::RemoteGui::Client::Button drawBoxes;
- };
- RemoteGuiTab tab;
- */
-
- /* (Requires the armarx::ArVizComponentPluginUser.)
- * When used from different threads, an ArViz client needs to be synchronized.
- /// Protects the arviz client inherited from the ArViz plugin.
- std::mutex arvizMutex;
- */
+ // Private member variables go here.
+
+ /// Properties shown in the Scenario GUI.
+ struct Properties
+ {
+ std::string topicName = "SICKLaserScanner";
+ //scanner parameters
+ std::string hostname = "192.168.8.133";
+ std::string port = "2112";
+ std::string frameName = "LaserScannerFront";
+ int timelimit = 5;
+ std::string scannerType = "sick_tim_5xx";
+ double angleOffset = 0.0;
+ double rangeMin = 0.0;
+ double rangeMax = 10.0;
+ double timeIncrement = 0.1;
+ };
+ Properties properties;
+ SickLaserScanDevice scanDevice;
+ LaserScannerUnitListenerPrx topic;
+ //HeartbeatComponentPlugin heartbeat;
+
+ /* Use a mutex if you access variables from different threads
+ * (e.g. ice functions and RemoteGui_update()).
+ std::mutex propertiesMutex;
+ */
+
+ /* (Requires the armarx::LightweightRemoteGuiComponentPluginUser.)
+ /// Tab shown in the Remote GUI.
+ struct RemoteGuiTab : armarx::RemoteGui::Client::Tab
+ {
+ armarx::RemoteGui::Client::LineEdit boxLayerName;
+ armarx::RemoteGui::Client::IntSpinBox numBoxes;
+
+ armarx::RemoteGui::Client::Button drawBoxes;
+ };
+ RemoteGuiTab tab;
+ */
+
+ /* (Requires the armarx::ArVizComponentPluginUser.)
+ * When used from different threads, an ArViz client needs to be synchronized.
+ /// Protects the arviz client inherited from the ArViz plugin.
+ std::mutex arvizMutex;
+ */
};
} // namespace armarx
diff --git a/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.cpp b/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.cpp
index d288b372f9803fc8ae21a3967f20387d47be3c69..6371db0624ce1426b8688df28916aef21e23e8eb 100644
--- a/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.cpp
+++ b/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.cpp
@@ -100,96 +100,8 @@ static int getDiagnosticErrorCode()
namespace armarx
{
- bool emulateReply(UINT8* requestData, int requestLen, std::vector<unsigned char>* replyVector)
- {
- std::string request;
- std::string reply;
- std::vector<std::string> keyWordList;
- std::vector<std::string> answerList;
-
- std::string scannerType = "???";
- ros::NodeHandle nhPriv;
-
- enum
- {
- SIMU_RADAR = 0, SIMU_MRS_1XXX = 1, SIMU_NUM
- };
- // XXX
- keyWordList.push_back("sMN SetAccessMode");
- answerList.push_back("sAN SetAccessMode 1");
-
- keyWordList.push_back("sWN EIHstCola");
- answerList.push_back("sWA EIHstCola");
-
- keyWordList.push_back("sRN FirmwareVersion");
- answerList.push_back("sRA FirmwareVersion 8 1.0.0.0R");
-
- keyWordList.push_back("sRN OrdNum");
- answerList.push_back("sRA OrdNum 7 1234567");
-
- keyWordList.push_back("sWN TransmitTargets 1");
- answerList.push_back("sWA TransmitTargets");
-
- keyWordList.push_back("sWN TransmitObjects 1");
- answerList.push_back("sWA TransmitObjects");
-
- keyWordList.push_back("sWN TCTrackingMode 0");
- answerList.push_back("sWA TCTrackingMode");
-
- keyWordList.push_back("sRN SCdevicestate");
- answerList.push_back("sRA SCdevicestate 1");
-
- keyWordList.push_back("sRN DItype");
- answerList.push_back("sRA DItype D RMS3xx-xxxxxx");
-
- keyWordList.push_back("sRN ODoprh");
- answerList.push_back("sRA ODoprh 451");
-
- keyWordList.push_back("sMN mSCloadappdef");
- answerList.push_back("sAN mSCloadappdef");
-
-
- keyWordList.push_back("sRN SerialNumber");
- answerList.push_back("sRA SerialNumber 8 18020073");
-
- keyWordList.push_back("sMN Run");
- answerList.push_back("sAN Run 1s");
-
- keyWordList.push_back("sRN ODpwrc");
- answerList.push_back("sRA ODpwrc 20");
-
- keyWordList.push_back("sRN LocationName");
- answerList.push_back("sRA LocationName B not defined");
-
- keyWordList.push_back("sEN LMDradardata 1");
- answerList.push_back("sEA LMDradardata 1");
-
- for (int i = 0; i < requestLen; i++)
- {
- request += (char) requestData[i];
- }
- for (size_t i = 0; i < keyWordList.size(); i++)
- {
- if (request.find(keyWordList[i]) != std::string::npos)
- {
- reply = (char) 0x02;
- reply += answerList[i];
- reply += (char) 0x03;
- }
- }
-
- replyVector->clear();
- for (size_t i = 0; i < reply.length(); i++)
- {
- replyVector->push_back((unsigned char) reply[i]);
- }
-
- return (true);
- }
-
-
SickScanAdapter::SickScanAdapter(const std::string& hostname, const std::string& port, int& timelimit,
- sick_scan::SickGenericParser* parser, char cola_dialect_id, const std::string& topicName)
+ sick_scan::SickGenericParser* parser, char cola_dialect_id)
:
sick_scan::SickScanCommon(parser),
socket_(io_service_),
@@ -197,11 +109,9 @@ namespace armarx
parser_ptr(parser),
hostname_(hostname),
port_(port),
- timelimit_(timelimit),
- topicName(topicName)
+ timelimit_(timelimit)
{
- setEmulSensor(false);
if ((cola_dialect_id == 'a') || (cola_dialect_id == 'A'))
{
this->setProtocolType(CoLa_A);
@@ -223,7 +133,6 @@ namespace armarx
// http://www.boost.org/doc/libs/1_46_0/doc/html/boost_asio/example/timeouts/blocking_tcp_client.cpp
deadline_.expires_at(boost::posix_time::pos_infin);
checkDeadline();
-
}
SickScanAdapter::~SickScanAdapter()
@@ -241,127 +150,73 @@ namespace armarx
\brief parsing datagram and publishing ros messages
\return error code
*/
- int SickScanAdapter::loopOnce(LaserScan& scanData)
+ int SickScanAdapter::loopOnce(LaserScan& scanData, IceUtil::Time& scanTime, LaserScannerInfo& scanInfo, bool updateScannerInfo)
{
- static int cnt = 0;
-
unsigned char receiveBuffer[65536];
int actual_length = 0;
- static unsigned int iteration_count = 0;
- //Always ASCII
- bool useBinaryProtocol = false;
+ int packetsInLoop = 0;
ros::Time recvTimeStamp = ros::Time::now(); // timestamp incoming package, will be overwritten by get_datagram
- // tcp packet
- //ros::Time recvTimeStampPush = ros::Time::now(); // timestamp
-
- int packetsInLoop = 0;
- const int maxNumAllowedPacketsToProcess = 25; // maximum number of packets, which will be processed in this loop.
- int numPacketsProcessed = 0; // count number of processed datagrams
- do
+ int result = get_datagram(recvTimeStamp, receiveBuffer, 65536, &actual_length, false, &packetsInLoop);
+ //ros::Duration dur = recvTimeStampPush - recvTimeStamp;
+ if (result != 0)
{
-
- int result = get_datagram(recvTimeStamp, receiveBuffer, 65536, &actual_length, useBinaryProtocol, &packetsInLoop);
- numPacketsProcessed++;
-
- //TODO: convert ros::Time recvTimeStamp to IceUtil
- IceUtil::Time time_measure = TimeUtil::GetTime();
- TimestampVariantPtr now(new TimestampVariant(time_measure));
-
- //ros::Duration dur = recvTimeStampPush - recvTimeStamp;
-
- if (result != 0)
+ ARMARX_ERROR_S << "Read Error when getting datagram: " << result;
+ return sick_scan::ExitError; // return failure to exit node
+ }
+ if (actual_length <= 0)
+ {
+ return sick_scan::ExitSuccess;
+ } // return success to continue looping
+
+ //TODO: convert ros::Time recvTimeStamp to IceUtil
+ scanTime = TimeUtil::GetTime();
+ //msg.header.stamp = recvTimeStamp + ros::Duration(config_.time_offset);
+
+ //datagrams are enclosed in <STX> (0x02), <ETX> (0x03) pairs
+ char* buffer_pos = (char*) receiveBuffer;
+ char* dstart, *dend;
+ bool dataToProcess = true;
+ std::vector<float> vang_vec;
+ vang_vec.clear();
+ dstart = NULL;
+ dend = NULL;
+
+ while (dataToProcess)
+ {
+ size_t dlength;
+ int success = -1;
+ // Always Parsing Ascii-Encoding of datagram
+ dstart = strchr(buffer_pos, 0x02);
+ if (dstart != NULL)
{
- ARMARX_ERROR_S << "Read Error when getting datagram: " << result;
- return sick_scan::ExitError; // return failure to exit node
+ dend = strchr(dstart + 1, 0x03);
}
- if (actual_length <= 0)
+ if ((dstart != NULL) && (dend != NULL))
{
- return sick_scan::ExitSuccess;
- } // return success to continue looping
-
- // ----- if requested, skip frames
- if (iteration_count++ % (config_.skip + 1) != 0)
+ dataToProcess = true; // continue parsing
+ dlength = dend - dstart;
+ *dend = '\0';
+ dstart++;
+ }
+ else
{
- return sick_scan::ExitSuccess;
+ dataToProcess = false;
+ break;
}
- //msg.header.stamp = recvTimeStamp + ros::Duration(config_.time_offset);
- //datagrams are enclosed in <STX> (0x02), <ETX> (0x03) pairs
- char* buffer_pos = (char*) receiveBuffer;
- char* dstart, *dend;
- bool dataToProcess = true;
- std::vector<float> vang_vec;
- vang_vec.clear();
- dstart = NULL;
- dend = NULL;
-
- while (dataToProcess)
+ // HEADER of data followed by DIST1 ... DIST2 ... DIST3 .... RSSI1 ... RSSI2.... RSSI3...
+ // <frameid>_<sign>00500_DIST[1|2|3]
+ success = parseDatagram(dstart, dlength, scanData, scanInfo, updateScannerInfo);
+ if (success != sick_scan::ExitSuccess)
{
- size_t dlength;
- int success = -1;
- LaserScannerInfo scanInfo;
-
- // Always Parsing Ascii-Encoding of datagram
- dstart = strchr(buffer_pos, 0x02);
- if (dstart != NULL)
- {
- dend = strchr(dstart + 1, 0x03);
- }
- if ((dstart != NULL) && (dend != NULL))
- {
- dataToProcess = true; // continue parsing
- dlength = dend - dstart;
- *dend = '\0';
- dstart++;
- }
- else
- {
- dataToProcess = false;
- break;
- }
-
- // HEADER of data followed by DIST1 ... DIST2 ... DIST3 .... RSSI1 ... RSSI2.... RSSI3...
- // <frameid>_<sign>00500_DIST[1|2|3]
- success = parseDatagram(dstart, dlength, scanData, scanInfo, true);
-
- if (success == sick_scan::ExitSuccess)
- {
- // cloud_.header.stamp = recvTimeStamp + ros::Duration(config_.time_offset);
- // cloud_.header.frame_id = config_.frame_id;
- // cloud_.header.seq = 0;
- // cloud_.height = numTmpLayer * numValidEchos; // due to multi echo multiplied by num. of layers
- // cloud_.width = msg.ranges.size();
- // cloud_.is_bigendian = false;
- // cloud_.is_dense = true;
- // cloud_.point_step = numChannels * sizeof(float);
- // cloud_.row_step = cloud_.point_step * cloud_.width;
- // cloud_.fields.resize(numChannels);
- // for (int i = 0; i < numChannels; i++)
- // {
- // std::string channelId[] = {"x", "y", "z", "intensity"};
- // cloud_.fields[i].name = channelId[i];
- // cloud_.fields[i].offset = i * sizeof(float);
- // cloud_.fields[i].count = 1;
- // cloud_.fields[i].datatype = sensor_msgs::PointField::FLOAT32;
- // }
- if (scanTopic)
- {
- scanTopic->reportSensorValues(ip, "frame1", scanData, now);
- }
- else
- {
- ARMARX_WARNING << "No scan topic available: IP: " << ip << ", Port: " << port;
- }
-
- }
- // Start Point
- if (dend != NULL)
- {
- buffer_pos = dend + 1;
- }
- } // end of while loop
- }
- while ((packetsInLoop > 0) && (numPacketsProcessed < maxNumAllowedPacketsToProcess));
+ ARMARX_WARNING << "parseDatagram returned ErrorCode: " << success;
+ }
+ // Start Point
+ if (dend != NULL)
+ {
+ buffer_pos = dend + 1;
+ }
+ } // end of while loop
return sick_scan::ExitSuccess; // return success to continue looping
}
@@ -630,26 +485,6 @@ namespace armarx
return (m_replyMode);
}
- /*!
- \brief Set emulation flag (using emulation instead of "real" scanner - currently implemented for radar
- \param _emulFlag: Flag to switch emulation on or off
- \return
- */
- void SickScanAdapter::setEmulSensor(bool _emulFlag)
- {
- m_emulSensor = _emulFlag;
- }
-
- /*!
- \brief get emulation flag (using emulation instead of "real" scanner - currently implemented for radar
- \param
- \return bool: Flag to switch emulation on or off
- */
- bool SickScanAdapter::getEmulSensor()
- {
- return (m_emulSensor);
- }
-
//
// Look for 23-frame (STX/ETX) in receive buffer.
// Move frame to start of buffer
@@ -938,14 +773,7 @@ namespace armarx
sscanf(port_.c_str(), "%d", &portInt);
m_nw.init(hostname_, portInt, disconnectFunctionS, (void*) this);
m_nw.setReadCallbackFunction(readCallbackFunctionS, (void*) this);
- if (this->getEmulSensor())
- {
- ARMARX_INFO_S << "Sensor emulation is switched on - network traffic is switched off.";
- }
- else
- {
- m_nw.connect();
- }
+ m_nw.connect();
return sick_scan::ExitSuccess;
}
@@ -1028,14 +856,6 @@ namespace armarx
*/
int SickScanAdapter::sendSOPASCommand(const char* request, std::vector<unsigned char>* reply, int cmdLen)
{
-#if 0
- if (!socket_.is_open())
- {
- ROS_ERROR("sendSOPASCommand: socket not open");
- diagnostics_.broadcast(getDiagnosticErrorCode(), "sendSOPASCommand: socket not open.");
- return sick_scan::ExitError;
- }
-#endif
int sLen = 0;
int preambelCnt = 0;
bool cmdIsBinary = false;
@@ -1078,25 +898,19 @@ namespace armarx
msgLen = 8 + dataLen + 1; // 8 Msg. Header + Packet +
}
#if 1
- if (getEmulSensor())
- {
- emulateReply((UINT8*) request, msgLen, reply);
- }
- else
+
+ bool debugBinCmd = false;
+ if (debugBinCmd)
{
- bool debugBinCmd = false;
- if (debugBinCmd)
+ printf("=== START HEX DUMP ===\n");
+ for (int i = 0; i < msgLen; i++)
{
- printf("=== START HEX DUMP ===\n");
- for (int i = 0; i < msgLen; i++)
- {
- unsigned char* ptr = (UINT8*) request;
- printf("%02x ", ptr[i]);
- }
- printf("\n=== END HEX DUMP ===\n");
+ unsigned char* ptr = (UINT8*) request;
+ printf("%02x ", ptr[i]);
}
- m_nw.sendCommandBuffer((UINT8*) request, msgLen);
+ printf("\n=== END HEX DUMP ===\n");
}
+ m_nw.sendCommandBuffer((UINT8*) request, msgLen);
#else
/*
@@ -1120,25 +934,18 @@ namespace armarx
char buffer[BUF_SIZE];
int bytes_read;
// !!!
- if (getEmulSensor())
+ if (readWithTimeout(getReadTimeOutInMs(), buffer, BUF_SIZE, &bytes_read, 0, cmdIsBinary) == sick_scan::ExitError)
{
-
+ ARMARX_INFO_S << "sendSOPASCommand: no full reply available for read after (ms): " << getReadTimeOutInMs();
+ return sick_scan::ExitError;
}
- else
- {
- if (readWithTimeout(getReadTimeOutInMs(), buffer, BUF_SIZE, &bytes_read, 0, cmdIsBinary) == sick_scan::ExitError)
- {
- ARMARX_INFO_S << "sendSOPASCommand: no full reply available for read after (ms): " << getReadTimeOutInMs();
- return sick_scan::ExitError;
- }
- if (reply)
- {
- reply->resize(bytes_read);
+ if (reply)
+ {
+ reply->resize(bytes_read);
- std::copy(buffer, buffer + bytes_read, &(*reply)[0]);
- }
+ std::copy(buffer, buffer + bytes_read, &(*reply)[0]);
}
return sick_scan::ExitSuccess;
}
@@ -1153,57 +960,34 @@ namespace armarx
*numberOfRemainingFifoEntries = 0;
}
this->setReplyMode(1);
-
- if (this->getEmulSensor())
+ const int maxWaitInMs = getReadTimeOutInMs();
+ std::vector<unsigned char> dataBuffer;
+#if 1 // prepared for reconnect
+ bool retVal = this->recvQueue.waitForIncomingObject(maxWaitInMs);
+ if (retVal == false)
{
-#ifndef ROSSIMU
- // boost::this_thread::sleep(boost::posix_time::milliseconds(waitingTimeInMs));
- ros::Time timeStamp = ros::Time::now();
- uint32_t nanoSec = timeStamp.nsec;
- double waitTime10Hz = 10.0 * (double) nanoSec / 1E9; // 10th of sec. [0..10[
-
- uint32_t waitTime = (int) waitTime10Hz; // round down
-
- double waitTimeUntilNextTime10Hz = 1 / 10.0 * (1.0 - (waitTime10Hz - waitTime));
-
- ros::Duration(waitTimeUntilNextTime10Hz).sleep();
- SickScanRadar radar(this);
- radar.setEmulation(true);
- radar.simulateAsciiDatagram(receiveBuffer, actual_length);
- recvTimeStamp = ros::Time::now();
-#endif
+ ARMARX_ERROR_S << "Timeout during waiting for new datagram";
+ return sick_scan::ExitError;
}
else
{
- const int maxWaitInMs = getReadTimeOutInMs();
- std::vector<unsigned char> dataBuffer;
-#if 1 // prepared for reconnect
- bool retVal = this->recvQueue.waitForIncomingObject(maxWaitInMs);
- if (retVal == false)
+ // Look into receiving queue for new Datagrams
+ //
+ //
+ DatagramWithTimeStamp datagramWithTimeStamp = this->recvQueue.pop();
+ if (NULL != numberOfRemainingFifoEntries)
{
- ARMARX_ERROR_S << "Timeout during waiting for new datagram";
- return sick_scan::ExitError;
+ *numberOfRemainingFifoEntries = this->recvQueue.getNumberOfEntriesInQueue();
}
- else
- {
- // Look into receiving queue for new Datagrams
- //
- //
- DatagramWithTimeStamp datagramWithTimeStamp = this->recvQueue.pop();
- if (NULL != numberOfRemainingFifoEntries)
- {
- *numberOfRemainingFifoEntries = this->recvQueue.getNumberOfEntriesInQueue();
- }
- recvTimeStamp = datagramWithTimeStamp.timeStamp;
- dataBuffer = datagramWithTimeStamp.datagram;
+ recvTimeStamp = datagramWithTimeStamp.timeStamp;
+ dataBuffer = datagramWithTimeStamp.datagram;
- }
-#endif
- // dataBuffer = this->recvQueue.pop();
- long size = dataBuffer.size();
- memcpy(receiveBuffer, &(dataBuffer[0]), size);
- *actual_length = size;
}
+#endif
+ // dataBuffer = this->recvQueue.pop();
+ long size = dataBuffer.size();
+ memcpy(receiveBuffer, &(dataBuffer[0]), size);
+ *actual_length = size;
#if 0
static int cnt = 0;
diff --git a/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.h b/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.h
index 5fff69513a24886b39d7ea963977867962c876e9..5b7e772f33217ed39e9c708275c6f7e5d10b2251 100644
--- a/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.h
+++ b/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.h
@@ -37,6 +37,7 @@
#include <ArmarXCore/core/Component.h>
#include <RobotAPI/interface/units/LaserScannerUnit.h>
+#include <ArmarXCore/observers/variant/TimestampVariant.h>
#include <stdio.h>
#include <stdlib.h>
@@ -68,12 +69,10 @@ namespace armarx
};
- class SickScanAdapter :
- virtual public armarx::LaserScannerUnitInterface,
- public sick_scan::SickScanCommon
+ class SickScanAdapter : public sick_scan::SickScanCommon
{
public:
- int loopOnce(LaserScan& scanData);
+ int loopOnce(LaserScan& scanData, IceUtil::Time& scanTime, LaserScannerInfo& scanInfo, bool updateScannerInfo = false);
int parseDatagram(char* datagram, size_t datagram_length, LaserScan& scanData, LaserScannerInfo& scanInfo, bool updateScannerInfo = false);
@@ -92,10 +91,6 @@ namespace armarx
int getReplyMode();
- void setEmulSensor(bool _emulFlag);
-
- bool getEmulSensor();
-
bool stopScanData();
int numberOfDatagramInInputFifo();
@@ -173,9 +168,6 @@ namespace armarx
std::string port_;
int timelimit_;
int m_replyMode;
-
- std::string topicName;
- LaserScannerUnitListenerPrx scanTopic;
};
}
diff --git a/source/RobotAPI/interface/units/LaserScannerUnit.ice b/source/RobotAPI/interface/units/LaserScannerUnit.ice
index 0f1fa77ec8fb39423d0d96c32eeff0d632b97aa6..208f51515387ee99e5dd095719c43482e772fae5 100644
--- a/source/RobotAPI/interface/units/LaserScannerUnit.ice
+++ b/source/RobotAPI/interface/units/LaserScannerUnit.ice
@@ -46,17 +46,18 @@ module armarx
**/
struct LaserScanStep
{
- float angle;
- float distance;
+ float angle;
+ float distance;
+ float intensity;
};
struct LaserScannerInfo
{
- string device;
- string frame;
- float minAngle;
- float maxAngle;
- float stepSize;
+ string device;
+ string frame;
+ float minAngle;
+ float maxAngle;
+ float stepSize;
};
sequence<LaserScanStep> LaserScan;
@@ -64,16 +65,16 @@ module armarx
interface LaserScannerUnitInterface extends armarx::SensorActorUnitInterface
{
- ["cpp:const"]
- idempotent string getReportTopicName() throws NotInitializedException;
+ ["cpp:const"]
+ idempotent string getReportTopicName() throws NotInitializedException;
- ["cpp:const"]
- LaserScannerInfoSeq getConnectedDevices();
+ ["cpp:const"]
+ LaserScannerInfoSeq getConnectedDevices();
};
interface LaserScannerUnitListener
- {
- void reportSensorValues(string device, string name, LaserScan values, TimestampBase timestamp);
+ {
+ void reportSensorValues(string device, string name, LaserScan values, TimestampBase timestamp);
};
interface LaserScannerUnitObserverInterface extends ObserverInterface, LaserScannerUnitListener