diff --git a/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.cpp b/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.cpp
index 8757c0fefae3eb9dcad4f35c9f7ab70e0e4aae04..7ac3f9a052e4f27dc7aea487da40071befa9e762 100644
--- a/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.cpp
+++ b/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.cpp
@@ -34,203 +34,209 @@ 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();
+ 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;
+ }
+ }
}
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');
+ //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
+ }
}
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.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->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;
}
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 << "initializing SickLaserUnit.";
-
- 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.
+ //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.";
}
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();
- }
- */
+ 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 e6c83c4c8893a26aa42a643e781cecdc07100d62..a25ee498266b940297af45ee648e57e5960dea81 100644
--- a/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.h
+++ b/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.h
@@ -45,49 +45,49 @@ 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;
-
-
- 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";
+ 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();
};
/**
* @defgroup Component-SickLaserUnit SickLaserUnit
@@ -100,98 +100,85 @@ namespace armarx
*
* Detailed description of class SickLaserUnit.
*/
- class SickLaserUnit : virtual public armarx::Component
+ class SickLaserUnit :
+ 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;
-
- /* (Requires armarx::LightweightRemoteGuiComponentPluginUser.)
- /// This function should be called once in onConnect() or when you
- /// need to re-create the Remote GUI tab.
- void createRemoteGuiTab();
-
- /// After calling `RemoteGui_startRunningTask`, this function is
- /// called periodically in a separate thread. If you update variables,
- /// make sure to synchronize access to them.
- void RemoteGui_update() override;
- */
+ /// @see armarx::ManagedIceObject::onExitComponent()
+ void onExitComponent() override;
private:
- // Private methods go here.
-
- // Forward declare `Properties` if you used it before its defined.
- // struct Properties;
-
- /* (Requires the armarx::ArVizComponentPluginUser.)
- /// Draw some boxes in ArViz.
- void drawBoxes(const Properties& p, viz::Client& arviz);
- */
+ // Private methods go here.
private:
- // Private member variables go here.
-
- /// Properties shown in the Scenario GUI.
- struct Properties
- {
- //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;
-
- /* 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;
+ //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;
+ */
};
} // namespace armarx
diff --git a/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.cpp b/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.cpp
index 54b6083314fcf759d0a936bad1ba650ded77a484..d288b372f9803fc8ae21a3967f20387d47be3c69 100644
--- a/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.cpp
+++ b/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.cpp
@@ -189,7 +189,7 @@ namespace armarx
SickScanAdapter::SickScanAdapter(const std::string& hostname, const std::string& port, int& timelimit,
- sick_scan::SickGenericParser* parser, char cola_dialect_id)
+ sick_scan::SickGenericParser* parser, char cola_dialect_id, const std::string& topicName)
:
sick_scan::SickScanCommon(parser),
socket_(io_service_),
@@ -197,7 +197,8 @@ namespace armarx
parser_ptr(parser),
hostname_(hostname),
port_(port),
- timelimit_(timelimit)
+ timelimit_(timelimit),
+ topicName(topicName)
{
setEmulSensor(false);
@@ -210,7 +211,6 @@ namespace armarx
{
this->setProtocolType(CoLa_B);
}
-
assert(this->getProtocolType() != CoLa_Unknown);
m_numberOfBytesInReceiveBuffer = 0;
@@ -253,7 +253,7 @@ namespace armarx
ros::Time recvTimeStamp = ros::Time::now(); // timestamp incoming package, will be overwritten by get_datagram
// tcp packet
- ros::Time recvTimeStampPush = ros::Time::now(); // timestamp
+ //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.
@@ -264,7 +264,11 @@ namespace armarx
int result = get_datagram(recvTimeStamp, receiveBuffer, 65536, &actual_length, useBinaryProtocol, &packetsInLoop);
numPacketsProcessed++;
- ros::Duration dur = recvTimeStampPush - recvTimeStamp;
+ //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)
{
@@ -318,7 +322,7 @@ namespace armarx
// HEADER of data followed by DIST1 ... DIST2 ... DIST3 .... RSSI1 ... RSSI2.... RSSI3...
// <frameid>_<sign>00500_DIST[1|2|3]
- success = parseDatagram(dstart, dlength, config_, scanData, scanInfo, true);
+ success = parseDatagram(dstart, dlength, scanData, scanInfo, true);
if (success == sick_scan::ExitSuccess)
{
@@ -340,12 +344,15 @@ namespace armarx
// cloud_.fields[i].count = 1;
// cloud_.fields[i].datatype = sensor_msgs::PointField::FLOAT32;
// }
- cnt++;
- if (cnt == 25)
+ if (scanTopic)
+ {
+ scanTopic->reportSensorValues(ip, "frame1", scanData, now);
+ }
+ else
{
- ARMARX_INFO_S << "last measurement: (" << scanData.back().angle << ", " << scanData.back().distance << ")";
- cnt = 0;
+ ARMARX_WARNING << "No scan topic available: IP: " << ip << ", Port: " << port;
}
+
}
// Start Point
if (dend != NULL)
@@ -359,8 +366,7 @@ namespace armarx
}
//Adapted from sick_generic_parser
- int SickScanAdapter::parseDatagram(char* datagram, size_t datagram_length, sick_scan::SickScanConfig& config,
- LaserScan& scanData, LaserScannerInfo& scanInfo, bool updateScannerInfo)
+ int SickScanAdapter::parseDatagram(char* datagram, size_t datagram_length, LaserScan& scanData, LaserScannerInfo& scanInfo, bool updateScannerInfo)
{
int HEADER_FIELDS = 32;
char* cur_field;
@@ -448,7 +454,7 @@ namespace armarx
}
}
- ros::Time start_time = ros::Time::now(); // will be adjusted in the end
+ //ros::Time start_time = ros::Time::now(); // will be adjusted in the end
// <STX> (\x02)
// 0: Type of command (SN)
@@ -581,6 +587,7 @@ namespace armarx
LaserScanStep step;
step.angle = i * scanInfo.stepSize;
step.distance = distVal[i];
+ step.intensity = intensityVal[i];
scanData.push_back(step);
}
diff --git a/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.h b/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.h
index 387a558abead377e3bbdf739fd8c321f7c675cda..5fff69513a24886b39d7ea963977867962c876e9 100644
--- a/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.h
+++ b/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.h
@@ -68,13 +68,14 @@ namespace armarx
};
- class SickScanAdapter : public sick_scan::SickScanCommon
+ class SickScanAdapter :
+ virtual public armarx::LaserScannerUnitInterface,
+ public sick_scan::SickScanCommon
{
public:
int loopOnce(LaserScan& scanData);
- int parseDatagram(char* datagram, size_t datagram_length, sick_scan::SickScanConfig& config,
- LaserScan& scanData, LaserScannerInfo& scanInfo, bool updateScannerInfo = false);
+ int parseDatagram(char* datagram, size_t datagram_length, LaserScan& scanData, LaserScannerInfo& scanInfo, bool updateScannerInfo = false);
static void disconnectFunctionS(void* obj);
@@ -172,6 +173,9 @@ namespace armarx
std::string port_;
int timelimit_;
int m_replyMode;
+
+ std::string topicName;
+ LaserScannerUnitListenerPrx scanTopic;
};
}