diff --git a/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.h b/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.h
index 8ea76934d685f3b3a7b078dc76d0cb3250a699f8..9bbc4fa64f50b417a0f93330464167c472d0f5b9 100644
--- a/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.h
+++ b/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.h
@@ -41,45 +41,45 @@ 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";
- //communication parameters
- std::string ip;
- std::string port;
- int timelimit = 5;
- double rangeMin = 0.0;
- double rangeMax = 10.0;
- bool useTcp = false;
- //data and task pointers
- IceUtil::Time scanTime;
- LaserScan scanData;
- LaserScannerInfo scanInfo;
- int initCnt = 0;
- RunState runState = RunState::scannerFinalize;
- RunningTask<SickLaserScanDevice>::pointer_type task;
- std::string frameName = "LaserScannerFront";
- LaserScannerUnitListenerPrx scanTopic;
- //scanner pointers
- sick_scan::SickGenericParser* parser;
- SickScanAdapter* scanner;
- int result = sick_scan::ExitError;
- bool isSensorInitialized = false;
-
- void initScanner();
- void run();
+ //scanner parameters
+ std::string scannerType = "sick_tim_5xx";
+ //communication parameters
+ std::string ip;
+ std::string port;
+ int timelimit = 5;
+ double rangeMin = 0.0;
+ double rangeMax = 10.0;
+ bool useTcp = false;
+ //data and task pointers
+ IceUtil::Time scanTime;
+ LaserScan scanData;
+ LaserScannerInfo scanInfo;
+ int initCnt = 0;
+ RunState runState = RunState::scannerFinalize;
+ RunningTask<SickLaserScanDevice>::pointer_type task;
+ std::string frameName = "LaserScannerFront";
+ LaserScannerUnitListenerPrx scanTopic;
+ //scanner pointers
+ sick_scan::SickGenericParser* parser;
+ SickScanAdapter* scanner;
+ int result = sick_scan::ExitError;
+ bool isSensorInitialized = false;
+
+ void initScanner();
+ void run();
};
/**
* @defgroup Component-SickLaserUnit SickLaserUnit
@@ -94,75 +94,74 @@ namespace armarx
*/
class SickLaserUnit :
//virtual public armarx::LaserScannerUnitInterface,
- virtual public armarx::Component
+ 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 = "SICKLaserScanner";
- //scanner parameters
- std::string devices = "LaserScannerFront,192.168.8.133,2112";
- std::string scannerType = "sick_tim_5xx";
- ScanProtocol protocol = ScanProtocol::ASCII;
- int timelimit = 5;
- double rangeMin = 0.0;
- double rangeMax = 10.0;
- };
- Properties properties;
- std::vector<SickLaserScanDevice> scanDevices;
- 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 devices = "LaserScannerFront,192.168.8.133,2112";
+ std::string scannerType = "sick_tim_5xx";
+ int timelimit = 5;
+ double rangeMin = 0.0;
+ double rangeMax = 10.0;
+ };
+ Properties properties;
+ std::vector<SickLaserScanDevice> scanDevices;
+ 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 54f94e7eb508f995585650332904c53043b5c7ee..69d33ced49a00c415f7e08456d21177c27dfb8b6 100644
--- a/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.cpp
+++ b/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.cpp
@@ -187,442 +187,37 @@ namespace armarx
while (dataToProcess)
{
- /*
- if (useBinaryProtocol)
- {
- // if binary protocol used then parse binary message
- std::vector<unsigned char> receiveBufferVec = std::vector<unsigned char>(receiveBuffer,
- receiveBuffer + actual_length);
- if (receiveBufferVec.size() > 8)
+ size_t dlength;
+ int success = -1;
+ // Always Parsing Ascii-Encoding of datagram
+ dstart = strchr(buffer_pos, 0x02);
+ if (dstart != NULL)
{
- long idVal = 0;
- long lenVal = 0;
- memcpy(&idVal, receiveBuffer + 0, 4); // read identifier
- memcpy(&lenVal, receiveBuffer + 4, 4); // read length indicator
- swap_endian((unsigned char *) &lenVal, 4);
-
- if (idVal == 0x02020202) // id for binary message
- {
- // binary message
- if (lenVal < actual_length)
- {
- short elevAngleX200 = 0; // signed short (F5 B2 -> Layer 24
- // F5B2h -> -2638/200= -13.19°
- int scanFrequencyX100 = 0;
- double elevAngle = 0.00;
- double scanFrequency = 0.0;
- long measurementFrequencyDiv100 = 0; // multiply with 100
- int numOfEncoders = 0;
- int numberOf16BitChannels = 0;
- int numberOf8BitChannels = 0;
- uint32_t SystemCountScan = 0;
- uint32_t SystemCountTransmit = 0;
-
- memcpy(&elevAngleX200, receiveBuffer + 50, 2);
- swap_endian((unsigned char *) &elevAngleX200, 2);
-
- memcpy(&SystemCountScan, receiveBuffer + 0x26, 4);
- swap_endian((unsigned char *) &SystemCountScan, 4);
-
- memcpy(&SystemCountTransmit, receiveBuffer + 0x2A, 4);
- swap_endian((unsigned char *) &SystemCountTransmit, 4);
-
- memcpy(&scanFrequencyX100, receiveBuffer + 52, 4);
- swap_endian((unsigned char *) &scanFrequencyX100, 4);
-
- memcpy(&measurementFrequencyDiv100, receiveBuffer + 56, 4);
- swap_endian((unsigned char *) &measurementFrequencyDiv100, 4);
-
- float scan_time = 1.0 / (scanFrequencyX100 / 100.0);
-
- //due firmware inconsistency
- if (measurementFrequencyDiv100 > 10000)
- {
- measurementFrequencyDiv100 /= 100;
- }
- float time_increment = 1.0 / (measurementFrequencyDiv100 * 100.0);
- timeIncrement = time_increment;
-
- memcpy(&numOfEncoders, receiveBuffer + 60, 2);
- swap_endian((unsigned char *) &numOfEncoders, 2);
- int encoderDataOffset = 6 * numOfEncoders;
- int32_t EncoderPosTicks[4] = {0};
- int16_t EncoderSpeed[4] = {0};
-
- if (numOfEncoders > 0 && numOfEncoders < 5)
- {
- FireEncoder = true;
- for (int EncoderNum = 0; EncoderNum < numOfEncoders; EncoderNum++)
- {
- memcpy(&EncoderPosTicks[EncoderNum], receiveBuffer + 62 + EncoderNum * 6, 4);
- swap_endian((unsigned char *) &EncoderPosTicks[EncoderNum], 4);
- memcpy(&EncoderSpeed[EncoderNum], receiveBuffer + 66 + EncoderNum * 6, 2);
- swap_endian((unsigned char *) &EncoderSpeed[EncoderNum], 2);
- }
- }
- //TODO handle multi encoder with multiple encode msg or different encoder msg definition now using only first encoder
- EncoderMsg.enc_position = EncoderPosTicks[0];
- EncoderMsg.enc_speed = EncoderSpeed[0];
- memcpy(&numberOf16BitChannels, receiveBuffer + 62 + encoderDataOffset, 2);
- swap_endian((unsigned char *) &numberOf16BitChannels, 2);
-
- int parseOff = 64 + encoderDataOffset;
-
- char szChannel[255] = {0};
- float scaleFactor = 1.0;
- float scaleFactorOffset = 0.0;
- int32_t startAngleDiv10000 = 1;
- int32_t sizeOfSingleAngularStepDiv10000 = 1;
- double startAngle = 0.0;
- double sizeOfSingleAngularStep = 0.0;
- short numberOfItems = 0;
-
- static int cnt = 0;
- cnt++;
- // get number of 8 bit channels
- // we must jump of the 16 bit data blocks including header ...
- for (int i = 0; i < numberOf16BitChannels; i++)
- {
- int numberOfItems = 0x00;
- memcpy(&numberOfItems, receiveBuffer + parseOff + 19, 2);
- swap_endian((unsigned char *) &numberOfItems, 2);
- parseOff += 21; // 21 Byte header followed by data entries
- parseOff += numberOfItems * 2;
- }
-
- // now we can read the number of 8-Bit-Channels
- memcpy(&numberOf8BitChannels, receiveBuffer + parseOff, 2);
- swap_endian((unsigned char *) &numberOf8BitChannels, 2);
-
- parseOff = 64 + encoderDataOffset;
- enum datagram_parse_task
- {
- process_dist,
- process_vang,
- process_rssi,
- process_idle
- };
- int rssiCnt = 0;
- int vangleCnt = 0;
- int distChannelCnt = 0;
-
- for (int processLoop = 0; processLoop < 2; processLoop++)
- {
- int totalChannelCnt = 0;
-
-
- bool bCont = true;
-
- datagram_parse_task task = process_idle;
- bool parsePacket = true;
- parseOff = 64 + encoderDataOffset;
- bool processData = false;
-
- if (processLoop == 0)
- {
- distChannelCnt = 0;
- rssiCnt = 0;
- vangleCnt = 0;
- }
-
- if (processLoop == 1)
- {
- processData = true;
- numEchos = distChannelCnt;
- ranges.resize(numberOfItems * numEchos);
- if (rssiCnt > 0)
- {
- intensities.resize(numberOfItems * rssiCnt);
- }
- else
- {
- }
- if (vangleCnt > 0) // should be 0 or 1
- {
- vang_vec.resize(numberOfItems * vangleCnt);
- }
- else
- {
- vang_vec.clear();
- }
- echoMask = (1 << numEchos) - 1;
-
- // reset count. We will use the counter for index calculation now.
- distChannelCnt = 0;
- rssiCnt = 0;
- vangleCnt = 0;
-
- }
-
- szChannel[6] = '\0';
- scaleFactor = 1.0;
- scaleFactorOffset = 0.0;
- startAngleDiv10000 = 1;
- sizeOfSingleAngularStepDiv10000 = 1;
- startAngle = 0.0;
- sizeOfSingleAngularStep = 0.0;
- numberOfItems = 0;
-
-
- #if 1 // prepared for multiecho parsing
-
- bCont = true;
- bool doVangVecProc = false;
- // try to get number of DIST and RSSI from binary data
- task = process_idle;
- do
- {
- task = process_idle;
- doVangVecProc = false;
- int processDataLenValuesInBytes = 2;
-
- if (totalChannelCnt == numberOf16BitChannels)
- {
- parseOff += 2; // jump of number of 8 bit channels- already parsed above
- }
-
- if (totalChannelCnt >= numberOf16BitChannels)
- {
- processDataLenValuesInBytes = 1; // then process 8 bit values ...
- }
- bCont = false;
- strcpy(szChannel, "");
-
- if (totalChannelCnt < (numberOf16BitChannels + numberOf8BitChannels))
- {
- szChannel[5] = '\0';
- strncpy(szChannel, (const char *) receiveBuffer + parseOff, 5);
- }
- else
- {
- // all channels processed (16 bit and 8 bit channels)
- }
-
- if (strstr(szChannel, "DIST") == szChannel)
- {
- task = process_dist;
- distChannelCnt++;
- bCont = true;
- numberOfItems = 0;
- memcpy(&numberOfItems, receiveBuffer + parseOff + 19, 2);
- swap_endian((unsigned char *) &numberOfItems, 2);
-
- }
- if (strstr(szChannel, "VANG") == szChannel)
- {
- vangleCnt++;
- task = process_vang;
- bCont = true;
- numberOfItems = 0;
- memcpy(&numberOfItems, receiveBuffer + parseOff + 19, 2);
- swap_endian((unsigned char *) &numberOfItems, 2);
-
- vang_vec.resize(numberOfItems);
-
- }
- if (strstr(szChannel, "RSSI") == szChannel)
- {
- task = process_rssi;
- rssiCnt++;
- bCont = true;
- numberOfItems = 0;
- // copy two byte value (unsigned short to numberOfItems
- memcpy(&numberOfItems, receiveBuffer + parseOff + 19, 2);
- swap_endian((unsigned char *) &numberOfItems, 2); // swap
-
- }
- if (bCont)
- {
- scaleFactor = 0.0;
- scaleFactorOffset = 0.0;
- startAngleDiv10000 = 0;
- sizeOfSingleAngularStepDiv10000 = 0;
- numberOfItems = 0;
-
- memcpy(&scaleFactor, receiveBuffer + parseOff + 5, 4);
- memcpy(&scaleFactorOffset, receiveBuffer + parseOff + 9, 4);
- memcpy(&startAngleDiv10000, receiveBuffer + parseOff + 13, 4);
- memcpy(&sizeOfSingleAngularStepDiv10000, receiveBuffer + parseOff + 17, 2);
- memcpy(&numberOfItems, receiveBuffer + parseOff + 19, 2);
-
-
- swap_endian((unsigned char *) &scaleFactor, 4);
- swap_endian((unsigned char *) &scaleFactorOffset, 4);
- swap_endian((unsigned char *) &startAngleDiv10000, 4);
- swap_endian((unsigned char *) &sizeOfSingleAngularStepDiv10000, 2);
- swap_endian((unsigned char *) &numberOfItems, 2);
-
- if (processData)
- {
- unsigned short *data = (unsigned short *) (receiveBuffer + parseOff + 21);
-
- unsigned char *swapPtr = (unsigned char *) data;
- // copy RSSI-Values +2 for 16-bit values +1 for 8-bit value
- for (int i = 0;
- i < numberOfItems * processDataLenValuesInBytes; i += processDataLenValuesInBytes)
- {
- if (processDataLenValuesInBytes == 1)
- {
- }
- else
- {
- unsigned char tmp;
- tmp = swapPtr[i + 1];
- swapPtr[i + 1] = swapPtr[i];
- swapPtr[i] = tmp;
- }
- }
- int idx = 0;
-
- switch (task)
- {
-
- case process_dist:
- {
- startAngle = startAngleDiv10000 / 10000.00;
- sizeOfSingleAngularStep = sizeOfSingleAngularStepDiv10000 / 10000.0;
- sizeOfSingleAngularStep *= (M_PI / 180.0);
-
- msg.angle_min = startAngle / 180.0 * M_PI - M_PI / 2;
- msg.angle_increment = sizeOfSingleAngularStep;
- msg.angle_max = msg.angle_min + (numberOfItems - 1) * msg.angle_increment;
-
- if (this->parser_->getCurrentParamPtr()->getScanMirroredAndShifted())
- {
- msg.angle_min -= M_PI/2;
- msg.angle_max -= M_PI/2;
-
- msg.angle_min *= -1.0;
- msg.angle_increment *= -1.0;
- msg.angle_max *= -1.0;
-
- }
- float *rangePtr = NULL;
-
- if (numberOfItems > 0)
- {
- rangePtr = &msg.ranges[0];
- }
- float scaleFactor_001 = 0.001F * scaleFactor;// to avoid repeated multiplication
- for (int i = 0; i < numberOfItems; i++)
- {
- idx = i + numberOfItems * (distChannelCnt - 1);
- rangePtr[idx] = (float) data[i] * scaleFactor_001 + scaleFactorOffset;
- #ifdef DEBUG_DUMP_ENABLED
- if (distChannelCnt == 1)
- {
- if (i == floor(numberOfItems / 2))
- {
- double curTimeStamp = SystemCountScan + i * msg.time_increment * 1E6;
- //DataDumper::instance().pushData(curTimeStamp, "DIST", rangePtr[idx]);
- }
- }
- #endif
- //XXX
- }
-
- }
- break;
- case process_rssi:
- {
- // Das muss vom Protokoll abgeleitet werden. !!!
-
- float *intensityPtr = NULL;
-
- if (numberOfItems > 0)
- {
- intensityPtr = &msg.intensities[0];
-
- }
- for (int i = 0; i < numberOfItems; i++)
- {
- idx = i + numberOfItems * (rssiCnt - 1);
- // we must select between 16 bit and 8 bit values
- float rssiVal = 0.0;
- if (processDataLenValuesInBytes == 2)
- {
- rssiVal = (float) data[i];
- }
- else
- {
- unsigned char *data8Ptr = (unsigned char *) data;
- rssiVal = (float) data8Ptr[i];
- }
- intensityPtr[idx] = rssiVal * scaleFactor + scaleFactorOffset;
- }
- }
- break;
-
- case process_vang:
- float *vangPtr = NULL;
- if (numberOfItems > 0)
- {
- vangPtr = &vang_vec[0]; // much faster, with vang_vec[i] each time the size will be checked
- }
- for (int i = 0; i < numberOfItems; i++)
- {
- vangPtr[i] = (float) data[i] * scaleFactor + scaleFactorOffset;
- }
- break;
- }
- }
- parseOff += 21 + processDataLenValuesInBytes * numberOfItems;
-
-
- }
- totalChannelCnt++;
- } while (bCont);
- }
- #endif
-
- elevAngle = elevAngleX200 / 200.0;
- scanFrequency = scanFrequencyX100 / 100.0;
-
-
- }
- }
+ dend = strchr(dstart + 1, 0x03);
}
-
- success = sick_scan::ExitSuccess;
- // change Parsing Mode
- dataToProcess = false; // only one package allowed - no chaining
- }
- else // Parsing of Ascii-Encoding of datagram, xxx
- */
+ if ((dstart != NULL) && (dend != NULL))
{
-
- size_t dlength;
- int success = -1;
- // 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, updateScannerInfo);
- if (success != sick_scan::ExitSuccess)
- {
- ARMARX_WARNING << "parseDatagram returned ErrorCode: " << success;
- }
- // Start Point
- if (dend != NULL)
- {
- buffer_pos = dend + 1;
- }
+ 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, updateScannerInfo);
+ if (success != sick_scan::ExitSuccess)
+ {
+ 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