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

remove code for binary parsing to reduce complexity

parent fdf75c43
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.h
+ 85
− 86
View file @ 4cf007c1
......@@ -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
source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.cpp
+ 28
− 433
View file @ 4cf007c1
......@@ -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
......
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