From 5d917c59fbaddfe2e89ee4d2c56c2f8c7aade43b Mon Sep 17 00:00:00 2001
From: Johann Mantel <j-mantel@gmx.net>
Date: Thu, 15 Jul 2021 17:37:07 +0200
Subject: [PATCH] remove all the unnecessary code from loopOnce()
---
.../drivers/SickLaserUnit/SickLaserUnit.cpp | 2 +-
.../drivers/SickLaserUnit/SickLaserUnit.h | 6 +-
.../drivers/SickLaserUnit/SickScanAdapter.cpp | 2675 +++++++----------
.../drivers/SickLaserUnit/SickScanAdapter.h | 7 +-
4 files changed, 1024 insertions(+), 1666 deletions(-)
diff --git a/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.cpp b/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.cpp
index 1897cbf13..092cecf19 100644
--- a/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.cpp
+++ b/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.cpp
@@ -45,7 +45,7 @@ namespace armarx
if (result == sick_scan::ExitSuccess) // OK -> loop again
{
ARMARX_INFO_S << "looping";
- result = scanner->loopOnce();
+ result = scanner->loopOnce(scanData);
ARMARX_INFO_S << "finished looping";
}
else
diff --git a/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.h b/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.h
index 4990eb80c..e6c83c4c8 100644
--- a/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.h
+++ b/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.h
@@ -26,6 +26,7 @@
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/services/tasks/RunningTask.h>
+#include <RobotAPI/interface/units/LaserScannerUnit.h>
// #include <ArmarXCore/libraries/ArmarXCoreComponentPlugins/DebugObserverComponentPlugin.h>
// #include <ArmarXGui/libraries/ArmarXGuiComponentPlugins/LightweightRemoteGuiComponentPlugin.h>
@@ -72,9 +73,12 @@ namespace armarx
bool useTcp = false;
char colaDialectId = 'A';
//data and task pointers
- std::vector<long> scanData;
+ std::vector<long> lengthData;
+ LaserScan scanData;
RunState runState = RunState::scannerFinalize;
RunningTask<SickLaserScanDevice>::pointer_type task;
+
+
sick_scan::SickScanConfig cfg;
SickScanAdapter* scanner;
//sick_scan::SickScanCommonTcp* scanner;
diff --git a/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.cpp b/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.cpp
index 446676544..ae45d3132 100644
--- a/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.cpp
+++ b/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.cpp
@@ -86,7 +86,7 @@
std::vector<unsigned char> exampleData(65536);
std::vector<unsigned char> receivedData(65536);
-static long receivedDataLen = 0;
+//static long receivedDataLen = 0;
static int getDiagnosticErrorCode()
{
@@ -102,134 +102,134 @@ namespace armarx
{
bool emulateReply(UINT8* requestData, int requestLen, std::vector<unsigned char>* replyVector)
{
- std::string request;
- std::string reply;
- std::vector<std::string> keyWordList;
- std::vector<std::string> answerList;
+ std::string request;
+ std::string reply;
+ std::vector<std::string> keyWordList;
+ std::vector<std::string> answerList;
- std::string scannerType = "???";
- ros::NodeHandle nhPriv;
+ std::string scannerType = "???";
+ ros::NodeHandle nhPriv;
- enum
- {
- SIMU_RADAR = 0, SIMU_MRS_1XXX = 1, SIMU_NUM
- };
- // XXX
- keyWordList.push_back("sMN SetAccessMode");
- answerList.push_back("sAN SetAccessMode 1");
+ enum
+ {
+ SIMU_RADAR = 0, SIMU_MRS_1XXX = 1, SIMU_NUM
+ };
+ // XXX
+ keyWordList.push_back("sMN SetAccessMode");
+ answerList.push_back("sAN SetAccessMode 1");
- keyWordList.push_back("sWN EIHstCola");
- answerList.push_back("sWA EIHstCola");
+ keyWordList.push_back("sWN EIHstCola");
+ answerList.push_back("sWA EIHstCola");
- keyWordList.push_back("sRN FirmwareVersion");
- answerList.push_back("sRA FirmwareVersion 8 1.0.0.0R");
+ keyWordList.push_back("sRN FirmwareVersion");
+ answerList.push_back("sRA FirmwareVersion 8 1.0.0.0R");
- keyWordList.push_back("sRN OrdNum");
- answerList.push_back("sRA OrdNum 7 1234567");
+ keyWordList.push_back("sRN OrdNum");
+ answerList.push_back("sRA OrdNum 7 1234567");
- keyWordList.push_back("sWN TransmitTargets 1");
- answerList.push_back("sWA TransmitTargets");
+ keyWordList.push_back("sWN TransmitTargets 1");
+ answerList.push_back("sWA TransmitTargets");
- keyWordList.push_back("sWN TransmitObjects 1");
- answerList.push_back("sWA TransmitObjects");
+ keyWordList.push_back("sWN TransmitObjects 1");
+ answerList.push_back("sWA TransmitObjects");
- keyWordList.push_back("sWN TCTrackingMode 0");
- answerList.push_back("sWA TCTrackingMode");
+ keyWordList.push_back("sWN TCTrackingMode 0");
+ answerList.push_back("sWA TCTrackingMode");
- keyWordList.push_back("sRN SCdevicestate");
- answerList.push_back("sRA SCdevicestate 1");
+ keyWordList.push_back("sRN SCdevicestate");
+ answerList.push_back("sRA SCdevicestate 1");
- keyWordList.push_back("sRN DItype");
- answerList.push_back("sRA DItype D RMS3xx-xxxxxx");
+ keyWordList.push_back("sRN DItype");
+ answerList.push_back("sRA DItype D RMS3xx-xxxxxx");
- keyWordList.push_back("sRN ODoprh");
- answerList.push_back("sRA ODoprh 451");
+ keyWordList.push_back("sRN ODoprh");
+ answerList.push_back("sRA ODoprh 451");
- keyWordList.push_back("sMN mSCloadappdef");
- answerList.push_back("sAN mSCloadappdef");
+ keyWordList.push_back("sMN mSCloadappdef");
+ answerList.push_back("sAN mSCloadappdef");
- keyWordList.push_back("sRN SerialNumber");
- answerList.push_back("sRA SerialNumber 8 18020073");
+ keyWordList.push_back("sRN SerialNumber");
+ answerList.push_back("sRA SerialNumber 8 18020073");
- keyWordList.push_back("sMN Run");
- answerList.push_back("sAN Run 1s");
+ keyWordList.push_back("sMN Run");
+ answerList.push_back("sAN Run 1s");
- keyWordList.push_back("sRN ODpwrc");
- answerList.push_back("sRA ODpwrc 20");
+ keyWordList.push_back("sRN ODpwrc");
+ answerList.push_back("sRA ODpwrc 20");
- keyWordList.push_back("sRN LocationName");
- answerList.push_back("sRA LocationName B not defined");
+ keyWordList.push_back("sRN LocationName");
+ answerList.push_back("sRA LocationName B not defined");
- keyWordList.push_back("sEN LMDradardata 1");
- answerList.push_back("sEA LMDradardata 1");
+ keyWordList.push_back("sEN LMDradardata 1");
+ answerList.push_back("sEA LMDradardata 1");
- for (int i = 0; i < requestLen; i++)
- {
- request += (char) requestData[i];
- }
- for (size_t i = 0; i < keyWordList.size(); i++)
- {
- if (request.find(keyWordList[i]) != std::string::npos)
- {
- reply = (char) 0x02;
- reply += answerList[i];
- reply += (char) 0x03;
- }
- }
+ for (int i = 0; i < requestLen; i++)
+ {
+ request += (char) requestData[i];
+ }
+ for (size_t i = 0; i < keyWordList.size(); i++)
+ {
+ if (request.find(keyWordList[i]) != std::string::npos)
+ {
+ reply = (char) 0x02;
+ reply += answerList[i];
+ reply += (char) 0x03;
+ }
+ }
- replyVector->clear();
- for (size_t i = 0; i < reply.length(); i++)
- {
- replyVector->push_back((unsigned char) reply[i]);
- }
+ replyVector->clear();
+ for (size_t i = 0; i < reply.length(); i++)
+ {
+ replyVector->push_back((unsigned char) reply[i]);
+ }
- return (true);
+ return (true);
}
SickScanAdapter::SickScanAdapter(const std::string& hostname, const std::string& port, int& timelimit,
- sick_scan::SickGenericParser* parser, char cola_dialect_id)
- :
- sick_scan::SickScanCommon(parser),
- socket_(io_service_),
- deadline_(io_service_),
- parser_ptr(parser),
- hostname_(hostname),
- port_(port),
- timelimit_(timelimit)
+ sick_scan::SickGenericParser* parser, char cola_dialect_id)
+ :
+ sick_scan::SickScanCommon(parser),
+ socket_(io_service_),
+ deadline_(io_service_),
+ parser_ptr(parser),
+ hostname_(hostname),
+ port_(port),
+ timelimit_(timelimit)
{
- setEmulSensor(false);
- if ((cola_dialect_id == 'a') || (cola_dialect_id == 'A'))
- {
- this->setProtocolType(CoLa_A);
- }
+ setEmulSensor(false);
+ if ((cola_dialect_id == 'a') || (cola_dialect_id == 'A'))
+ {
+ this->setProtocolType(CoLa_A);
+ }
- if ((cola_dialect_id == 'b') || (cola_dialect_id == 'B'))
- {
- this->setProtocolType(CoLa_B);
- }
+ if ((cola_dialect_id == 'b') || (cola_dialect_id == 'B'))
+ {
+ this->setProtocolType(CoLa_B);
+ }
- assert(this->getProtocolType() != CoLa_Unknown);
+ assert(this->getProtocolType() != CoLa_Unknown);
- m_numberOfBytesInReceiveBuffer = 0;
- m_alreadyReceivedBytes = 0;
- this->setReplyMode(0);
- // io_service_.setReadCallbackFunction(boost::bind(&SopasDevice::readCallbackFunction, this, _1, _2));
+ m_numberOfBytesInReceiveBuffer = 0;
+ m_alreadyReceivedBytes = 0;
+ this->setReplyMode(0);
+ // io_service_.setReadCallbackFunction(boost::bind(&SopasDevice::readCallbackFunction, this, _1, _2));
- // Set up the deadline actor to implement timeouts.
- // Based on blocking TCP example on:
- // http://www.boost.org/doc/libs/1_46_0/doc/html/boost_asio/example/timeouts/blocking_tcp_client.cpp
- deadline_.expires_at(boost::posix_time::pos_infin);
- checkDeadline();
+ // Set up the deadline actor to implement timeouts.
+ // Based on blocking TCP example on:
+ // http://www.boost.org/doc/libs/1_46_0/doc/html/boost_asio/example/timeouts/blocking_tcp_client.cpp
+ deadline_.expires_at(boost::posix_time::pos_infin);
+ checkDeadline();
}
SickScanAdapter::~SickScanAdapter()
{
- // stop_scanner();
- close_device();
+ // stop_scanner();
+ close_device();
}
using boost::asio::ip::tcp;
@@ -241,1045 +241,403 @@ namespace armarx
\brief parsing datagram and publishing ros messages
\return error code
*/
- int SickScanAdapter::loopOnce()
+ int SickScanAdapter::loopOnce(LaserScan& scanData)
{
- ARMARX_INFO_S << "myLoop.";
- static int cnt = 0;
- diagnostics_.update();
-
- unsigned char receiveBuffer[65536];
- int actual_length = 0;
- static unsigned int iteration_count = 0;
- bool useBinaryProtocol = this->parser_ptr->getCurrentParamPtr()->getUseBinaryProtocol();
-
- ros::Time recvTimeStamp = ros::Time::now(); // timestamp incoming package, will be overwritten by get_datagram
- // tcp packet
- ros::Time recvTimeStampPush = ros::Time::now(); // timestamp
-
- /*
- * Try to get datagram
- *
- *
- */
-
-
- int packetsInLoop = 0;
-
- const int maxNumAllowedPacketsToProcess = 25; // maximum number of packets, which will be processed in this loop.
-
- int numPacketsProcessed = 0; // count number of processed datagrams
-
- static bool firstTimeCalled = true;
- static bool dumpData = false;
- static int verboseLevel = 0;
- static bool slamBundle = false;
- float timeIncrement;
- static std::string echoForSlam = "";
- if (firstTimeCalled == true)
- {
-
- /* Dump Binary Protocol */
- ros::NodeHandle tmpParam("~");
- tmpParam.getParam("slam_echo", echoForSlam);
- tmpParam.getParam("slam_bundle", slamBundle);
- tmpParam.getParam("verboseLevel", verboseLevel);
- firstTimeCalled = false;
- }
- do
- {
-
- int result = get_datagram(recvTimeStamp, receiveBuffer, 65536, &actual_length, useBinaryProtocol, &packetsInLoop);
- numPacketsProcessed++;
-
- ros::Duration dur = recvTimeStampPush - recvTimeStamp;
-
- if (result != 0)
- {
- ROS_ERROR("Read Error when getting datagram: %i.", result);
- diagnostics_.broadcast(getDiagnosticErrorCode(), "Read Error when getting datagram.");
- return sick_scan::ExitError; // return failure to exit node
- }
- if (actual_length <= 0)
- {
- return sick_scan::ExitSuccess;
- } // return success to continue looping
-
- // ----- if requested, skip frames
- if (iteration_count++ % (config_.skip + 1) != 0)
- {
- return sick_scan::ExitSuccess;
- }
-
- // if (publish_datagram_)
- // {
- // std_msgs::String datagram_msg;
- // datagram_msg.data = std::string(reinterpret_cast<char*>(receiveBuffer));
- // datagram_pub_.publish(datagram_msg);
- // }
-
-
- if (verboseLevel > 0)
- {
- dumpDatagramForDebugging(receiveBuffer, actual_length);
- }
-
-
- bool deviceIsRadar = false;
-
- if (this->parser_ptr->getCurrentParamPtr()->getDeviceIsRadar() == true)
- {
- deviceIsRadar = true;
- }
-
- if (true == deviceIsRadar)
- {
- int errorCode = sick_scan::ExitSuccess;
-#ifndef ROSSIMU
- SickScanRadarSingleton* radar = SickScanRadarSingleton::getInstance();
- // parse radar telegram and send pointcloud2-debug messages
- errorCode = radar->parseDatagram(recvTimeStamp, (unsigned char*) receiveBuffer, actual_length,
- useBinaryProtocol);
-#endif
- return errorCode; // return success to continue looping
- }
-
- static sick_scan::SickScanImu scanImu(this); // todo remove static
- if (scanImu.isImuDatagram((char*) receiveBuffer, actual_length))
- {
- ARMARX_INFO_S << "myLoop1 if.";
- int errorCode = sick_scan::ExitSuccess;
- if (scanImu.isImuAckDatagram((char*) receiveBuffer, actual_length))
- {
-
- }
- else
- {
- // parse radar telegram and send pointcloud2-debug messages
- errorCode = scanImu.parseDatagram(recvTimeStamp, (unsigned char*) receiveBuffer, actual_length,
- useBinaryProtocol);
-
- }
- return errorCode; // return success to continue looping
-
-
- }
- else
- {
- ARMARX_INFO_S << "myLoop1 else.";
- sensor_msgs::LaserScan msg;
-#ifndef _MSC_VER
- sick_scan::Encoder EncoderMsg;
- EncoderMsg.header.stamp = recvTimeStamp + ros::Duration(config_.time_offset);
- //TODO remove this hardcoded variable
- bool FireEncoder = false;
- EncoderMsg.header.frame_id = "Encoder";
- EncoderMsg.header.seq = numPacketsProcessed;
-#endif
- msg.header.stamp = recvTimeStamp + ros::Duration(config_.time_offset);
- double elevationAngleInRad = 0.0;
- /*
- * datagrams are enclosed in <STX> (0x02), <ETX> (0x03) pairs
- */
- char* buffer_pos = (char*) receiveBuffer;
- char* dstart, *dend;
- bool dumpDbg = false;
- bool dataToProcess = true;
- std::vector<float> vang_vec;
- vang_vec.clear();
- dstart = NULL;
- dend = NULL;
-
- while (dataToProcess)
- {
- ARMARX_INFO_S << "myLoop2 dataProcess.";
-
- const int maxAllowedEchos = 5;
-
- uint numValidEchos = 0;
- int aiValidEchoIdx[maxAllowedEchos] = {0};
- size_t dlength;
- int success = -1;
- int numEchos = 0;
- int echoMask = 0;
- bool publishPointCloud = true;
-
-
- // Always Parsing Ascii-Encoding of datagram
- ARMARX_INFO_S << "myLoop3 ASCII.";
- dstart = strchr(buffer_pos, 0x02);
- if (dstart != NULL)
- {
- dend = strchr(dstart + 1, 0x03);
- }
- if ((dstart != NULL) && (dend != NULL))
- {
- dataToProcess = true; // continue parasing
- dlength = dend - dstart;
- *dend = '\0';
- dstart++;
- }
- else
- {
- dataToProcess = false;
- break; //
- }
-
- if (dumpDbg)
- {
- {
- static int cnt = 0;
- char szFileName[255];
- sprintf(szFileName, "/tmp/dump%05d.txt", cnt);
- cnt++;
- }
- }
-
- // HEADER of data followed by DIST1 ... DIST2 ... DIST3 .... RSSI1 ... RSSI2.... RSSI3...
-
- // <frameid>_<sign>00500_DIST[1|2|3]
- numEchos = 1;
- // numEchos contains number of echos (1..5)
- // _msg holds ALL data of all echos
-
- ARMARX_INFO_S << "myLoop trying to parse...";
- success = parser_ptr->parse_datagram(dstart, dlength, config_, msg, numEchos, echoMask);
- publishPointCloud = true; // for MRS1000
-
- numValidEchos = 0;
- for (int i = 0; i < maxAllowedEchos; i++)
- {
- aiValidEchoIdx[i] = 0;
- }
-
-
-
- int numOfLayers = parser_ptr->getCurrentParamPtr()->getNumberOfLayers();
-
- ARMARX_INFO_S << "numOfLayers: " << numOfLayers;
-
- if (success == sick_scan::ExitSuccess)
- {
- ARMARX_INFO_S << "myLoop datagram parsed successfully.";
-
- bool elevationPreCalculated = false;
- double elevationAngleDegree = 0.0;
-
- std::vector<float> rangeTmp = msg.ranges; // copy all range value
- std::vector<float> intensityTmp = msg.intensities; // copy all intensity value
-
- int intensityTmpNum = intensityTmp.size();
- float* intensityTmpPtr = NULL;
- if (intensityTmpNum > 0)
- {
- intensityTmpPtr = &intensityTmp[0];
- }
-
- // Helpful: https://answers.ros.org/question/191265/pointcloud2-access-data/
- // https://gist.github.com/yuma-m/b5dcce1b515335c93ce8
- // Copy to pointcloud
- int layer = 0;
- int baseLayer = 0;
- bool useGivenElevationAngle = false;
- switch (numOfLayers)
- {
- case 1: // TIM571 etc.
- baseLayer = 0;
- break;
- case 4:
-
- baseLayer = -1;
- if (msg.header.seq == 250)
- {
- layer = -1;
- }
- else if (msg.header.seq == 0)
- {
- layer = 0;
- }
- else if ((int) msg.header.seq == -250)
- {
- layer = 1;
- }
- else if ((int) msg.header.seq == -500)
- {
- layer = 2;
- }
- elevationAngleDegree = this->parser_ptr->getCurrentParamPtr()->getElevationDegreeResolution();
- elevationAngleDegree = elevationAngleDegree / 180.0 * M_PI;
- // 0.0436332 /*2.5 degrees*/;
- break;
- case 24: // Preparation for MRS6000
- baseLayer = -1;
- layer = (msg.header.seq - (-2638)) / 125;
- layer = (23 - layer) - 1;
-#if 0
- elevationAngleDegree = this->parser_ptr->getCurrentParamPtr()->getElevationDegreeResolution();
- elevationAngleDegree = elevationAngleDegree / 180.0 * M_PI;
-#endif
-
- elevationPreCalculated = true;
- if (vang_vec.size() > 0)
- {
- useGivenElevationAngle = true;
- }
- break;
- default:
- assert(0);
- break; // unsupported
-
- }
-
-
-
-
-
- // XXX - HIER MEHRERE SCANS publish, falls Mehrzielszenario läuft
- if (numEchos > 5)
- {
- ARMARX_WARNING_S << "Too many echos";
- }
- else
- {
-
- size_t startOffset = 0;
- size_t endOffset = 0;
- int echoPartNum = msg.ranges.size() / numEchos;
- for (int i = 0; i < numEchos; i++)
- {
-
- bool sendMsg = false;
- if ((echoMask & (1 << i)) & 1 /*for TIM5xx outputChannelFlagId == 1*/)
- {
- aiValidEchoIdx[numValidEchos] = i; // save index
- numValidEchos++;
- sendMsg = true;
- }
- startOffset = i * echoPartNum;
- endOffset = (i + 1) * echoPartNum;
-
- msg.ranges.clear();
- msg.intensities.clear();
- msg.ranges = std::vector<float>(
- rangeTmp.begin() + startOffset,
- rangeTmp.begin() + endOffset);
- // check also for MRS1104
- if (endOffset <= intensityTmp.size() && (intensityTmp.size() > 0))
- {
- msg.intensities = std::vector<float>(
- intensityTmp.begin() + startOffset,
- intensityTmp.begin() + endOffset);
- }
- else
- {
- msg.intensities.resize(echoPartNum); // fill with zeros
- }
- {
- // numEchos
- char szTmp[255] = {0};
- if (this->parser_ptr->getCurrentParamPtr()->getNumberOfLayers() > 1)
- {
- const char* cpFrameId = config_.frame_id.c_str();
- char szSignName[10] = {0};
- if ((int) msg.header.seq < 0)
- {
- strcpy(szSignName, "NEG");
- }
- else
- {
- strcpy(szSignName, "POS");
-
- }
-
- sprintf(szTmp, "%s_%s_%03d_DIST%d", cpFrameId, szSignName, abs((int) msg.header.seq), i + 1);
- }
- else
- {
- strcpy(szTmp, config_.frame_id.c_str());
- }
-
- msg.header.frame_id = std::string(szTmp);
- // Hector slam can only process ONE valid frame id.
- if (echoForSlam.length() > 0)
- {
- if (slamBundle)
- {
- // try to map first echos to horizontal layers.
- if (i == 0)
- {
- // first echo
- msg.header.frame_id = echoForSlam;
- strcpy(szTmp, echoForSlam.c_str()); //
- if (elevationAngleInRad != 0.0)
- {
- float cosVal = cos(elevationAngleInRad);
- int rangeNum = msg.ranges.size();
- for (int j = 0; j < rangeNum; j++)
- {
- msg.ranges[j] *= cosVal;
- }
- }
- }
- }
-
- if (echoForSlam.compare(szTmp) == 0)
- {
- sendMsg = true;
- }
- else
- {
- sendMsg = false;
- }
- }
-
- }
-#ifndef _MSC_VER
- if (parser_ptr->getCurrentParamPtr()->getEncoderMode() >= 0 && FireEncoder == true)//
- {
- Encoder_pub.publish(EncoderMsg);
- }
- if (numOfLayers > 4)
- {
- sendMsg = false; // too many layers for publish as scan message. Only pointcloud messages will be pub.
- }
-
- //for TIM5xx outputChannelFlagId == 1
- if (sendMsg & 1) // publish only configured channels - workaround for cfg-bug MRS1104
- {
- //TODO: publish in armarx
- //pub_.publish(msg);
- }
-#else
- printf("MSG received...");
-#endif
- }
- }
-
-
- if (publishPointCloud == true)
- {
-
-
- const int numChannels = 4; // x y z i (for intensity)
-
- int numTmpLayer = numOfLayers;
-
-
- cloud_.header.stamp = recvTimeStamp + ros::Duration(config_.time_offset);
-
-
- cloud_.header.frame_id = config_.frame_id;
- cloud_.header.seq = 0;
- cloud_.height = numTmpLayer * numValidEchos; // due to multi echo multiplied by num. of layers
- cloud_.width = msg.ranges.size();
- cloud_.is_bigendian = false;
- cloud_.is_dense = true;
- cloud_.point_step = numChannels * sizeof(float);
- cloud_.row_step = cloud_.point_step * cloud_.width;
- cloud_.fields.resize(numChannels);
- for (int i = 0; i < numChannels; i++)
- {
- std::string channelId[] = {"x", "y", "z", "intensity"};
- cloud_.fields[i].name = channelId[i];
- cloud_.fields[i].offset = i * sizeof(float);
- cloud_.fields[i].count = 1;
- cloud_.fields[i].datatype = sensor_msgs::PointField::FLOAT32;
- }
-
- cloud_.data.resize(cloud_.row_step * cloud_.height);
-
- unsigned char* cloudDataPtr = &(cloud_.data[0]);
-
-
- // prepare lookup for elevation angle table
-
- std::vector<float> cosAlphaTable; // Lookup table for cos
- std::vector<float> sinAlphaTable; // Lookup table for sin
- size_t rangeNum = rangeTmp.size() / numValidEchos;
- cosAlphaTable.resize(rangeNum);
- sinAlphaTable.resize(rangeNum);
- float mirror_factor = 1.0;
- float angleShift = 0;
- if (this->parser_ptr->getCurrentParamPtr()->getScanMirroredAndShifted())
- {
- // mirror_factor = -1.0;
- // angleShift = +M_PI/2.0; // add 90 deg for NAV3xx-series
- }
-
- for (size_t iEcho = 0; iEcho < numValidEchos; iEcho++)
- {
-
- float angle = config_.min_ang;
-
-
- float* cosAlphaTablePtr = &cosAlphaTable[0];
- float* sinAlphaTablePtr = &sinAlphaTable[0];
-
- float* vangPtr = NULL;
- float* rangeTmpPtr = &rangeTmp[0];
- if (vang_vec.size() > 0)
- {
- vangPtr = &vang_vec[0];
- }
- for (size_t i = 0; i < rangeNum; i++)
- {
- enum enum_index_descr
- {
- idx_x,
- idx_y,
- idx_z,
- idx_intensity,
- idx_num
- };
- long adroff = i * (numChannels * (int) sizeof(float));
-
- adroff += (layer - baseLayer) * cloud_.row_step;
-
- adroff += iEcho * cloud_.row_step * numTmpLayer;
-
- unsigned char* ptr = cloudDataPtr + adroff;
- float* fptr = (float*)(cloudDataPtr + adroff);
-
- geometry_msgs::Point32 point;
- float range_meter = rangeTmpPtr[iEcho * rangeNum + i];
- float phi = angle; // azimuth angle
- float alpha = 0.0; // elevation angle
-
- if (useGivenElevationAngle) // FOR MRS6124
- {
- alpha = -vangPtr[i] * deg2rad_const;
- }
- else
- {
- if (elevationPreCalculated) // FOR MRS6124 without VANGL
- {
- alpha = elevationAngleInRad;
- }
- else
- {
- alpha = layer * elevationAngleDegree; // for MRS1104
- }
- }
-
- if (iEcho == 0)
- {
- cosAlphaTablePtr[i] = cos(alpha); // for z-value (elevation)
- sinAlphaTablePtr[i] = sin(alpha);
- }
- else
- {
- // Just for Debugging: printf("%3d %8.3lf %8.3lf\n", (int)i, cosAlphaTablePtr[i], sinAlphaTablePtr[i]);
- }
- // Thanks to Sebastian Pütz <spuetz@uos.de> for his hint
- float rangeCos = range_meter * cosAlphaTablePtr[i];
-
- double phi_used = phi + angleShift;
- //TODO Angle Compensation?
- // if (this->angleCompensator != NULL)
- // {
- // phi_used = angleCompensator->compensateAngleInRadFromRos(phi_used);
- // }
- fptr[idx_x] = rangeCos * cos(phi_used); // copy x value in pointcloud
- fptr[idx_y] = rangeCos * sin(phi_used); // copy y value in pointcloud
- fptr[idx_z] = range_meter * sinAlphaTablePtr[i] * mirror_factor;// copy z value in pointcloud
-
- fptr[idx_intensity] = 0.0;
- if (config_.intensity)
- {
- int intensityIndex = aiValidEchoIdx[iEcho] * rangeNum + i;
- // intensity values available??
- if (intensityIndex < intensityTmpNum)
- {
- fptr[idx_intensity] = intensityTmpPtr[intensityIndex]; // copy intensity value in pointcloud
- }
- }
- angle += msg.angle_increment;
- }
- // Publish
- static int cnt = 0;
- int layerOff = (layer - baseLayer);
-
- }
- // if ( (msg.header.seq == 0) || (layerOff == 0)) // FIXEN!!!!
- bool shallIFire = false;
- if ((msg.header.seq == 0) || (msg.header.seq == 237))
- {
- shallIFire = true;
- }
-
-
- static int layerCnt = 0;
- static int layerSeq[4];
-
- if (config_.cloud_output_mode > 0)
- {
-
- layerSeq[layerCnt % 4] = layer;
- if (layerCnt >= 4) // mind. erst einmal vier Layer zusammensuchen
- {
- shallIFire = true; // here are at least 4 layers available
- }
- else
- {
- shallIFire = false;
- }
-
- layerCnt++;
- }
-
- if (shallIFire) // shall i fire the signal???
- {
-#ifdef ROSSIMU
- static int cnt = 0;
- cnt++;
-
- printf("PUBLISH_DATA:\n");
-
- unsigned char* cloudDataPtr = &(cloud_.data[0]);
- int w = cloud_.width;
- int h = cloud_.height;
-
- int numShots = w * h;
-
- float* ptr = (float*) cloudDataPtr;
-
- if (cnt == 25)
- {
- char jpgFileName_tmp[255] = { 0 };
-#if linux
- strcpy(jpgFileName_tmp, "./demo/scan.jpg_tmp");
-#else
- strcpy(jpgFileName_tmp, "..\\demo\\scan.jpg_tmp");
-#endif
- int xic = 400;
- int yic = 400;
- int w2i = 400;
- int h2i = 400;
- int hi = h2i * 2 + 1;
- int wi = w2i * 2 + 1;
- int pixNum = hi * wi;
- int numColorChannel = 3;
- unsigned char* pixel = (unsigned char*) malloc(numColorChannel * hi * wi);
- memset(pixel, 0, numColorChannel * pixNum);
- double scaleFac = 50.0;
-
- for (int i = 0; i < hi; i++)
- {
- int pixAdr = numColorChannel * (i * wi + xic);
- pixel[pixAdr] = 0x40;
- pixel[pixAdr + 1] = 0x40;
- pixel[pixAdr + 2] = 0x40;
- }
- for (int i = 0; i < wi; i++)
- {
- int pixAdr = numColorChannel * (yic * wi + i);
- pixel[pixAdr] = 0x40;
- pixel[pixAdr + 1] = 0x40;
- pixel[pixAdr + 2] = 0x40;
- }
-
- scaleFac *= -1.0;
- for (int i = 0; i < numShots; i++)
- {
- double x, y, z, intensity;
- x = ptr[0];
- y = ptr[1];
- z = ptr[2];
- intensity = ptr[3];
- ptr += 4;
- int xi = (x * scaleFac) + xic;
- int yi = (y * scaleFac) + yic;
- if ((xi >= 0) && (xi < wi))
- {
- if ((yi >= 0) && (xi < hi))
- {
- // yi shows left (due to neg. scaleFac)
- // xi shows up (due to neg. scaleFac)
- int pixAdr = numColorChannel * (xi * wi + yi);
- int layer = i / w;
- unsigned char color[3] = {0x00};
- switch (layer)
- {
- case 0:
- color[0] = 0xFF;
- break;
- case 1:
- color[1] = 0xFF;
- break;
- case 2:
- color[2] = 0xFF;
- break;
- case 3:
- color[0] = 0xFF;
- color[1] = 0xFF;
- break;
- }
-
- for (int kk = 0; kk < 3; kk++)
- {
- pixel[pixAdr + kk] = color[kk];
-
- }
- }
- }
-
- }
-
- // Write CSV-File
- char csvFileNameTmp[255];
-#if linux
- strcpy(csvFileNameTmp, "./demo/scan.csv_tmp");
-#endif
- FILE* foutCsv = fopen(csvFileNameTmp, "w");
- if (foutCsv)
- {
- // ZIEL: fprintf(foutCsv,"timestamp;range;elevation;azimuth;x;y;z;intensity\n");
- fprintf(foutCsv, "timestamp_sec;timestamp_nanosec;range;azimuth_deg;elevation_deg;x;y;z;intensity\n");
- unsigned char* cloudDataPtr = &(cloud_.data[0]);
-
- int numShots = w * h;
-
- float* ptr = (float*) cloudDataPtr;
-
-
- long timestamp_sec = cloud_.header.stamp.sec;
- long timestamp_nanosec = cloud_.header.stamp.nsec;
- for (int i = 0; i < numShots; i++)
- {
- double x, y, z, intensity;
- x = ptr[0];
- y = ptr[1];
- z = ptr[2];
- float range_xy = sqrt(x * x + y * y);
- float range_xyz = sqrt(x * x + y * y + z * z);
- float elevation = atan2(z, range_xy);
- float azimuth = atan2(y, x);
- float elevationDeg = elevation * 180.0 / M_PI;
- float azimuthDeg = azimuth * 180.0 / M_PI;
-
- intensity = ptr[3];
- ptr += 4;
- fprintf(foutCsv, "%12ld;%12ld;%8.3lf;%8.3lf;%8.3lf;%8.3f;%8.3f;%8.3f;%8.0f\n", timestamp_sec, timestamp_nanosec, range_xyz, azimuthDeg, elevationDeg, x, y, z, intensity);
- }
- fclose(foutCsv);
-#ifdef linux
- rename(csvFileNameTmp, "./demo/scan.csv");
-#else
- _unlink("..\\demo\\scan.csv");
- rename(csvFileNameTmp, "..\\demo\\scan.csv");
-#endif
- }
- else
- {
- ARMARX_INFO_S << "PANIC: Can not open file for writing. Check existience of demo dir. or patch code.";
- }
- cnt = 0;
- }
-
-#else
- if (config_.cloud_output_mode == 0)
- {
- // standard handling of scans
- cloud_pub_.publish(cloud_);
-
- }
-
- // cloud_pub_.publish(cloud_);
-#endif
- }
- }
- }
- // Start Point
- if (dend != NULL)
- {
- buffer_pos = dend + 1;
- }
- } // end of while loop
- }
-
- // shall we process more data? I.e. are there more packets to process in the input queue???
-
- }
- while ((packetsInLoop > 0) && (numPacketsProcessed < maxNumAllowedPacketsToProcess));
- return sick_scan::ExitSuccess; // return success to continue looping
+ static int cnt = 0;
+
+ unsigned char receiveBuffer[65536];
+ int actual_length = 0;
+ static unsigned int iteration_count = 0;
+ //Always ASCII
+ bool useBinaryProtocol = false;
+
+ ros::Time recvTimeStamp = ros::Time::now(); // timestamp incoming package, will be overwritten by get_datagram
+ // tcp packet
+ ros::Time recvTimeStampPush = ros::Time::now(); // timestamp
+
+ int packetsInLoop = 0;
+ const int maxNumAllowedPacketsToProcess = 25; // maximum number of packets, which will be processed in this loop.
+ int numPacketsProcessed = 0; // count number of processed datagrams
+ do
+ {
+
+ int result = get_datagram(recvTimeStamp, receiveBuffer, 65536, &actual_length, useBinaryProtocol, &packetsInLoop);
+ numPacketsProcessed++;
+
+ ros::Duration dur = recvTimeStampPush - recvTimeStamp;
+
+ if (result != 0)
+ {
+ ARMARX_ERROR_S << "Read Error when getting datagram: " << result;
+ return sick_scan::ExitError; // return failure to exit node
+ }
+ if (actual_length <= 0)
+ {
+ return sick_scan::ExitSuccess;
+ } // return success to continue looping
+
+ // ----- if requested, skip frames
+ if (iteration_count++ % (config_.skip + 1) != 0)
+ {
+ return sick_scan::ExitSuccess;
+ }
+ //msg.header.stamp = recvTimeStamp + ros::Duration(config_.time_offset);
+ //datagrams are enclosed in <STX> (0x02), <ETX> (0x03) pairs
+ char* buffer_pos = (char*) receiveBuffer;
+ char* dstart, *dend;
+ bool dataToProcess = true;
+ std::vector<float> vang_vec;
+ vang_vec.clear();
+ dstart = NULL;
+ dend = NULL;
+
+ while (dataToProcess)
+ {
+ size_t dlength;
+ int success = -1;
+ int numEchos = 1;
+ int echoMask = 0;
+ LaserScannerInfo scanInfo;
+
+ // 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 = parser_ptr->parse_datagram(dstart, dlength, config_, msg, numEchos, echoMask);
+ success = parseDatagram(dstart, dlength, config_, scanData, scanInfo, numEchos, echoMask, true);
+
+ if (success == sick_scan::ExitSuccess)
+ {
+ if (numEchos > 5)
+ {
+ ARMARX_ERROR_S << "Too many echos";
+ }
+ // cloud_.header.stamp = recvTimeStamp + ros::Duration(config_.time_offset);
+ // cloud_.header.frame_id = config_.frame_id;
+ // cloud_.header.seq = 0;
+ // cloud_.height = numTmpLayer * numValidEchos; // due to multi echo multiplied by num. of layers
+ // cloud_.width = msg.ranges.size();
+ // cloud_.is_bigendian = false;
+ // cloud_.is_dense = true;
+ // cloud_.point_step = numChannels * sizeof(float);
+ // cloud_.row_step = cloud_.point_step * cloud_.width;
+ // cloud_.fields.resize(numChannels);
+ // for (int i = 0; i < numChannels; i++)
+ // {
+ // std::string channelId[] = {"x", "y", "z", "intensity"};
+ // cloud_.fields[i].name = channelId[i];
+ // cloud_.fields[i].offset = i * sizeof(float);
+ // cloud_.fields[i].count = 1;
+ // cloud_.fields[i].datatype = sensor_msgs::PointField::FLOAT32;
+ // }
+ cnt++;
+ if (cnt == 25)
+ {
+ ARMARX_INFO_S << "10th measurement: (" << scanData[10].angle << ", " << scanData[10].distance << ")";
+ cnt = 0;
+ }
+ }
+ // Start Point
+ if (dend != NULL)
+ {
+ buffer_pos = dend + 1;
+ }
+ } // end of while loop
+ }
+ while ((packetsInLoop > 0) && (numPacketsProcessed < maxNumAllowedPacketsToProcess));
+ return sick_scan::ExitSuccess; // return success to continue looping
}
- /*
//Adapted from sick_generic_parser
- int SickScanAdapter::parse_datagram(char* datagram, size_t datagram_length, sick_scan::SickScanConfig& config,
- sensor_msgs::LaserScan& msg, int& numEchos, int& echoMask)
- {
- // echoMask introduced to get a workaround for cfg bug using MRS1104
- ros::NodeHandle tmpParam("~");
- bool dumpData = false;
- int verboseLevel = 0;
- tmpParam.getParam("verboseLevel", verboseLevel);
-
- int HEADER_FIELDS = 32;
- char* cur_field;
- size_t count;
- int scannerIdx = parser_ptr->lookUpForAllowedScanner(parser_ptr->getScannerType());
-
- // Reserve sufficient space
- std::vector<char*> fields;
- fields.reserve(datagram_length / 2);
-
- // ----- only for debug output
- std::vector<char> datagram_copy_vec;
- datagram_copy_vec.resize(datagram_length + 1); // to avoid using malloc. destructor frees allocated mem.
- char* datagram_copy = &(datagram_copy_vec[0]);
-
- if (verboseLevel > 0)
- {
- ARMARX_WARNING("Verbose LEVEL activated. Only for DEBUG.");
- }
-
- if (verboseLevel > 0)
- {
- static int cnt = 0;
- char szDumpFileName[511] = {0};
- char szDir[255] = {0};
- #ifdef _MSC_VER
- strcpy(szDir, "C:\\temp\\");
- #else
- strcpy(szDir, "/tmp/");
- #endif
- sprintf(szDumpFileName, "%stmp%06d.bin", szDir, cnt);
- bool isBinary = parser_ptr->getCurrentParamPtr()->getUseBinaryProtocol();
- if (isBinary)
- {
- FILE* ftmp;
- ftmp = fopen(szDumpFileName, "wb");
- if (ftmp != NULL)
- {
- fwrite(datagram, datagram_length, 1, ftmp);
- fclose(ftmp);
- }
- }
- cnt++;
- }
- ARMARX_INFO("parse_datagram(): Copying");
- strncpy(datagram_copy, datagram, datagram_length); // datagram will be changed by strtok
- datagram_copy[datagram_length] = 0;
-
- count = 0;
- cur_field = strtok(datagram, " ");
-
- while (cur_field != NULL)
- {
- fields.push_back(cur_field);
- cur_field = strtok(NULL, " ");
- }
- count = fields.size();
-
- if (verboseLevel > 0)
- {
- static int cnt = 0;
- char szDumpFileName[511] = {0};
- char szDir[255] = {0};
-
- strcpy(szDir, "/tmp/");
- sprintf(szDumpFileName, "%stmp%06d.txt", szDir, cnt);
- ARMARX_WARNING("Verbose LEVEL activated. Only for DEBUG.");
- FILE* ftmp;
- ftmp = fopen(szDumpFileName, "w");
- if (ftmp != NULL)
- {
- for (uint i = 0; i < count; i++)
- {
- fprintf(ftmp, "%3d: %s\n", i, fields[i]);
- }
- fclose(ftmp);
- }
- cnt++;
- }
-
-
- ARMARX_INFO("parse_datagram(): Validate header");
- // Validate header. Total number of tokens is highly unreliable as this may
- // change when you change the scanning range or the device name using SOPAS ET
- // tool. The header remains stable, however.
- if (count < HEADER_FIELDS)
- {
- ARMARX_WARNING(
- "received less fields than minimum fields (actual: %d, minimum: %d), ignoring scan", (int) count,
- HEADER_FIELDS);
- ARMARX_WARNING(
- "are you using the correct node? (124 --> sick_tim310_1130000m01, > 33 --> sick_tim551_2050001, 580 --> sick_tim310s01, 592 --> sick_tim310)");
- // ROS_DEBUG("received message was: %s", datagram_copy);
- return sick_scan::ExitError;
- }
-
- if (strcmp(fields[15], "0"))
- {
- ARMARX_WARNING("Field 15 of received data is not equal to 0 (%s). Unexpected data, ignoring scan", fields[15]);
- return sick_scan::ExitError;
- }
- if (strcmp(fields[20], "DIST1"))
- {
- ARMARX_WARNING("Field 20 of received data is not equal to DIST1i (%s). Unexpected data, ignoring scan", fields[20]);
- return sick_scan::ExitError;
- }
-
- // More in depth checks: check data length and RSSI availability
- // 25: Number of data (<= 10F)
- unsigned short int number_of_data = 0;
- sscanf(fields[25], "%hx", &number_of_data);
-
- int numOfExpectedShots = parser_ptr->basicParams[scannerIdx].getNumberOfShots();
- if (number_of_data < 1 || number_of_data > numOfExpectedShots)
- {
- ARMARX_WARNING("Data length is outside acceptable range 1-%d (%d). Ignoring scan", numOfExpectedShots, number_of_data);
- return sick_scan::ExitError;
- }
- if (count < HEADER_FIELDS + number_of_data)
- {
- ARMARX_WARNING("Less fields than expected for %d data points (%zu). Ignoring scan", number_of_data, count);
- return sick_scan::ExitError;
- }
- ROS_DEBUG("Number of data: %d", number_of_data);
-
- // Calculate offset of field that contains indicator of whether or not RSSI data is included
- size_t rssi_idx = 26 + number_of_data;
- bool rssi = false;
- if (strcmp(fields[rssi_idx], "RSSI1") == 0)
- {
- rssi = true;
- }
- unsigned short int number_of_rssi_data = 0;
- if (rssi)
+ int SickScanAdapter::parseDatagram(char* datagram, size_t datagram_length, sick_scan::SickScanConfig& config,
+ LaserScan& scanData, LaserScannerInfo& scanInfo, int& numEchos, int& echoMask, bool updateScannerInfo)
{
- sscanf(fields[rssi_idx + 5], "%hx", &number_of_rssi_data);
-
- // Number of RSSI data should be equal to number of data
- if (number_of_rssi_data != number_of_data)
- {
- ARMARX_WARNING("Number of RSSI data is lower than number of range data (%d vs %d", number_of_data,
- number_of_rssi_data);
- return sick_scan::ExitError;
- }
-
- // Check if the total length is still appropriate.
- // RSSI data size = number of RSSI readings + 6 fields describing the data
- if (count < HEADER_FIELDS + number_of_data + number_of_rssi_data + 6)
- {
- ARMARX_WARNING("Less fields than expected for %d data points (%zu). Ignoring scan", number_of_data, count);
- return sick_scan::ExitError;
+ int HEADER_FIELDS = 32;
+ char* cur_field;
+ size_t count;
+
+ // Reserve sufficient space
+ std::vector<char*> fields;
+ fields.reserve(datagram_length / 2);
+
+ // ----- only for debug output
+ std::vector<char> datagram_copy_vec;
+ datagram_copy_vec.resize(datagram_length + 1); // to avoid using malloc. destructor frees allocated mem.
+ char* datagram_copy = &(datagram_copy_vec[0]);
+
+ ARMARX_INFO_S << "parse_datagram(): Copying";
+ strncpy(datagram_copy, datagram, datagram_length); // datagram will be changed by strtok
+ datagram_copy[datagram_length] = 0;
+
+ count = 0;
+ cur_field = strtok(datagram, " ");
+
+ while (cur_field != NULL)
+ {
+ fields.push_back(cur_field);
+ cur_field = strtok(NULL, " ");
+ }
+ count = fields.size();
+
+ ARMARX_INFO_S << "parse_datagram(): Validate header";
+ // Validate header. Total number of tokens is highly unreliable as this may
+ // change when you change the scanning range or the device name using SOPAS ET
+ // tool. The header remains stable, however.
+ if ((int) count < HEADER_FIELDS)
+ {
+ ARMARX_ERROR_S << "received less fields than minimum fields (actual: " << count
+ << ", minimum: " << HEADER_FIELDS << "), ignoring scan\n"
+ << "are you using the correct node? (124 --> sick_tim310_1130000m01, > 33 --> sick_tim551_2050001, 580 --> sick_tim310s01, 592 --> sick_tim310)";
+ return sick_scan::ExitError;
+ }
+
+ if (strcmp(fields[15], "0"))
+ {
+ ARMARX_ERROR_S << "Field 15 of received data is not equal to 0. Unexpected data, ignoring scan";
+ return sick_scan::ExitError;
+ }
+ if (strcmp(fields[20], "DIST1"))
+ {
+ ARMARX_ERROR_S << "Field 20 of received data is not equal to DIST1i. Unexpected data, ignoring scan";
+ return sick_scan::ExitError;
+ }
+
+ // More in depth checks: check data length and RSSI availability
+ // 25: Number of data (<= 10F)
+ unsigned short int number_of_data = 0;
+ sscanf(fields[25], "%hx", &number_of_data);
+
+ //int numOfExpectedShots = parser_ptr->basicParams[scannerIdx].getNumberOfShots();
+ //if (number_of_data < 1 || number_of_data > numOfExpectedShots)
+ //{
+ // ARMARX_ERROR_S << "Data length is outside acceptable range 1-%d (%d). Ignoring scan";//, numOfExpectedShots, number_of_data);
+ // return sick_scan::ExitError;
+ //}
+ if ((int) count < HEADER_FIELDS + number_of_data)
+ {
+ ARMARX_ERROR_S << "Less fields than expected for %d data points (%zu). Ignoring scan"; //, number_of_data, count);
+ return sick_scan::ExitError;
+ }
+
+ // Calculate offset of field that contains indicator of whether or not RSSI data is included
+ size_t rssi_idx = 26 + number_of_data;
+ bool rssi = false;
+ if (strcmp(fields[rssi_idx], "RSSI1") == 0)
+ {
+ rssi = true;
+ }
+ unsigned short int number_of_rssi_data = 0;
+ if (rssi)
+ {
+ sscanf(fields[rssi_idx + 5], "%hx", &number_of_rssi_data);
+
+ // Number of RSSI data should be equal to number of data
+ if (number_of_rssi_data != number_of_data)
+ {
+ // ARMARX_ERROR("Number of RSSI data is lower than number of range data (%d vs %d", number_of_data,
+ // number_of_rssi_data);
+ return sick_scan::ExitError;
+ }
+
+ // Check if the total length is still appropriate.
+ // RSSI data size = number of RSSI readings + 6 fields describing the data
+ if ((int) count < HEADER_FIELDS + number_of_data + number_of_rssi_data + 6)
+ {
+ // ARMARX_ERROR("Less fields than expected for %d data points (%zu). Ignoring scan", number_of_data, count);
+ return sick_scan::ExitError;
+ }
+
+ if (strcmp(fields[rssi_idx], "RSSI1"))
+ {
+ // ARMARX_ERROR("Field %zu of received data is not equal to RSSI1 (%s). Unexpected data, ignoring scan", rssi_idx + 1,
+ // fields[rssi_idx + 1]);
+ }
+ }
+
+ ARMARX_INFO_S << "parse_datagram(): Checks complete";
+
+ // ----- read fields into msg
+ //msg.header.frame_id = config.frame_id;
+ // evtl. debug stream benutzen
+ // ROS_DEBUG("publishing with frame_id %s", config.frame_id.c_str());
+
+ ros::Time start_time = ros::Time::now(); // will be adjusted in the end
+
+ // <STX> (\x02)
+ // 0: Type of command (SN)
+ // 1: Command (LMDscandata)
+ // 2: Firmware version number (1)
+ // 3: Device number (1)
+ // 4: Serial number (eg. B96518)
+ // 5 + 6: Device Status (0 0 = ok, 0 1 = error)
+ // 7: Telegram counter (eg. 99)
+ // 8: Scan counter (eg. 9A)
+ // 9: Time since startup (eg. 13C8E59)
+ // 10: Time of transmission (eg. 13C9CBE)
+ // 11 + 12: Input status (0 0)
+ // 13 + 14: Output status (8 0)
+ // 15: Reserved Byte A (0)
+ // 16: Scanning Frequency (5DC)
+ unsigned short scanning_freq = -1;
+ sscanf(fields[16], "%hx", &scanning_freq);
+ // msg.scan_time = 1.0 / (scanning_freq / 100.0);
+
+ // 17: Measurement Frequency (36)
+ unsigned short measurement_freq = -1;
+ sscanf(fields[17], "%hx", &measurement_freq);
+ // msg.time_increment = 1.0 / (measurement_freq * 100.0);
+
+ // 18: Number of encoders (0)
+ // 19: Number of 16 bit channels (1)
+ // 20: Measured data contents (DIST1)
+ // 21: Scaling factor (3F800000)
+ // ignored for now (is always 1.0):
+ // 22: Scaling offset (00000000) -- always 0
+ // 23: Starting angle (FFF92230)
+ if (updateScannerInfo)
+ {
+ uint starting_angle = (uint) - 1;
+ sscanf(fields[23], "%x", &starting_angle);
+ scanInfo.minAngle = (starting_angle / 10000.0) / 180.0 * M_PI - M_PI / 2;
+ // ROS_DEBUG("starting_angle: %d, angle_min: %f", starting_angle, msg.angle_min);
+
+ // 24: Angular step width (2710)
+ unsigned short angular_step_width = -1;
+ sscanf(fields[24], "%hx", &angular_step_width);
+ scanInfo.stepSize = (angular_step_width / 10000.0) / 180.0 * M_PI;
+ scanInfo.maxAngle = scanInfo.minAngle + (number_of_data - 1) * scanInfo.stepSize;
+ }
+
+ // 25: Number of data (<= 10F)
+ // This is already determined above in number_of_data
+ //int index_min = 0;
+
+ ARMARX_INFO_S << "parse_datagram(): Parsing";
+ //param_ptr->checkForDistAndRSSI(fields, number_of_data, distNum, rssiNum, msg.ranges, msg.intensities, echoMask);
+
+ int distNum = 0;
+ int rssiNum = 0;
+ int baseOffset = 20;
+
+ echoMask = 0;
+ // More in depth checks: check data length and RSSI availability
+ // 25: Number of data (<= 10F)
+ unsigned short int curr_number_of_data = 0;
+ if (strstr(fields[baseOffset], "DIST") != fields[baseOffset]) // First initial check
+ {
+ // ARMARX_ERROR("Field 20 of received data does not start with DIST (is: %s). Unexpected data, ignoring scan",
+ // fields[20]);
+ return sick_scan::ExitError;
+ }
+
+ //Read either intensity or distance data, regarding what type is given in the datagram
+ std::vector<float> distVal, intensityVal;
+ for (int offset = 20; offset < (int) fields.size();)
+ {
+ bool distFnd = false;
+ bool rssiFnd = false;
+ if (strlen(fields[offset]) == 5)
+ {
+ if (strstr(fields[offset], "DIST") == fields[offset])
+ {
+ distFnd = true;
+ distNum++;
+ int distId = -1;
+ if (1 == sscanf(fields[offset], "DIST%d", &distId))
+ {
+ echoMask |= (1 << (distId - 1)); // set bit regarding to id
+ }
+ }
+ else if (strstr(fields[offset], "RSSI") == fields[offset])
+ {
+ rssiNum++;
+ rssiFnd = true;
+ }
+ }
+ if (rssiFnd || distFnd)
+ {
+ offset += 5;
+ if (offset >= (int) fields.size())
+ {
+ ARMARX_ERROR_S << "Missing RSSI or DIST data";
+ return sick_scan::ExitError;
+ }
+ curr_number_of_data = 0;
+ sscanf(fields[offset], "%hx", &curr_number_of_data);
+ if (curr_number_of_data != number_of_data)
+ {
+ ARMARX_ERROR_S << "number of dist or rssi values mismatching.";
+ return sick_scan::ExitError;
+ }
+ offset++;
+ // Here is the first value
+ for (int i = 0; i < curr_number_of_data; i++)
+ {
+ if (distFnd)
+ {
+ unsigned short iRange;
+ sscanf(fields[offset + i], "%hx", &iRange);
+ float range = iRange / 1000.0;
+ distVal.push_back(range);
+ }
+ else
+ {
+ unsigned short iRSSI;
+ sscanf(fields[offset + i], "%hx", &iRSSI);
+ intensityVal.push_back((float) iRSSI);
+ }
+ }
+ offset += number_of_data;
+ }
+ else
+ {
+ offset++; // necessary????
+ }
+ }
+ numEchos = distNum;
+
+ ARMARX_INFO_S << "numDistanceVals: " << distNum << ", numIntensityVals: " << rssiNum;
+ //TODO: Write ScanSteps with intensity
+ scanData.reserve(distVal.size());
+ for (int i = 0; i < (int) distVal.size(); i++)
+ {
+ LaserScanStep step;
+ step.angle = i * scanInfo.stepSize;
+ step.distance = distVal[i];
+ scanData.push_back(step);
+ }
+
+ // 26 + n: RSSI data included
+ // IF RSSI not included:
+ // 26 + n + 1 .. 26 + n + 3 = unknown (but seems to be [0, 1, B] always)
+ // 26 + n + 4 .. count - 4 = device label
+ // count - 3 .. count - 1 = unknown (but seems to be 0 always)
+ // <ETX> (\x03)
+
+ // msg.range_min = override_range_min_;
+ // msg.range_max = override_range_max_;
+
+ ARMARX_INFO_S << "parse_datagram(): Time check";
+ return sick_scan::ExitSuccess;
}
- if (strcmp(fields[rssi_idx], "RSSI1"))
- {
- ARMARX_WARNING("Field %zu of received data is not equal to RSSI1 (%s). Unexpected data, ignoring scan", rssi_idx + 1,
- fields[rssi_idx + 1]);
- }
- }
-
- ARMARX_INFO("parse_datagram(): Checks complete");
-
- // ----- read fields into msg
- msg.header.frame_id = config.frame_id;
- // evtl. debug stream benutzen
- // ROS_DEBUG("publishing with frame_id %s", config.frame_id.c_str());
-
- ros::Time start_time = ros::Time::now(); // will be adjusted in the end
-
- // <STX> (\x02)
- // 0: Type of command (SN)
- // 1: Command (LMDscandata)
- // 2: Firmware version number (1)
- // 3: Device number (1)
- // 4: Serial number (eg. B96518)
- // 5 + 6: Device Status (0 0 = ok, 0 1 = error)
- // 7: Telegram counter (eg. 99)
- // 8: Scan counter (eg. 9A)
- // 9: Time since startup (eg. 13C8E59)
- // 10: Time of transmission (eg. 13C9CBE)
- // 11 + 12: Input status (0 0)
- // 13 + 14: Output status (8 0)
- // 15: Reserved Byte A (0)
- // 16: Scanning Frequency (5DC)
- unsigned short scanning_freq = -1;
- sscanf(fields[16], "%hx", &scanning_freq);
- msg.scan_time = 1.0 / (scanning_freq / 100.0);
- // ROS_DEBUG("hex: %s, scanning_freq: %d, scan_time: %f", fields[16], scanning_freq, msg.scan_time);
-
- // 17: Measurement Frequency (36)
- unsigned short measurement_freq = -1;
- sscanf(fields[17], "%hx", &measurement_freq);
- msg.time_increment = 1.0 / (measurement_freq * 100.0);
- // ROS_DEBUG("measurement_freq: %d, time_increment: %f", measurement_freq, msg.time_increment);
-
- // 18: Number of encoders (0)
- // 19: Number of 16 bit channels (1)
- // 20: Measured data contents (DIST1)
-
- // 21: Scaling factor (3F800000)
- // ignored for now (is always 1.0):
- // unsigned int scaling_factor_int = -1;
- // sscanf(fields[21], "%x", &scaling_factor_int);
- //
- // float scaling_factor = reinterpret_cast<float&>(scaling_factor_int);
- // // ROS_DEBUG("hex: %s, scaling_factor_int: %d, scaling_factor: %f", fields[21], scaling_factor_int, scaling_factor);
-
- // 22: Scaling offset (00000000) -- always 0
- // 23: Starting angle (FFF92230)
- int starting_angle = -1;
- sscanf(fields[23], "%x", &starting_angle);
- msg.angle_min = (starting_angle / 10000.0) / 180.0 * M_PI - M_PI / 2;
- // ROS_DEBUG("starting_angle: %d, angle_min: %f", starting_angle, msg.angle_min);
-
- // 24: Angular step width (2710)
- unsigned short angular_step_width = -1;
- sscanf(fields[24], "%hx", &angular_step_width);
- msg.angle_increment = (angular_step_width / 10000.0) / 180.0 * M_PI;
- msg.angle_max = msg.angle_min + (number_of_data - 1) * msg.angle_increment;
-
- // 25: Number of data (<= 10F)
- // This is already determined above in number_of_data
- int index_min = 0;
-
- ARMARX_INFO("parse_datagram(): Parsing");
- #if 1 // neuer Ansatz
- int distNum = 0;
- int rssiNum = 0;
-
- param_ptr->checkForDistAndRSSI(fields, number_of_data, distNum, rssiNum, msg.ranges, msg.intensities, echoMask);
- if (config.intensity)
- {
- if (rssiNum > 0)
- {
-
- }
- else
- {
- ARMARX_WARNING("Intensity parameter is enabled, but the scanner is not configured to send RSSI values! "
- "Please read the section 'Enabling intensity (RSSI) output' here: http://wiki.ros.org/sick_tim.");
- }
- }
- numEchos = distNum;
- #endif
- // 26 + n: RSSI data included
- // IF RSSI not included:
- // 26 + n + 1 .. 26 + n + 3 = unknown (but seems to be [0, 1, B] always)
- // 26 + n + 4 .. count - 4 = device label
- // count - 3 .. count - 1 = unknown (but seems to be 0 always)
- // <ETX> (\x03)
-
- msg.range_min = override_range_min_;
- msg.range_max = override_range_max_;
-
- ARMARX_INFO("parse_datagram(): Time check");
- float expected_time_increment =
- fabs(param_ptr->getCurrentParamPtr()->getNumberOfLayers() * msg.scan_time * msg.angle_increment / (2.0 * M_PI));//If the direction of rotation is reversed, i.e. negative angle increment, a negative scan time results. This does not makes sense, therefore the absolute value is calculated.
- if (fabs(expected_time_increment - msg.time_increment) > 0.00001)
- {
- ARMARX_WARNING("The time_increment, scan_time and angle_increment values reported by the scanner are inconsistent! "
- "Expected time_increment: %.9f, reported time_increment: %.9f. "
- "Perhaps you should set the parameter time_increment to the expected value. This message will print every 60 seconds.",
- expected_time_increment, msg.time_increment);
- }
- return sick_scan::ExitSuccess;
- }
- */
void SickScanAdapter::disconnectFunction()
{
@@ -1288,26 +646,26 @@ namespace armarx
void SickScanAdapter::disconnectFunctionS(void* obj)
{
- if (obj != NULL)
- {
- ((SickScanAdapter*)(obj))->disconnectFunction();
- }
+ if (obj != NULL)
+ {
+ ((SickScanAdapter*)(obj))->disconnectFunction();
+ }
}
void SickScanAdapter::readCallbackFunctionS(void* obj, UINT8* buffer, UINT32& numOfBytes)
{
- ((SickScanAdapter*) obj)->readCallbackFunction(buffer, numOfBytes);
+ ((SickScanAdapter*) obj)->readCallbackFunction(buffer, numOfBytes);
}
void SickScanAdapter::setReplyMode(int _mode)
{
- m_replyMode = _mode;
+ m_replyMode = _mode;
}
int SickScanAdapter::getReplyMode()
{
- return (m_replyMode);
+ return (m_replyMode);
}
/*!
@@ -1317,7 +675,7 @@ namespace armarx
*/
void SickScanAdapter::setEmulSensor(bool _emulFlag)
{
- m_emulSensor = _emulFlag;
+ m_emulSensor = _emulFlag;
}
/*!
@@ -1327,7 +685,7 @@ namespace armarx
*/
bool SickScanAdapter::getEmulSensor()
{
- return (m_emulSensor);
+ return (m_emulSensor);
}
//
@@ -1339,173 +697,173 @@ namespace armarx
//
SopasEventMessage SickScanAdapter::findFrameInReceiveBuffer()
{
- UINT32 frameLen = 0;
- UINT32 i;
-
- // Depends on protocol...
- if (getProtocolType() == CoLa_A)
- {
- //
- // COLA-A
- //
- // Must start with STX (0x02)
- if (m_receiveBuffer[0] != 0x02)
- {
- // Look for starting STX (0x02)
- for (i = 1; i < m_numberOfBytesInReceiveBuffer; i++)
- {
- if (m_receiveBuffer[i] == 0x02)
- {
- break;
- }
- }
-
- // Found beginning of frame?
- if (i >= m_numberOfBytesInReceiveBuffer)
- {
- // No start found, everything can be discarded
- m_numberOfBytesInReceiveBuffer = 0; // Invalidate buffer
- return SopasEventMessage(); // No frame found
- }
-
- // Move frame start to index 0
- UINT32 newLen = m_numberOfBytesInReceiveBuffer - i;
- memmove(&(m_receiveBuffer[0]), &(m_receiveBuffer[i]), newLen);
- m_numberOfBytesInReceiveBuffer = newLen;
- }
-
- // Look for ending ETX (0x03)
- for (i = 1; i < m_numberOfBytesInReceiveBuffer; i++)
- {
- if (m_receiveBuffer[i] == 0x03)
- {
- break;
- }
- }
-
- // Found end?
- if (i >= m_numberOfBytesInReceiveBuffer)
- {
- // No end marker found, so it's not a complete frame (yet)
- return SopasEventMessage(); // No frame found
- }
-
- // Calculate frame length in byte
- frameLen = i + 1;
-
- return SopasEventMessage(m_receiveBuffer, CoLa_A, frameLen);
- }
- else if (getProtocolType() == CoLa_B)
- {
- UINT32 magicWord;
- UINT32 payloadlength;
-
- if (m_numberOfBytesInReceiveBuffer < 4)
- {
- return SopasEventMessage();
- }
- UINT16 pos = 0;
- magicWord = colab::getIntegerFromBuffer<UINT32>(m_receiveBuffer, pos);
- if (magicWord != 0x02020202)
- {
- // Look for starting STX (0x02020202)
- for (i = 1; i <= m_numberOfBytesInReceiveBuffer - 4; i++)
- {
- pos = i; // this is needed, as the position value is updated by getIntegerFromBuffer
- magicWord = colab::getIntegerFromBuffer<UINT32>(m_receiveBuffer, pos);
- if (magicWord == 0x02020202)
- {
- // found magic word
- break;
- }
- }
-
- // Found beginning of frame?
- if (i > m_numberOfBytesInReceiveBuffer - 4)
- {
- // No start found, everything can be discarded
- m_numberOfBytesInReceiveBuffer = 0; // Invalidate buffer
- return SopasEventMessage(); // No frame found
- }
- else
- {
- // Move frame start to index
- UINT32 bytesToMove = m_numberOfBytesInReceiveBuffer - i;
- memmove(&(m_receiveBuffer[0]), &(m_receiveBuffer[i]), bytesToMove); // payload+magic+length+s+checksum
- m_numberOfBytesInReceiveBuffer = bytesToMove;
- }
- }
-
- // Pruefe Laenge des Pufferinhalts
- if (m_numberOfBytesInReceiveBuffer < 9)
- {
- // Es sind nicht genug Daten fuer einen Frame
- printInfoMessage("SickScanCommonNw::findFrameInReceiveBuffer: Frame cannot be decoded yet, only " +
- ::toString(m_numberOfBytesInReceiveBuffer) + " bytes in the buffer.", m_beVerbose);
- return SopasEventMessage();
- }
-
- // Read length of payload
- pos = 4;
- payloadlength = colab::getIntegerFromBuffer<UINT32>(m_receiveBuffer, pos);
- printInfoMessage(
- "SickScanCommonNw::findFrameInReceiveBuffer: Decoded payload length is " + ::toString(payloadlength) +
- " bytes.", m_beVerbose);
-
- // Ist die Datenlaenge plausibel und wuede in den Puffer passen?
- if (payloadlength > (sizeof(m_receiveBuffer) - 9))
- {
- // magic word + length + checksum = 9
- printWarning(
- "SickScanCommonNw::findFrameInReceiveBuffer: Frame too big for receive buffer. Frame discarded with length:"
- + ::toString(payloadlength) + ".");
- m_numberOfBytesInReceiveBuffer = 0;
- return SopasEventMessage();
- }
- if ((payloadlength + 9) > m_numberOfBytesInReceiveBuffer)
- {
- // magic word + length + s + checksum = 10
- printInfoMessage(
- "SickScanCommonNw::findFrameInReceiveBuffer: Frame not complete yet. Waiting for the rest of it (" +
- ::toString(payloadlength + 9 - m_numberOfBytesInReceiveBuffer) + " bytes missing).", m_beVerbose);
- return SopasEventMessage(); // frame not complete
- }
-
- // Calculate the total frame length in bytes: Len = Frame (9 bytes) + Payload
- frameLen = payloadlength + 9;
-
- //
- // test checksum of payload
- //
- UINT8 temp = 0;
- UINT8 temp_xor = 0;
- UINT8 checkSum;
-
- // Read original checksum
- pos = frameLen - 1;
- checkSum = colab::getIntegerFromBuffer<UINT8>(m_receiveBuffer, pos);
-
- // Erzeuge die Pruefsumme zum Vergleich
- for (UINT16 i = 8; i < (frameLen - 1); i++)
- {
- pos = i;
- temp = colab::getIntegerFromBuffer<UINT8>(m_receiveBuffer, pos);
- temp_xor = temp_xor ^ temp;
- }
-
- // Vergleiche die Pruefsummen
- if (temp_xor != checkSum)
- {
- printWarning("SickScanCommonNw::findFrameInReceiveBuffer: Wrong checksum, Frame discarded.");
- m_numberOfBytesInReceiveBuffer = 0;
- return SopasEventMessage();
- }
-
- return SopasEventMessage(m_receiveBuffer, CoLa_B, frameLen);
- }
-
- // Return empty frame
- return SopasEventMessage();
+ UINT32 frameLen = 0;
+ UINT32 i;
+
+ // Depends on protocol...
+ if (getProtocolType() == CoLa_A)
+ {
+ //
+ // COLA-A
+ //
+ // Must start with STX (0x02)
+ if (m_receiveBuffer[0] != 0x02)
+ {
+ // Look for starting STX (0x02)
+ for (i = 1; i < m_numberOfBytesInReceiveBuffer; i++)
+ {
+ if (m_receiveBuffer[i] == 0x02)
+ {
+ break;
+ }
+ }
+
+ // Found beginning of frame?
+ if (i >= m_numberOfBytesInReceiveBuffer)
+ {
+ // No start found, everything can be discarded
+ m_numberOfBytesInReceiveBuffer = 0; // Invalidate buffer
+ return SopasEventMessage(); // No frame found
+ }
+
+ // Move frame start to index 0
+ UINT32 newLen = m_numberOfBytesInReceiveBuffer - i;
+ memmove(&(m_receiveBuffer[0]), &(m_receiveBuffer[i]), newLen);
+ m_numberOfBytesInReceiveBuffer = newLen;
+ }
+
+ // Look for ending ETX (0x03)
+ for (i = 1; i < m_numberOfBytesInReceiveBuffer; i++)
+ {
+ if (m_receiveBuffer[i] == 0x03)
+ {
+ break;
+ }
+ }
+
+ // Found end?
+ if (i >= m_numberOfBytesInReceiveBuffer)
+ {
+ // No end marker found, so it's not a complete frame (yet)
+ return SopasEventMessage(); // No frame found
+ }
+
+ // Calculate frame length in byte
+ frameLen = i + 1;
+
+ return SopasEventMessage(m_receiveBuffer, CoLa_A, frameLen);
+ }
+ else if (getProtocolType() == CoLa_B)
+ {
+ UINT32 magicWord;
+ UINT32 payloadlength;
+
+ if (m_numberOfBytesInReceiveBuffer < 4)
+ {
+ return SopasEventMessage();
+ }
+ UINT16 pos = 0;
+ magicWord = colab::getIntegerFromBuffer<UINT32>(m_receiveBuffer, pos);
+ if (magicWord != 0x02020202)
+ {
+ // Look for starting STX (0x02020202)
+ for (i = 1; i <= m_numberOfBytesInReceiveBuffer - 4; i++)
+ {
+ pos = i; // this is needed, as the position value is updated by getIntegerFromBuffer
+ magicWord = colab::getIntegerFromBuffer<UINT32>(m_receiveBuffer, pos);
+ if (magicWord == 0x02020202)
+ {
+ // found magic word
+ break;
+ }
+ }
+
+ // Found beginning of frame?
+ if (i > m_numberOfBytesInReceiveBuffer - 4)
+ {
+ // No start found, everything can be discarded
+ m_numberOfBytesInReceiveBuffer = 0; // Invalidate buffer
+ return SopasEventMessage(); // No frame found
+ }
+ else
+ {
+ // Move frame start to index
+ UINT32 bytesToMove = m_numberOfBytesInReceiveBuffer - i;
+ memmove(&(m_receiveBuffer[0]), &(m_receiveBuffer[i]), bytesToMove); // payload+magic+length+s+checksum
+ m_numberOfBytesInReceiveBuffer = bytesToMove;
+ }
+ }
+
+ // Pruefe Laenge des Pufferinhalts
+ if (m_numberOfBytesInReceiveBuffer < 9)
+ {
+ // Es sind nicht genug Daten fuer einen Frame
+ printInfoMessage("SickScanCommonNw::findFrameInReceiveBuffer: Frame cannot be decoded yet, only " +
+ ::toString(m_numberOfBytesInReceiveBuffer) + " bytes in the buffer.", m_beVerbose);
+ return SopasEventMessage();
+ }
+
+ // Read length of payload
+ pos = 4;
+ payloadlength = colab::getIntegerFromBuffer<UINT32>(m_receiveBuffer, pos);
+ printInfoMessage(
+ "SickScanCommonNw::findFrameInReceiveBuffer: Decoded payload length is " + ::toString(payloadlength) +
+ " bytes.", m_beVerbose);
+
+ // Ist die Datenlaenge plausibel und wuede in den Puffer passen?
+ if (payloadlength > (sizeof(m_receiveBuffer) - 9))
+ {
+ // magic word + length + checksum = 9
+ printWarning(
+ "SickScanCommonNw::findFrameInReceiveBuffer: Frame too big for receive buffer. Frame discarded with length:"
+ + ::toString(payloadlength) + ".");
+ m_numberOfBytesInReceiveBuffer = 0;
+ return SopasEventMessage();
+ }
+ if ((payloadlength + 9) > m_numberOfBytesInReceiveBuffer)
+ {
+ // magic word + length + s + checksum = 10
+ printInfoMessage(
+ "SickScanCommonNw::findFrameInReceiveBuffer: Frame not complete yet. Waiting for the rest of it (" +
+ ::toString(payloadlength + 9 - m_numberOfBytesInReceiveBuffer) + " bytes missing).", m_beVerbose);
+ return SopasEventMessage(); // frame not complete
+ }
+
+ // Calculate the total frame length in bytes: Len = Frame (9 bytes) + Payload
+ frameLen = payloadlength + 9;
+
+ //
+ // test checksum of payload
+ //
+ UINT8 temp = 0;
+ UINT8 temp_xor = 0;
+ UINT8 checkSum;
+
+ // Read original checksum
+ pos = frameLen - 1;
+ checkSum = colab::getIntegerFromBuffer<UINT8>(m_receiveBuffer, pos);
+
+ // Erzeuge die Pruefsumme zum Vergleich
+ for (UINT16 i = 8; i < (frameLen - 1); i++)
+ {
+ pos = i;
+ temp = colab::getIntegerFromBuffer<UINT8>(m_receiveBuffer, pos);
+ temp_xor = temp_xor ^ temp;
+ }
+
+ // Vergleiche die Pruefsummen
+ if (temp_xor != checkSum)
+ {
+ printWarning("SickScanCommonNw::findFrameInReceiveBuffer: Wrong checksum, Frame discarded.");
+ m_numberOfBytesInReceiveBuffer = 0;
+ return SopasEventMessage();
+ }
+
+ return SopasEventMessage(m_receiveBuffer, CoLa_B, frameLen);
+ }
+
+ // Return empty frame
+ return SopasEventMessage();
}
@@ -1517,195 +875,190 @@ namespace armarx
void SickScanAdapter::processFrame(ros::Time timeStamp, SopasEventMessage& frame)
{
- if (getProtocolType() == CoLa_A)
- {
- printInfoMessage(
- "SickScanCommonNw::processFrame: Calling processFrame_CoLa_A() with " + ::toString(frame.size()) + " bytes.",
- m_beVerbose);
- // processFrame_CoLa_A(frame);
- }
- else if (getProtocolType() == CoLa_B)
- {
- printInfoMessage(
- "SickScanCommonNw::processFrame: Calling processFrame_CoLa_B() with " + ::toString(frame.size()) + " bytes.",
- m_beVerbose);
- // processFrame_CoLa_B(frame);
- }
-
- // Push frame to recvQueue
-
- DatagramWithTimeStamp dataGramWidthTimeStamp(timeStamp, std::vector<unsigned char>(frame.getRawData(),
- frame.getRawData() +
- frame.size()));
- // recvQueue.push(std::vector<unsigned char>(frame.getRawData(), frame.getRawData() + frame.size()));
- recvQueue.push(dataGramWidthTimeStamp);
+ if (getProtocolType() == CoLa_A)
+ {
+ printInfoMessage(
+ "SickScanCommonNw::processFrame: Calling processFrame_CoLa_A() with " + ::toString(frame.size()) + " bytes.",
+ m_beVerbose);
+ // processFrame_CoLa_A(frame);
+ }
+ else if (getProtocolType() == CoLa_B)
+ {
+ printInfoMessage(
+ "SickScanCommonNw::processFrame: Calling processFrame_CoLa_B() with " + ::toString(frame.size()) + " bytes.",
+ m_beVerbose);
+ // processFrame_CoLa_B(frame);
+ }
+
+ // Push frame to recvQueue
+
+ DatagramWithTimeStamp dataGramWidthTimeStamp(timeStamp, std::vector<unsigned char>(frame.getRawData(),
+ frame.getRawData() +
+ frame.size()));
+ // recvQueue.push(std::vector<unsigned char>(frame.getRawData(), frame.getRawData() + frame.size()));
+ recvQueue.push(dataGramWidthTimeStamp);
}
void SickScanAdapter::readCallbackFunction(UINT8* buffer, UINT32& numOfBytes)
{
- ros::Time rcvTimeStamp = ros::Time::now(); // stamp received datagram
- bool beVerboseHere = false;
- printInfoMessage(
- "SickScanCommonNw::readCallbackFunction(): Called with " + toString(numOfBytes) + " available bytes.",
- beVerboseHere);
-
- ScopedLock lock(&m_receiveDataMutex); // Mutex for access to the input buffer
- UINT32 remainingSpace = sizeof(m_receiveBuffer) - m_numberOfBytesInReceiveBuffer;
- UINT32 bytesToBeTransferred = numOfBytes;
- if (remainingSpace < numOfBytes)
- {
- bytesToBeTransferred = remainingSpace;
- // printWarning("SickScanCommonNw::readCallbackFunction(): Input buffer space is to small, transferring only " +
- // ::toString(bytesToBeTransferred) + " of " + ::toString(numOfBytes) + " bytes.");
- }
- else
- {
- // printInfoMessage("SickScanCommonNw::readCallbackFunction(): Transferring " + ::toString(bytesToBeTransferred) +
- // " bytes from TCP to input buffer.", beVerboseHere);
- }
-
- if (bytesToBeTransferred > 0)
- {
- // Data can be transferred into our input buffer
- memcpy(&(m_receiveBuffer[m_numberOfBytesInReceiveBuffer]), buffer, bytesToBeTransferred);
- m_numberOfBytesInReceiveBuffer += bytesToBeTransferred;
-
- UINT32 size = 0;
-
- while (1)
- {
- // Now work on the input buffer until all received datasets are processed
- SopasEventMessage frame = findFrameInReceiveBuffer();
-
- size = frame.size();
- if (size == 0)
- {
- // Framesize = 0: There is no valid frame in the buffer. The buffer is either empty or the frame
- // is incomplete, so leave the loop
- printInfoMessage("SickScanCommonNw::readCallbackFunction(): No complete frame in input buffer, we are done.",
- beVerboseHere);
-
- // Leave the loop
- break;
- }
- else
- {
- // A frame was found in the buffer, so process it now.
- printInfoMessage(
- "SickScanCommonNw::readCallbackFunction(): Processing a frame of length " + ::toString(frame.size()) +
- " bytes.", beVerboseHere);
- processFrame(rcvTimeStamp, frame);
- UINT32 bytesToMove = m_numberOfBytesInReceiveBuffer - size;
- memmove(&(m_receiveBuffer[0]), &(m_receiveBuffer[size]), bytesToMove); // payload+magic+length+s+checksum
- m_numberOfBytesInReceiveBuffer = bytesToMove;
-
- }
- }
- }
- else
- {
- // There was input data from the TCP interface, but our input buffer was unable to hold a single byte.
- // Either we have not read data from our buffer for a long time, or something has gone wrong. To re-sync,
- // we clear the input buffer here.
- m_numberOfBytesInReceiveBuffer = 0;
- }
+ ros::Time rcvTimeStamp = ros::Time::now(); // stamp received datagram
+ bool beVerboseHere = false;
+ printInfoMessage(
+ "SickScanCommonNw::readCallbackFunction(): Called with " + toString(numOfBytes) + " available bytes.",
+ beVerboseHere);
+
+ ScopedLock lock(&m_receiveDataMutex); // Mutex for access to the input buffer
+ UINT32 remainingSpace = sizeof(m_receiveBuffer) - m_numberOfBytesInReceiveBuffer;
+ UINT32 bytesToBeTransferred = numOfBytes;
+ if (remainingSpace < numOfBytes)
+ {
+ bytesToBeTransferred = remainingSpace;
+ // printWarning("SickScanCommonNw::readCallbackFunction(): Input buffer space is to small, transferring only " +
+ // ::toString(bytesToBeTransferred) + " of " + ::toString(numOfBytes) + " bytes.");
+ }
+ else
+ {
+ // printInfoMessage("SickScanCommonNw::readCallbackFunction(): Transferring " + ::toString(bytesToBeTransferred) +
+ // " bytes from TCP to input buffer.", beVerboseHere);
+ }
+
+ if (bytesToBeTransferred > 0)
+ {
+ // Data can be transferred into our input buffer
+ memcpy(&(m_receiveBuffer[m_numberOfBytesInReceiveBuffer]), buffer, bytesToBeTransferred);
+ m_numberOfBytesInReceiveBuffer += bytesToBeTransferred;
+
+ UINT32 size = 0;
+
+ while (1)
+ {
+ // Now work on the input buffer until all received datasets are processed
+ SopasEventMessage frame = findFrameInReceiveBuffer();
+
+ size = frame.size();
+ if (size == 0)
+ {
+ // Framesize = 0: There is no valid frame in the buffer. The buffer is either empty or the frame
+ // is incomplete, so leave the loop
+ printInfoMessage("SickScanCommonNw::readCallbackFunction(): No complete frame in input buffer, we are done.",
+ beVerboseHere);
+
+ // Leave the loop
+ break;
+ }
+ else
+ {
+ // A frame was found in the buffer, so process it now.
+ printInfoMessage(
+ "SickScanCommonNw::readCallbackFunction(): Processing a frame of length " + ::toString(frame.size()) +
+ " bytes.", beVerboseHere);
+ processFrame(rcvTimeStamp, frame);
+ UINT32 bytesToMove = m_numberOfBytesInReceiveBuffer - size;
+ memmove(&(m_receiveBuffer[0]), &(m_receiveBuffer[size]), bytesToMove); // payload+magic+length+s+checksum
+ m_numberOfBytesInReceiveBuffer = bytesToMove;
+
+ }
+ }
+ }
+ else
+ {
+ // There was input data from the TCP interface, but our input buffer was unable to hold a single byte.
+ // Either we have not read data from our buffer for a long time, or something has gone wrong. To re-sync,
+ // we clear the input buffer here.
+ m_numberOfBytesInReceiveBuffer = 0;
+ }
}
int SickScanAdapter::init_device()
{
- int portInt;
- sscanf(port_.c_str(), "%d", &portInt);
- m_nw.init(hostname_, portInt, disconnectFunctionS, (void*) this);
- m_nw.setReadCallbackFunction(readCallbackFunctionS, (void*) this);
- if (this->getEmulSensor())
- {
- ARMARX_INFO_S << "Sensor emulation is switched on - network traffic is switched off.";
- }
- else
- {
- m_nw.connect();
- }
- return sick_scan::ExitSuccess;
+ int portInt;
+ sscanf(port_.c_str(), "%d", &portInt);
+ m_nw.init(hostname_, portInt, disconnectFunctionS, (void*) this);
+ m_nw.setReadCallbackFunction(readCallbackFunctionS, (void*) this);
+ if (this->getEmulSensor())
+ {
+ ARMARX_INFO_S << "Sensor emulation is switched on - network traffic is switched off.";
+ }
+ else
+ {
+ m_nw.connect();
+ }
+ return sick_scan::ExitSuccess;
}
int SickScanAdapter::close_device()
{
- ARMARX_WARNING_S << "Disconnecting TCP-Connection.";
- m_nw.disconnect();
- return 0;
+ ARMARX_ERROR_S << "Disconnecting TCP-Connection.";
+ m_nw.disconnect();
+ return 0;
}
bool SickScanAdapter::stopScanData()
{
- stop_scanner();
- return (true);
+ stop_scanner();
+ return (true);
}
void SickScanAdapter::handleRead(boost::system::error_code error, size_t bytes_transfered)
{
- ec_ = error;
- bytes_transfered_ += bytes_transfered;
+ ec_ = error;
+ bytes_transfered_ += bytes_transfered;
}
void SickScanAdapter::checkDeadline()
{
- if (deadline_.expires_at() <= boost::asio::deadline_timer::traits_type::now())
- {
- // The reason the function is called is that the deadline expired. Close
- // the socket to return all IO operations and reset the deadline
- socket_.close();
- deadline_.expires_at(boost::posix_time::pos_infin);
- }
-
- // Nothing bad happened, go back to sleep
- deadline_.async_wait(boost::bind(&SickScanAdapter::checkDeadline, this));
+ if (deadline_.expires_at() <= boost::asio::deadline_timer::traits_type::now())
+ {
+ // The reason the function is called is that the deadline expired. Close
+ // the socket to return all IO operations and reset the deadline
+ socket_.close();
+ deadline_.expires_at(boost::posix_time::pos_infin);
+ }
+
+ // Nothing bad happened, go back to sleep
+ deadline_.async_wait(boost::bind(&SickScanAdapter::checkDeadline, this));
}
int SickScanAdapter::numberOfDatagramInInputFifo()
{
- int numVal = this->recvQueue.getNumberOfEntriesInQueue();
- return (numVal);
+ int numVal = this->recvQueue.getNumberOfEntriesInQueue();
+ return (numVal);
}
int SickScanAdapter::readWithTimeout(size_t timeout_ms, char* buffer, int buffer_size, int* bytes_read,
- bool* exception_occured, bool isBinary)
+ bool* exception_occured, bool isBinary)
{
- // Set up the deadline to the proper timeout, error and delimiters
- deadline_.expires_from_now(boost::posix_time::milliseconds(timeout_ms));
- const char end_delim = static_cast<char>(0x03);
- int dataLen = 0;
- ec_ = boost::asio::error::would_block;
- bytes_transfered_ = 0;
-
- size_t to_read;
-
- int numBytes = 0;
- // Polling - should be changed to condition variable in the future
- int waitingTimeInMs = 1; // try to lookup for new incoming packages
- size_t i;
- for (i = 0; i < timeout_ms; i += waitingTimeInMs)
- {
- if (false == this->recvQueue.isQueueEmpty())
- {
- break;
- }
- boost::this_thread::sleep(boost::posix_time::milliseconds(waitingTimeInMs));
- }
- if (i >= timeout_ms)
- {
- ROS_ERROR("no answer received after %zu ms. Maybe sopas mode is wrong.\n", timeout_ms);
- return (sick_scan::ExitError);
- }
- boost::condition_variable cond_;
- DatagramWithTimeStamp datagramWithTimeStamp = this->recvQueue.pop();
-
- *bytes_read = datagramWithTimeStamp.datagram.size();
- memcpy(buffer, &(datagramWithTimeStamp.datagram[0]), datagramWithTimeStamp.datagram.size());
- return (sick_scan::ExitSuccess);
+ // Set up the deadline to the proper timeout, error and delimiters
+ deadline_.expires_from_now(boost::posix_time::milliseconds(timeout_ms));
+
+ ec_ = boost::asio::error::would_block;
+ bytes_transfered_ = 0;
+
+ // Polling - should be changed to condition variable in the future
+ int waitingTimeInMs = 1; // try to lookup for new incoming packages
+ size_t i;
+ for (i = 0; i < timeout_ms; i += waitingTimeInMs)
+ {
+ if (false == this->recvQueue.isQueueEmpty())
+ {
+ break;
+ }
+ boost::this_thread::sleep(boost::posix_time::milliseconds(waitingTimeInMs));
+ }
+ if (i >= timeout_ms)
+ {
+ ROS_ERROR("no answer received after %zu ms. Maybe sopas mode is wrong.\n", timeout_ms);
+ return (sick_scan::ExitError);
+ }
+ DatagramWithTimeStamp datagramWithTimeStamp = this->recvQueue.pop();
+
+ *bytes_read = datagramWithTimeStamp.datagram.size();
+ memcpy(buffer, &(datagramWithTimeStamp.datagram[0]), datagramWithTimeStamp.datagram.size());
+ return (sick_scan::ExitSuccess);
}
/**
@@ -1714,238 +1067,238 @@ namespace armarx
int SickScanAdapter::sendSOPASCommand(const char* request, std::vector<unsigned char>* reply, int cmdLen)
{
#if 0
- if (!socket_.is_open())
- {
- ROS_ERROR("sendSOPASCommand: socket not open");
- diagnostics_.broadcast(getDiagnosticErrorCode(), "sendSOPASCommand: socket not open.");
- return sick_scan::ExitError;
- }
+ if (!socket_.is_open())
+ {
+ ROS_ERROR("sendSOPASCommand: socket not open");
+ diagnostics_.broadcast(getDiagnosticErrorCode(), "sendSOPASCommand: socket not open.");
+ return sick_scan::ExitError;
+ }
#endif
- int sLen = 0;
- int preambelCnt = 0;
- bool cmdIsBinary = false;
-
- if (request != NULL)
- {
- sLen = cmdLen;
- preambelCnt = 0; // count 0x02 bytes to decide between ascii and binary command
- if (sLen >= 4)
- {
- for (int i = 0; i < 4; i++)
- {
- if (request[i] == 0x02)
- {
- preambelCnt++;
- }
- }
- }
-
- if (preambelCnt < 4)
- {
- cmdIsBinary = false;
- }
- else
- {
- cmdIsBinary = true;
- }
- int msgLen = 0;
- if (cmdIsBinary == false)
- {
- msgLen = strlen(request);
- }
- else
- {
- int dataLen = 0;
- for (int i = 4; i < 8; i++)
- {
- dataLen |= ((unsigned char) request[i] << (7 - i) * 8);
- }
- msgLen = 8 + dataLen + 1; // 8 Msg. Header + Packet +
- }
+ int sLen = 0;
+ int preambelCnt = 0;
+ bool cmdIsBinary = false;
+
+ if (request != NULL)
+ {
+ sLen = cmdLen;
+ preambelCnt = 0; // count 0x02 bytes to decide between ascii and binary command
+ if (sLen >= 4)
+ {
+ for (int i = 0; i < 4; i++)
+ {
+ if (request[i] == 0x02)
+ {
+ preambelCnt++;
+ }
+ }
+ }
+
+ if (preambelCnt < 4)
+ {
+ cmdIsBinary = false;
+ }
+ else
+ {
+ cmdIsBinary = true;
+ }
+ int msgLen = 0;
+ if (cmdIsBinary == false)
+ {
+ msgLen = strlen(request);
+ }
+ else
+ {
+ int dataLen = 0;
+ for (int i = 4; i < 8; i++)
+ {
+ dataLen |= ((unsigned char) request[i] << (7 - i) * 8);
+ }
+ msgLen = 8 + dataLen + 1; // 8 Msg. Header + Packet +
+ }
#if 1
- if (getEmulSensor())
- {
- emulateReply((UINT8*) request, msgLen, reply);
- }
- else
- {
- bool debugBinCmd = false;
- if (debugBinCmd)
- {
- printf("=== START HEX DUMP ===\n");
- for (int i = 0; i < msgLen; i++)
- {
- unsigned char* ptr = (UINT8*) request;
- printf("%02x ", ptr[i]);
- }
- printf("\n=== END HEX DUMP ===\n");
- }
- m_nw.sendCommandBuffer((UINT8*) request, msgLen);
- }
+ if (getEmulSensor())
+ {
+ emulateReply((UINT8*) request, msgLen, reply);
+ }
+ else
+ {
+ bool debugBinCmd = false;
+ if (debugBinCmd)
+ {
+ printf("=== START HEX DUMP ===\n");
+ for (int i = 0; i < msgLen; i++)
+ {
+ unsigned char* ptr = (UINT8*) request;
+ printf("%02x ", ptr[i]);
+ }
+ printf("\n=== END HEX DUMP ===\n");
+ }
+ m_nw.sendCommandBuffer((UINT8*) request, msgLen);
+ }
#else
- /*
- * Write a SOPAS variable read request to the device.
- */
- try
- {
- boost::asio::write(socket_, boost::asio::buffer(request, msgLen));
- }
- catch (boost::system::system_error& e)
- {
- ROS_ERROR("write error for command: %s", request);
- diagnostics_.broadcast(getDiagnosticErrorCode(), "Write error for sendSOPASCommand.");
- return sick_scan::ExitError;
- }
+ /*
+ * Write a SOPAS variable read request to the device.
+ */
+ try
+ {
+ boost::asio::write(socket_, boost::asio::buffer(request, msgLen));
+ }
+ catch (boost::system::system_error& e)
+ {
+ ROS_ERROR("write error for command: %s", request);
+ diagnostics_.broadcast(getDiagnosticErrorCode(), "Write error for sendSOPASCommand.");
+ return sick_scan::ExitError;
+ }
#endif
- }
-
- // Set timeout in 5 seconds
- const int BUF_SIZE = 65536;
- char buffer[BUF_SIZE];
- int bytes_read;
- // !!!
- if (getEmulSensor())
- {
-
- }
- else
- {
- if (readWithTimeout(getReadTimeOutInMs(), buffer, BUF_SIZE, &bytes_read, 0, cmdIsBinary) == sick_scan::ExitError)
- {
- ARMARX_INFO_S << "sendSOPASCommand: no full reply available for read after (ms): " << getReadTimeOutInMs();
- return sick_scan::ExitError;
- }
-
-
- if (reply)
- {
- reply->resize(bytes_read);
-
- std::copy(buffer, buffer + bytes_read, &(*reply)[0]);
- }
- }
- return sick_scan::ExitSuccess;
+ }
+
+ // Set timeout in 5 seconds
+ const int BUF_SIZE = 65536;
+ char buffer[BUF_SIZE];
+ int bytes_read;
+ // !!!
+ if (getEmulSensor())
+ {
+
+ }
+ else
+ {
+ if (readWithTimeout(getReadTimeOutInMs(), buffer, BUF_SIZE, &bytes_read, 0, cmdIsBinary) == sick_scan::ExitError)
+ {
+ ARMARX_INFO_S << "sendSOPASCommand: no full reply available for read after (ms): " << getReadTimeOutInMs();
+ return sick_scan::ExitError;
+ }
+
+
+ if (reply)
+ {
+ reply->resize(bytes_read);
+
+ std::copy(buffer, buffer + bytes_read, &(*reply)[0]);
+ }
+ }
+ return sick_scan::ExitSuccess;
}
int SickScanAdapter::get_datagram(ros::Time& recvTimeStamp, unsigned char* receiveBuffer, int bufferSize,
- int* actual_length,
- bool isBinaryProtocol, int* numberOfRemainingFifoEntries)
+ int* actual_length,
+ bool isBinaryProtocol, int* numberOfRemainingFifoEntries)
{
- if (NULL != numberOfRemainingFifoEntries)
- {
- *numberOfRemainingFifoEntries = 0;
- }
- this->setReplyMode(1);
-
- if (this->getEmulSensor())
- {
+ if (NULL != numberOfRemainingFifoEntries)
+ {
+ *numberOfRemainingFifoEntries = 0;
+ }
+ this->setReplyMode(1);
+
+ if (this->getEmulSensor())
+ {
#ifndef ROSSIMU
- // boost::this_thread::sleep(boost::posix_time::milliseconds(waitingTimeInMs));
- ros::Time timeStamp = ros::Time::now();
- uint32_t nanoSec = timeStamp.nsec;
- double waitTime10Hz = 10.0 * (double) nanoSec / 1E9; // 10th of sec. [0..10[
+ // boost::this_thread::sleep(boost::posix_time::milliseconds(waitingTimeInMs));
+ ros::Time timeStamp = ros::Time::now();
+ uint32_t nanoSec = timeStamp.nsec;
+ double waitTime10Hz = 10.0 * (double) nanoSec / 1E9; // 10th of sec. [0..10[
- uint32_t waitTime = (int) waitTime10Hz; // round down
+ uint32_t waitTime = (int) waitTime10Hz; // round down
- double waitTimeUntilNextTime10Hz = 1 / 10.0 * (1.0 - (waitTime10Hz - waitTime));
+ double waitTimeUntilNextTime10Hz = 1 / 10.0 * (1.0 - (waitTime10Hz - waitTime));
- ros::Duration(waitTimeUntilNextTime10Hz).sleep();
- SickScanRadar radar(this);
- radar.setEmulation(true);
- radar.simulateAsciiDatagram(receiveBuffer, actual_length);
- recvTimeStamp = ros::Time::now();
+ ros::Duration(waitTimeUntilNextTime10Hz).sleep();
+ SickScanRadar radar(this);
+ radar.setEmulation(true);
+ radar.simulateAsciiDatagram(receiveBuffer, actual_length);
+ recvTimeStamp = ros::Time::now();
#endif
- }
- else
- {
- const int maxWaitInMs = getReadTimeOutInMs();
- std::vector<unsigned char> dataBuffer;
+ }
+ else
+ {
+ const int maxWaitInMs = getReadTimeOutInMs();
+ std::vector<unsigned char> dataBuffer;
#if 1 // prepared for reconnect
- bool retVal = this->recvQueue.waitForIncomingObject(maxWaitInMs);
- if (retVal == false)
- {
- ARMARX_WARNING_S << "Timeout during waiting for new datagram";
- return sick_scan::ExitError;
- }
- else
- {
- // Look into receiving queue for new Datagrams
- //
- //
- DatagramWithTimeStamp datagramWithTimeStamp = this->recvQueue.pop();
- if (NULL != numberOfRemainingFifoEntries)
- {
- *numberOfRemainingFifoEntries = this->recvQueue.getNumberOfEntriesInQueue();
- }
- recvTimeStamp = datagramWithTimeStamp.timeStamp;
- dataBuffer = datagramWithTimeStamp.datagram;
-
- }
+ bool retVal = this->recvQueue.waitForIncomingObject(maxWaitInMs);
+ if (retVal == false)
+ {
+ ARMARX_ERROR_S << "Timeout during waiting for new datagram";
+ return sick_scan::ExitError;
+ }
+ else
+ {
+ // Look into receiving queue for new Datagrams
+ //
+ //
+ DatagramWithTimeStamp datagramWithTimeStamp = this->recvQueue.pop();
+ if (NULL != numberOfRemainingFifoEntries)
+ {
+ *numberOfRemainingFifoEntries = this->recvQueue.getNumberOfEntriesInQueue();
+ }
+ recvTimeStamp = datagramWithTimeStamp.timeStamp;
+ dataBuffer = datagramWithTimeStamp.datagram;
+
+ }
#endif
- // dataBuffer = this->recvQueue.pop();
- long size = dataBuffer.size();
- memcpy(receiveBuffer, &(dataBuffer[0]), size);
- *actual_length = size;
- }
+ // dataBuffer = this->recvQueue.pop();
+ long size = dataBuffer.size();
+ memcpy(receiveBuffer, &(dataBuffer[0]), size);
+ *actual_length = size;
+ }
#if 0
- static int cnt = 0;
- char szFileName[255];
- sprintf(szFileName, "/tmp/dg%06d.bin", cnt++);
-
- FILE* fout;
-
- fout = fopen(szFileName, "wb");
- if (fout != NULL)
- {
- fwrite(receiveBuffer, size, 1, fout);
- fclose(fout);
- }
+ static int cnt = 0;
+ char szFileName[255];
+ sprintf(szFileName, "/tmp/dg%06d.bin", cnt++);
+
+ FILE* fout;
+
+ fout = fopen(szFileName, "wb");
+ if (fout != NULL)
+ {
+ fwrite(receiveBuffer, size, 1, fout);
+ fclose(fout);
+ }
#endif
- return sick_scan::ExitSuccess;
-
- if (!socket_.is_open())
- {
- ROS_ERROR("get_datagram: socket not open");
- diagnostics_.broadcast(getDiagnosticErrorCode(), "get_datagram: socket not open.");
- return sick_scan::ExitError;
- }
-
- /*
- * Write a SOPAS variable read request to the device.
- */
- std::vector<unsigned char> reply;
-
- // Wait at most 5000ms for a new scan
- size_t timeout = 30000;
- bool exception_occured = false;
-
- char* buffer = reinterpret_cast<char*>(receiveBuffer);
-
- if (readWithTimeout(timeout, buffer, bufferSize, actual_length, &exception_occured, isBinaryProtocol) !=
- sick_scan::ExitSuccess)
- {
- ROS_ERROR_THROTTLE(1.0, "get_datagram: no data available for read after %zu ms", timeout);
- diagnostics_.broadcast(getDiagnosticErrorCode(), "get_datagram: no data available for read after timeout.");
-
- // Attempt to reconnect when the connection was terminated
- if (!socket_.is_open())
- {
+ return sick_scan::ExitSuccess;
+
+ if (!socket_.is_open())
+ {
+ ROS_ERROR("get_datagram: socket not open");
+ diagnostics_.broadcast(getDiagnosticErrorCode(), "get_datagram: socket not open.");
+ return sick_scan::ExitError;
+ }
+
+ /*
+ * Write a SOPAS variable read request to the device.
+ */
+ std::vector<unsigned char> reply;
+
+ // Wait at most 5000ms for a new scan
+ size_t timeout = 30000;
+ bool exception_occured = false;
+
+ char* buffer = reinterpret_cast<char*>(receiveBuffer);
+
+ if (readWithTimeout(timeout, buffer, bufferSize, actual_length, &exception_occured, isBinaryProtocol) !=
+ sick_scan::ExitSuccess)
+ {
+ ROS_ERROR_THROTTLE(1.0, "get_datagram: no data available for read after %zu ms", timeout);
+ diagnostics_.broadcast(getDiagnosticErrorCode(), "get_datagram: no data available for read after timeout.");
+
+ // Attempt to reconnect when the connection was terminated
+ if (!socket_.is_open())
+ {
#ifdef _MSC_VER
- Sleep(1000);
+ Sleep(1000);
#else
- sleep(1);
+ sleep(1);
#endif
- ARMARX_INFO << "Failure - attempting to reconnect";
- return init();
- }
+ ARMARX_INFO << "Failure - attempting to reconnect";
+ return init();
+ }
- return exception_occured ? sick_scan::ExitError : sick_scan::ExitSuccess; // keep on trying
- }
+ return exception_occured ? sick_scan::ExitError : sick_scan::ExitSuccess; // keep on trying
+ }
- return sick_scan::ExitSuccess;
+ return sick_scan::ExitSuccess;
}
}
diff --git a/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.h b/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.h
index d5ddd955e..76e880aaa 100644
--- a/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.h
+++ b/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.h
@@ -36,6 +36,7 @@
#define SICK_TIM3XX_COMMON_TCP_H
#include <ArmarXCore/core/Component.h>
+#include <RobotAPI/interface/units/LaserScannerUnit.h>
#include <stdio.h>
#include <stdlib.h>
@@ -70,10 +71,10 @@ namespace armarx
class SickScanAdapter : public sick_scan::SickScanCommon
{
public:
- int loopOnce();
+ int loopOnce(LaserScan& scanData);
- int parse_datagram(char* datagram, size_t datagram_length, sick_scan::SickScanConfig& config,
- sensor_msgs::LaserScan& msg, int& numEchos, int& echoMask);
+ int parseDatagram(char* datagram, size_t datagram_length, sick_scan::SickScanConfig& config,
+ LaserScan& scanData, LaserScannerInfo& scanInfo, int& numEchos, int& echoMask, bool updateScannerInfo = false);
static void disconnectFunctionS(void* obj);
--
GitLab