Skip to content
Snippets Groups Projects
Commit 23f2e453 authored by Johann Mantel's avatar Johann Mantel
Browse files

fix pointer problem

parent 87322223
No related branches found
No related tags found
1 merge request!185Clean up interfaces and unneeded code in memory core classes
Hide whitespace changes
Inline Side-by-side
source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.cpp
+ 1
− 2
View file @ 23f2e453
......@@ -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);
......
source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.h
+ 1
− 0
View file @ 23f2e453
......@@ -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;
......
source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.cpp
+ 435
− 29
View file @ 23f2e453
......@@ -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
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment