Skip to content
Snippets Groups Projects
Commit bfb10b5a authored by armar-user's avatar armar-user
Browse files

Merge branch 'master' of gitlab.com:ArmarX/RobotAPI

parents e732c5fc 4cf007c1
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.h
+ 85
− 86
View file @ bfb10b5a
...@@ -41,45 +41,45 @@ namespace armarx ...@@ -41,45 +41,45 @@ namespace armarx
enum class ScanProtocol enum class ScanProtocol
{ {
ASCII, ASCII,
Binary Binary
}; };
enum class RunState enum class RunState
{ {
scannerInit, scannerInit,
scannerRun, scannerRun,
scannerFinalize scannerFinalize
}; };
struct SickLaserScanDevice struct SickLaserScanDevice
{ {
//scanner parameters //scanner parameters
std::string scannerType = "sick_tim_5xx"; std::string scannerType = "sick_tim_5xx";
//communication parameters //communication parameters
std::string ip; std::string ip;
std::string port; std::string port;
int timelimit = 5; int timelimit = 5;
double rangeMin = 0.0; double rangeMin = 0.0;
double rangeMax = 10.0; double rangeMax = 10.0;
bool useTcp = false; bool useTcp = false;
//data and task pointers //data and task pointers
IceUtil::Time scanTime; IceUtil::Time scanTime;
LaserScan scanData; LaserScan scanData;
LaserScannerInfo scanInfo; LaserScannerInfo scanInfo;
int initCnt = 0; int initCnt = 0;
RunState runState = RunState::scannerFinalize; RunState runState = RunState::scannerFinalize;
RunningTask<SickLaserScanDevice>::pointer_type task; RunningTask<SickLaserScanDevice>::pointer_type task;
std::string frameName = "LaserScannerFront"; std::string frameName = "LaserScannerFront";
LaserScannerUnitListenerPrx scanTopic; LaserScannerUnitListenerPrx scanTopic;
//scanner pointers //scanner pointers
sick_scan::SickGenericParser* parser; sick_scan::SickGenericParser* parser;
SickScanAdapter* scanner; SickScanAdapter* scanner;
int result = sick_scan::ExitError; int result = sick_scan::ExitError;
bool isSensorInitialized = false; bool isSensorInitialized = false;
void initScanner(); void initScanner();
void run(); void run();
}; };
/** /**
* @defgroup Component-SickLaserUnit SickLaserUnit * @defgroup Component-SickLaserUnit SickLaserUnit
...@@ -94,75 +94,74 @@ namespace armarx ...@@ -94,75 +94,74 @@ namespace armarx
*/ */
class SickLaserUnit : class SickLaserUnit :
//virtual public armarx::LaserScannerUnitInterface, //virtual public armarx::LaserScannerUnitInterface,
virtual public armarx::Component virtual public armarx::Component
// , virtual public armarx::DebugObserverComponentPluginUser // , virtual public armarx::DebugObserverComponentPluginUser
// , virtual public armarx::LightweightRemoteGuiComponentPluginUser // , virtual public armarx::LightweightRemoteGuiComponentPluginUser
// , virtual public armarx::ArVizComponentPluginUser // , virtual public armarx::ArVizComponentPluginUser
{ {
public: public:
/// @see armarx::ManagedIceObject::getDefaultName() /// @see armarx::ManagedIceObject::getDefaultName()
std::string getDefaultName() const override; std::string getDefaultName() const override;
protected: protected:
/// @see PropertyUser::createPropertyDefinitions() /// @see PropertyUser::createPropertyDefinitions()
armarx::PropertyDefinitionsPtr createPropertyDefinitions() override; armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
/// @see armarx::ManagedIceObject::onInitComponent() /// @see armarx::ManagedIceObject::onInitComponent()
void onInitComponent() override; void onInitComponent() override;
/// @see armarx::ManagedIceObject::onConnectComponent() /// @see armarx::ManagedIceObject::onConnectComponent()
void onConnectComponent() override; void onConnectComponent() override;
/// @see armarx::ManagedIceObject::onDisconnectComponent() /// @see armarx::ManagedIceObject::onDisconnectComponent()
void onDisconnectComponent() override; void onDisconnectComponent() override;
/// @see armarx::ManagedIceObject::onExitComponent() /// @see armarx::ManagedIceObject::onExitComponent()
void onExitComponent() override; void onExitComponent() override;
private: private:
// Private methods go here. // Private methods go here.
private: private:
// Private member variables go here. // Private member variables go here.
/// Properties shown in the Scenario GUI. /// Properties shown in the Scenario GUI.
struct Properties struct Properties
{ {
std::string topicName = "SICKLaserScanner"; std::string topicName = "SICKLaserScanner";
//scanner parameters //scanner parameters
std::string devices = "LaserScannerFront,192.168.8.133,2112"; std::string devices = "LaserScannerFront,192.168.8.133,2112";
std::string scannerType = "sick_tim_5xx"; std::string scannerType = "sick_tim_5xx";
ScanProtocol protocol = ScanProtocol::ASCII; int timelimit = 5;
int timelimit = 5; double rangeMin = 0.0;
double rangeMin = 0.0; double rangeMax = 10.0;
double rangeMax = 10.0; };
}; Properties properties;
Properties properties; std::vector<SickLaserScanDevice> scanDevices;
std::vector<SickLaserScanDevice> scanDevices; LaserScannerUnitListenerPrx topic;
LaserScannerUnitListenerPrx topic; //HeartbeatComponentPlugin heartbeat;
//HeartbeatComponentPlugin heartbeat;
/* Use a mutex if you access variables from different threads
/* Use a mutex if you access variables from different threads * (e.g. ice functions and RemoteGui_update()).
* (e.g. ice functions and RemoteGui_update()). std::mutex propertiesMutex;
std::mutex propertiesMutex; */
*/
/* (Requires the armarx::LightweightRemoteGuiComponentPluginUser.)
/* (Requires the armarx::LightweightRemoteGuiComponentPluginUser.) /// Tab shown in the Remote GUI.
/// Tab shown in the Remote GUI. struct RemoteGuiTab : armarx::RemoteGui::Client::Tab
struct RemoteGuiTab : armarx::RemoteGui::Client::Tab {
{ armarx::RemoteGui::Client::LineEdit boxLayerName;
armarx::RemoteGui::Client::LineEdit boxLayerName; armarx::RemoteGui::Client::IntSpinBox numBoxes;
armarx::RemoteGui::Client::IntSpinBox numBoxes;
armarx::RemoteGui::Client::Button drawBoxes;
armarx::RemoteGui::Client::Button drawBoxes; };
}; RemoteGuiTab tab;
RemoteGuiTab tab; */
*/
/* (Requires the armarx::ArVizComponentPluginUser.)
/* (Requires the armarx::ArVizComponentPluginUser.) * When used from different threads, an ArViz client needs to be synchronized.
* When used from different threads, an ArViz client needs to be synchronized. /// Protects the arviz client inherited from the ArViz plugin.
/// Protects the arviz client inherited from the ArViz plugin. std::mutex arvizMutex;
std::mutex arvizMutex; */
*/
}; };
} // namespace armarx } // namespace armarx
source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.cpp
+ 28
− 433
View file @ bfb10b5a
...@@ -187,442 +187,37 @@ namespace armarx ...@@ -187,442 +187,37 @@ namespace armarx
while (dataToProcess) while (dataToProcess)
{ {
/* size_t dlength;
if (useBinaryProtocol) int success = -1;
{ // Always Parsing Ascii-Encoding of datagram
// if binary protocol used then parse binary message dstart = strchr(buffer_pos, 0x02);
std::vector<unsigned char> receiveBufferVec = std::vector<unsigned char>(receiveBuffer, if (dstart != NULL)
receiveBuffer + actual_length);
if (receiveBufferVec.size() > 8)
{ {
long idVal = 0; dend = strchr(dstart + 1, 0x03);
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;
}
}
} }
if ((dstart != NULL) && (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
*/
{ {
dataToProcess = true; // continue parsing
size_t dlength; dlength = dend - dstart;
int success = -1; *dend = '\0';
// Always Parsing Ascii-Encoding of datagram dstart++;
dstart = strchr(buffer_pos, 0x02); }
if (dstart != NULL) else
{ {
dend = strchr(dstart + 1, 0x03); dataToProcess = false;
} break;
if ((dstart != NULL) && (dend != NULL)) }
{ // HEADER of data followed by DIST1 ... DIST2 ... DIST3 .... RSSI1 ... RSSI2.... RSSI3...
dataToProcess = true; // continue parsing // <frameid>_<sign>00500_DIST[1|2|3]
dlength = dend - dstart; success = parseDatagram(dstart, dlength, scanData, scanInfo, updateScannerInfo);
*dend = '\0'; if (success != sick_scan::ExitSuccess)
dstart++; {
} ARMARX_WARNING << "parseDatagram returned ErrorCode: " << success;
else }
{ // Start Point
dataToProcess = false; if (dend != NULL)
break; {
} buffer_pos = dend + 1;
// 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 } // end of while loop
return sick_scan::ExitSuccess; // return success to continue looping 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