From 23f2e453628ea4249b76067752cf9c34e831fbe2 Mon Sep 17 00:00:00 2001
From: Johann Mantel <j-mantel@gmx.net>
Date: Wed, 28 Jul 2021 14:25:56 +0200
Subject: [PATCH] fix pointer problem
---
.../drivers/SickLaserUnit/SickLaserUnit.cpp | 3 +-
.../drivers/SickLaserUnit/SickLaserUnit.h | 1 +
.../drivers/SickLaserUnit/SickScanAdapter.cpp | 464 ++++++++++++++++--
3 files changed, 437 insertions(+), 31 deletions(-)
diff --git a/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.cpp b/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.cpp
index 1cc0f69d9..fbfe25bdd 100644
--- a/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.cpp
+++ b/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.cpp
@@ -163,7 +163,7 @@ namespace armarx
<< " (split size: " << deviceInfo.size() << ", expected: 3)";
continue;
}
- SickLaserScanDevice device;
+ SickLaserScanDevice& device = scanDevices.emplace_back();
device.scanTopic = topic;
device.isSensorInitialized = false;
device.frameName = deviceInfo[0];
@@ -192,7 +192,6 @@ namespace armarx
{
ARMARX_ERROR_S << "Could not create parser. Wrong Scanner name.";
return;
- scanDevices.push_back(device);
}
//addPlugin(heartbeat);
diff --git a/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.h b/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.h
index 2829a8e8f..8ea76934d 100644
--- a/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.h
+++ b/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.h
@@ -132,6 +132,7 @@ namespace armarx
//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;
diff --git a/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.cpp b/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.cpp
index 0d2e9368b..54f94e7eb 100644
--- a/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.cpp
+++ b/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.cpp
@@ -154,9 +154,10 @@ namespace armarx
unsigned char receiveBuffer[65536];
int actual_length = 0;
int packetsInLoop = 0;
+ bool useBinaryProtocol = parser_ptr->getCurrentParamPtr()->getUseBinaryProtocol();
ros::Time recvTimeStamp = ros::Time::now(); // timestamp incoming package, will be overwritten by get_datagram
- int result = get_datagram(recvTimeStamp, receiveBuffer, 65536, &actual_length, false, &packetsInLoop);
+ int result = get_datagram(recvTimeStamp, receiveBuffer, 65536, &actual_length, useBinaryProtocol, &packetsInLoop);
//ros::Duration dur = recvTimeStampPush - recvTimeStamp;
if (result != 0)
{
@@ -186,37 +187,442 @@ namespace armarx
while (dataToProcess)
{
- 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)
+ /*
+ 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)
{
- ARMARX_WARNING << "parseDatagram returned ErrorCode: " << success;
+ 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;
+
+
+ }
+ }
}
- // Start Point
- if (dend != NULL)
+
+ success = sick_scan::ExitSuccess;
+ // change Parsing Mode
+ dataToProcess = false; // only one package allowed - no chaining
+ }
+ else // Parsing of Ascii-Encoding of datagram, xxx
+ */
{
- buffer_pos = dend + 1;
+
+ 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;
+ }
}
} // end of while loop
return sick_scan::ExitSuccess; // return success to continue looping
--
GitLab