From c15de133622d123025204537b1b2dec691a28468 Mon Sep 17 00:00:00 2001
From: Johann Mantel <j-mantel@gmx.net>
Date: Fri, 2 Jul 2021 12:46:10 +0200
Subject: [PATCH] Add SickScanAdapter which inherits from SickScanCommon in
order to override the data handling and publishing method with an
ArmarX-compatible implementation
---
.../drivers/SickLaserUnit/CMakeLists.txt | 6 +-
.../drivers/SickLaserUnit/SickScanAdapter.cpp | 2211 +++++++++++++++++
.../drivers/SickLaserUnit/SickScanAdapter.h | 172 ++
3 files changed, 2386 insertions(+), 3 deletions(-)
create mode 100644 source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.cpp
create mode 100644 source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.h
diff --git a/source/RobotAPI/drivers/SickLaserUnit/CMakeLists.txt b/source/RobotAPI/drivers/SickLaserUnit/CMakeLists.txt
index 24f8769f4..4f22c48fb 100644
--- a/source/RobotAPI/drivers/SickLaserUnit/CMakeLists.txt
+++ b/source/RobotAPI/drivers/SickLaserUnit/CMakeLists.txt
@@ -41,15 +41,15 @@ armarx_add_component(
## SickLaserUnitInterfaces # If you defined a component ice interface above.
${sick_scan_base_LIBRARIES}
- ${Boost_LIBRARY_DIRS}
+ ${Boost_LIBRARIES}
SOURCES
SickLaserUnit.cpp
- #SickScanAdapter.cpp
+ #SickScanAdapter.cpp
HEADERS
SickLaserUnit.h
- #SickScanAdapter.h
+ #SickScanAdapter.h
)
diff --git a/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.cpp b/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.cpp
new file mode 100644
index 000000000..7bdb37dec
--- /dev/null
+++ b/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.cpp
@@ -0,0 +1,2211 @@
+/**
+* \file
+* \brief Laser Scanner communication (TCP Helper Class)
+* Copyright (C) 2013, Osnabrück University
+* Copyright (C) 2017, Ing.-Buero Dr. Michael Lehning, Hildesheim
+* Copyright (C) 2017, SICK AG, Waldkirch
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above copyright
+* notice, this list of conditions and the following disclaimer in the
+* documentation and/or other materials provided with the distribution.
+* * Neither the name of Osnabrück University nor the names of its
+* contributors may be used to endorse or promote products derived from
+* this software without specific prior written permission.
+* * Neither the name of SICK AG nor the names of its
+* contributors may be used to endorse or promote products derived from
+* this software without specific prior written permission
+* * Neither the name of Ing.-Buero Dr. Michael Lehning nor the names of its
+* contributors may be used to endorse or promote products derived from
+* this software without specific prior written permission
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*
+* Last modified: 12th Dec 2017
+*
+* Authors:
+* Michael Lehning <michael.lehning@lehning.de>
+* Jochen Sprickerhof <jochen@sprickerhof.de>
+* Martin Günther <mguenthe@uos.de>
+*
+* Based on the TiM communication example by SICK AG.
+*
+*/
+
+#ifdef _MSC_VER
+#pragma warning(disable: 4996)
+#pragma warning(disable: 4267)
+#pragma warning(disable: 4101) // C4101: "e" : Unreferenzierte lokale Variable
+#define _WIN32_WINNT 0x0501
+
+#endif
+
+#include "SickScanAdapter.h"
+#include <sick_scan/tcp/colaa.hpp>
+#include <sick_scan/tcp/colab.hpp>
+#include <sick_scan/sick_generic_radar.h>
+
+#include <boost/asio.hpp>
+#include <boost/lambda/lambda.hpp>
+#include <algorithm>
+#include <iterator>
+#include <boost/lexical_cast.hpp>
+#include <vector>
+
+#ifdef ROSSIMU
+
+#include <sick_scan/rosconsole_simu.hpp>
+
+#endif
+
+std::vector<unsigned char> exampleData(65536);
+std::vector<unsigned char> receivedData(65536);
+static long receivedDataLen = 0;
+
+static int getDiagnosticErrorCode()
+{
+#ifdef _MSC_VER
+#undef ERROR
+ return (2);
+#else
+ return (diagnostic_msgs::DiagnosticStatus::ERROR);
+#endif
+}
+
+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 scannerType = "???";
+ ros::NodeHandle nhPriv;
+
+ enum
+ {
+ SIMU_RADAR = 0, SIMU_MRS_1XXX = 1, SIMU_NUM
+ };
+ int enumType = SIMU_RADAR; // Default simulation
+ if (true == nhPriv.getParam("scanner_type", scannerType))
+ {
+ if (scannerType.compare("sick_mrs_1xxx") == 0)
+ {
+ ROS_INFO("Simulate MRS1xxx");
+ enumType = SIMU_MRS_1XXX;
+ }
+ }
+
+ switch (enumType)
+ {
+ case SIMU_RADAR:
+
+ // 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("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("sWN TransmitTargets 1");
+ answerList.push_back("sWA TransmitTargets");
+
+ 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("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 ODoprh");
+ answerList.push_back("sRA ODoprh 451");
+
+ 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("sMN Run");
+ answerList.push_back("sAN Run 1s");
+
+ 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("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;
+ }
+ }
+
+ replyVector->clear();
+ for (size_t i = 0; i < reply.length(); i++)
+ {
+ replyVector->push_back((unsigned char) reply[i]);
+ }
+ break;
+ case SIMU_MRS_1XXX:
+ ROS_INFO("Emulation of MRS_1xxx is not implemented.\n");
+ assert(0);
+ // XXX
+ break;
+ }
+
+
+ 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_),
+ hostname_(hostname),
+ port_(port),
+ timelimit_(timelimit)
+ {
+
+ 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);
+ }
+
+ assert(this->getProtocolType() != CoLa_Unknown);
+
+ 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();
+
+ }
+
+ SickScanAdapter::~SickScanAdapter()
+ {
+ // stop_scanner();
+ close_device();
+ }
+
+ using boost::asio::ip::tcp;
+ using boost::lambda::var;
+ using boost::lambda::_1;
+
+
+ /*!
+ \brief parsing datagram and publishing ros messages
+ \return error code
+ */
+ int SickScanAdapter::loopOnce()
+ {
+ static int cnt = 0;
+ diagnostics_.update();
+
+ unsigned char receiveBuffer[65536];
+ int actual_length = 0;
+ static unsigned int iteration_count = 0;
+ bool useBinaryProtocol = this->parser_->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_->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 SickScanImu scanImu(this); // todo remove static
+ if (scanImu.isImuDatagram((char*) receiveBuffer, actual_length))
+ {
+ 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
+ {
+
+ 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)
+ {
+ const int maxAllowedEchos = 5;
+
+ int numValidEchos = 0;
+ int aiValidEchoIdx[maxAllowedEchos] = {0};
+ size_t dlength;
+ int success = -1;
+ int numEchos = 0;
+ int echoMask = 0;
+ bool publishPointCloud = true;
+
+ if (useBinaryProtocol)
+ {
+ // if binary protocol used then parse binary message
+ std::vector<unsigned char> receiveBufferVec = std::vector<unsigned char>(receiveBuffer,
+ receiveBuffer + actual_length);
+#ifdef DEBUG_DUMP_TO_CONSOLE_ENABLED
+ if (actual_length > 1000)
+ {
+ DataDumper::instance().dumpUcharBufferToConsole(receiveBuffer, actual_length);
+
+ }
+
+ DataDumper::instance().dumpUcharBufferToConsole(receiveBuffer, actual_length);
+#endif
+ if (receiveBufferVec.size() > 8)
+ {
+ long idVal = 0;
+ long lenVal = 0;
+ memcpy(&idVal, receiveBuffer + 0, 4); // read identifier
+ memcpy(&lenVal, receiveBuffer + 4, 4); // read length indicator
+ swap_endian((unsigned char*) &lenVal, 4);
+
+ if (idVal == 0x02020202) // id for binary message
+ {
+#if 0
+ {
+ static int cnt = 0;
+ char szFileName[255];
+
+#ifdef _MSC_VER
+ sprintf(szFileName, "c:\\temp\\dump%05d.bin", cnt);
+#else
+ sprintf(szFileName, "/tmp/dump%05d.txt", cnt);
+#endif
+ FILE* fout;
+ fout = fopen(szFileName, "wb");
+ fwrite(receiveBuffer, actual_length, 1, fout);
+ fclose(fout);
+ cnt++;
+ }
+#endif
+ // 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;
+ static uint32_t lastSystemCountScan = 0;// this variable is used to ensure that only the first time stamp of an multi layer scann is used for PLL updating
+ uint32_t SystemCountTransmit = 0;
+
+ memcpy(&elevAngleX200, receiveBuffer + 50, 2);
+ swap_endian((unsigned char*) &elevAngleX200, 2);
+
+ elevationAngleInRad = -elevAngleX200 / 200.0 * deg2rad_const;
+ //TODO check this ??
+ msg.header.seq = elevAngleX200; // should be multiple of 0.625° starting with -2638 (corresponding to 13.19°)
+
+ memcpy(&SystemCountScan, receiveBuffer + 0x26, 4);
+ swap_endian((unsigned char*) &SystemCountScan, 4);
+
+ memcpy(&SystemCountTransmit, receiveBuffer + 0x2A, 4);
+ swap_endian((unsigned char*) &SystemCountTransmit, 4);
+ double timestampfloat = recvTimeStamp.sec + recvTimeStamp.nsec * 1e-9;
+ bool bRet;
+ if (SystemCountScan !=
+ lastSystemCountScan)//MRS 6000 sends 6 packets with same SystemCountScan we should only update the pll once with this time stamp since the SystemCountTransmit are different and this will only increase jitter of the pll
+ {
+ bRet = SoftwarePLL::instance().updatePLL(recvTimeStamp.sec, recvTimeStamp.nsec,
+ SystemCountTransmit);
+ lastSystemCountScan = SystemCountScan;
+ }
+ ros::Time tmp_time = recvTimeStamp;
+ bRet = SoftwarePLL::instance().getCorrectedTimeStamp(recvTimeStamp.sec, recvTimeStamp.nsec,
+ SystemCountScan);
+ double timestampfloat_coor = recvTimeStamp.sec + recvTimeStamp.nsec * 1e-9;
+ double DeltaTime = timestampfloat - timestampfloat_coor;
+ //ROS_INFO("%F,%F,%u,%u,%F",timestampfloat,timestampfloat_coor,SystemCountTransmit,SystemCountScan,DeltaTime);
+ //TODO Handle return values
+ if (config_.sw_pll_only_publish == true && bRet == false)
+ {
+ int packets_expected_to_drop = SoftwarePLL::instance().fifoSize - 1;
+ SoftwarePLL::instance().packeds_droped++;
+ ROS_INFO("%i / %i Packet dropped Software PLL not yet locked.",
+ SoftwarePLL::instance().packeds_droped, packets_expected_to_drop);
+ if (SoftwarePLL::instance().packeds_droped == packets_expected_to_drop)
+ {
+ ROS_INFO("Software PLL is expected to be ready now!");
+ }
+ if (SoftwarePLL::instance().packeds_droped > packets_expected_to_drop)
+ {
+ ROS_WARN("More packages than expected were dropped!!\n"
+ "Check the network connection.\n"
+ "Check if the system time has been changed in a leap.\n"
+ "If the problems can persist, disable the software PLL with the option sw_pll_only_publish=False !");
+ }
+ dataToProcess = false;
+ break;
+ }
+
+#ifdef DEBUG_DUMP_ENABLED
+ double elevationAngleInDeg = elevationAngleInRad = -elevAngleX200 / 200.0;
+ // DataDumper::instance().pushData((double)SystemCountScan, "LAYER", elevationAngleInDeg);
+ //DataDumper::instance().pushData((double)SystemCountScan, "LASESCANTIME", SystemCountScan);
+ //DataDumper::instance().pushData((double)SystemCountTransmit, "LASERTRANSMITTIME", SystemCountTransmit);
+ //DataDumper::instance().pushData((double)SystemCountScan, "LASERTRANSMITDELAY", debug_duration.toSec());
+#endif
+
+ memcpy(&scanFrequencyX100, receiveBuffer + 52, 4);
+ swap_endian((unsigned char*) &scanFrequencyX100, 4);
+
+ memcpy(&measurementFrequencyDiv100, receiveBuffer + 56, 4);
+ swap_endian((unsigned char*) &measurementFrequencyDiv100, 4);
+
+
+ msg.scan_time = 1.0 / (scanFrequencyX100 / 100.0);
+
+ //due firmware inconsistency
+ if (measurementFrequencyDiv100 > 10000)
+ {
+ measurementFrequencyDiv100 /= 100;
+ }
+ msg.time_increment = 1.0 / (measurementFrequencyDiv100 * 100.0);
+ timeIncrement = msg.time_increment;
+ msg.range_min = parser_->get_range_min();
+ msg.range_max = parser_->get_range_max();
+
+ 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};
+
+#ifndef _MSC_VER
+ 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];
+#endif
+ 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;
+ msg.ranges.resize(numberOfItems * numEchos);
+ if (rssiCnt > 0)
+ {
+ msg.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;
+
+
+ }
+ }
+ }
+
+
+ parser_->checkScanTiming(msg.time_increment, msg.scan_time, msg.angle_increment, 0.00001f);
+
+ success = sick_scan::ExitSuccess;
+ // change Parsing Mode
+ dataToProcess = false; // only one package allowed - no chaining
+ }
+ else // Parsing of Ascii-Encoding of datagram, xxx
+ {
+ 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];
+
+#ifdef _MSC_VER
+ sprintf(szFileName, "c:\\temp\\dump%05d.txt", cnt);
+#else
+ sprintf(szFileName, "/tmp/dump%05d.txt", cnt);
+#endif
+#if 0
+ FILE* fout;
+ fout = fopen(szFileName, "wb");
+ fwrite(dstart, dlength, 1, fout);
+ fclose(fout);
+#endif
+ 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
+ success = parser_->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_->getCurrentParamPtr()->getNumberOfLayers();
+
+ if (success == sick_scan::ExitSuccess)
+ {
+ 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 (msg.header.seq == -250)
+ {
+ layer = 1;
+ }
+ else if (msg.header.seq == -500)
+ {
+ layer = 2;
+ }
+ elevationAngleDegree = this->parser_->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_->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)
+ {
+ ROS_WARN("Too much 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)) & outputChannelFlagId)
+ {
+ 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_->getCurrentParamPtr()->getNumberOfLayers() > 1)
+ {
+ const char* cpFrameId = config_.frame_id.c_str();
+#if 0
+ sprintf(szTmp, "%s_%+04d_DIST%d", cpFrameId, msg.header.seq, i + 1);
+#else // experimental
+ 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);
+#endif
+ }
+ 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_->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.
+ }
+ if (sendMsg&
+ outputChannelFlagId) // publish only configured channels - workaround for cfg-bug MRS1104
+ {
+
+ 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_->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;
+ 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 JPEG Scan Top View
+ foutJpg = fopen(jpgFileName_tmp, "wb");
+ if (foutJpg == NULL)
+ {
+ ROS_INFO("PANIC: Can not open %s for writing. Check existience of demo dir. or patch code.\n", jpgFileName_tmp);
+ }
+ else
+ {
+ TooJpeg::writeJpeg(jpegOutputCallback, pixel, wi, hi, true, 99);
+ fclose(foutJpg);
+
+ free(pixel);
+
+#if linux
+ rename(jpgFileName_tmp, "./demo/scan.jpg");
+#else
+ _unlink("..\\demo\\scan.jpg");
+ rename(jpgFileName_tmp, "..\\demo\\scan.jpg");
+#endif
+
+ }
+ // Write CSV-File
+ char csvFileNameTmp[255];
+#if linux
+ strcpy(csvFileNameTmp, "./demo/scan.csv_tmp");
+#else
+ 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
+ {
+ ROS_INFO("PANIC: Can not open %s for writing. Check existience of demo dir. or patch code.\n", csvFileNameTmp);
+ }
+ 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
+ }
+
+ void SickScanAdapter::disconnectFunction()
+ {
+
+ }
+
+ void SickScanAdapter::disconnectFunctionS(void* obj)
+ {
+ if (obj != NULL)
+ {
+ ((SickScanAdapter*)(obj))->disconnectFunction();
+ }
+ }
+
+ void SickScanAdapter::readCallbackFunctionS(void* obj, UINT8* buffer, UINT32& numOfBytes)
+ {
+ ((SickScanAdapter*) obj)->readCallbackFunction(buffer, numOfBytes);
+ }
+
+
+ void SickScanAdapter::setReplyMode(int _mode)
+ {
+ m_replyMode = _mode;
+ }
+
+ int SickScanAdapter::getReplyMode()
+ {
+ return (m_replyMode);
+ }
+
+#if 0
+ void SickScanAdapter::setProtocolType(char cola_dialect_id)
+ {
+ if ((cola_dialect_id == 'a') || (cola_dialect_id == 'A'))
+ {
+ this->m_protocol = CoLa_A;
+ }
+ else
+ {
+ this->m_protocol = CoLa_B;
+ }
+ }
+#endif
+
+ /*!
+ \brief Set emulation flag (using emulation instead of "real" scanner - currently implemented for radar
+ \param _emulFlag: Flag to switch emulation on or off
+ \return
+ */
+ void SickScanAdapter::setEmulSensor(bool _emulFlag)
+ {
+ m_emulSensor = _emulFlag;
+ }
+
+ /*!
+ \brief get emulation flag (using emulation instead of "real" scanner - currently implemented for radar
+ \param
+ \return bool: Flag to switch emulation on or off
+ */
+ bool SickScanAdapter::getEmulSensor()
+ {
+ return (m_emulSensor);
+ }
+
+ //
+ // Look for 23-frame (STX/ETX) in receive buffer.
+ // Move frame to start of buffer
+ //
+ // Return: 0 : No (complete) frame found
+ // >0 : Frame length
+ //
+ 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();
+ }
+
+
+ /**
+ * Read callback. Diese Funktion wird aufgerufen, sobald Daten auf der Schnittstelle
+ * hereingekommen sind.
+ */
+
+ 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);
+ }
+
+ 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;
+ }
+ }
+
+
+ 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())
+ {
+ ROS_INFO("Sensor emulation is switched on - network traffic is switched off.");
+ }
+ else
+ {
+ m_nw.connect();
+ }
+ return sick_scan::ExitSuccess;
+ }
+
+ int SickScanAdapter::close_device()
+ {
+ ROS_WARN("Disconnecting TCP-Connection.");
+ m_nw.disconnect();
+ return 0;
+ }
+
+
+ bool SickScanAdapter::stopScanData()
+ {
+ stop_scanner();
+ return (true);
+ }
+
+ void SickScanAdapter::handleRead(boost::system::error_code error, size_t 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));
+ }
+
+
+ int SickScanAdapter::numberOfDatagramInInputFifo()
+ {
+ 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)
+ {
+ // 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);
+ }
+
+ /**
+ * Send a SOPAS command to the device and print out the response to the console.
+ */
+ 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;
+ }
+#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 +
+ }
+#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);
+ }
+#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;
+ }
+#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)
+ {
+ ROS_INFO_THROTTLE(1.0, "sendSOPASCommand: no full reply available for read after %d ms", getReadTimeOutInMs());
+ diagnostics_.broadcast(getDiagnosticErrorCode(),
+ "sendSOPASCommand: no full reply available for read after timeout.");
+ 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)
+ {
+ 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[
+
+ uint32_t waitTime = (int) waitTime10Hz; // round down
+
+ 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();
+#endif
+ }
+ else
+ {
+ const int maxWaitInMs = getReadTimeOutInMs();
+ std::vector<unsigned char> dataBuffer;
+#if 1 // prepared for reconnect
+ bool retVal = this->recvQueue.waitForIncomingObject(maxWaitInMs);
+ if (retVal == false)
+ {
+ ROS_WARN("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;
+ }
+
+#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);
+ }
+#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())
+ {
+#ifdef _MSC_VER
+ Sleep(1000);
+#else
+ sleep(1);
+#endif
+ ROS_INFO("Failure - attempting to reconnect");
+ return init();
+ }
+
+ return exception_occured ? sick_scan::ExitError : sick_scan::ExitSuccess; // keep on trying
+ }
+
+ return sick_scan::ExitSuccess;
+ }
+
+}
diff --git a/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.h b/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.h
new file mode 100644
index 000000000..08daf8276
--- /dev/null
+++ b/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.h
@@ -0,0 +1,172 @@
+/*
+ * Copyright (C) 2013, Freiburg University
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in the
+ * documentation and/or other materials provided with the distribution.
+ * * Neither the name of Osnabrück University nor the names of its
+ * contributors may be used to endorse or promote products derived from
+ * this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Created on: 15.11.2013
+ *
+ * Authors:
+ * Christian Dornhege <c.dornhege@googlemail.com>
+ */
+
+#ifndef SICK_TIM3XX_COMMON_TCP_H
+#define SICK_TIM3XX_COMMON_TCP_H
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <boost/asio.hpp>
+
+#undef NOMINMAX // to get rid off warning C4005: "NOMINMAX": Makro-Neudefinition
+
+#include <sick_scan/sick_scan_common.h>
+#include <sick_scan/sick_generic_parser.h>
+#include <sick_scan/template_queue.h>
+
+namespace armarx
+{
+ /* class prepared for optimized time stamping */
+
+ class DatagramWithTimeStamp
+ {
+ public:
+ DatagramWithTimeStamp(ros::Time timeStamp_, std::vector<unsigned char> datagram_)
+ {
+ timeStamp = timeStamp_;
+ datagram = datagram_;
+ }
+
+ // private:
+ ros::Time timeStamp;
+ std::vector<unsigned char> datagram;
+ };
+
+
+ class SickScanAdapter : public sick_scan::SickScanCommon
+ {
+ public:
+ int loopOnce();
+
+ static void disconnectFunctionS(void* obj);
+
+ SickScanAdapter(const std::string& hostname, const std::string& port, int& timelimit, sick_scan::SickGenericParser* parser,
+ char cola_dialect_id);
+
+ virtual ~SickScanAdapter();
+
+ static void readCallbackFunctionS(void* obj, UINT8* buffer, UINT32& numOfBytes);
+
+ void readCallbackFunction(UINT8* buffer, UINT32& numOfBytes);
+
+ void setReplyMode(int _mode);
+
+ int getReplyMode();
+
+ void setEmulSensor(bool _emulFlag);
+
+ bool getEmulSensor();
+
+ bool stopScanData();
+
+ int numberOfDatagramInInputFifo();
+
+ SopasEventMessage findFrameInReceiveBuffer();
+
+ void processFrame(ros::Time timeStamp, SopasEventMessage& frame);
+
+ // Queue<std::vector<unsigned char> > recvQueue;
+ Queue<DatagramWithTimeStamp> recvQueue;
+ UINT32 m_alreadyReceivedBytes;
+ UINT32 m_lastPacketSize;
+ UINT8 m_packetBuffer[480000];
+ /**
+ * Read callback. Diese Funktion wird aufgerufen, sobald Daten auf der Schnittstelle
+ * hereingekommen sind.
+ */
+
+ protected:
+ void disconnectFunction();
+
+ void readCallbackFunctionOld(UINT8* buffer, UINT32& numOfBytes);
+
+ virtual int init_device();
+
+ virtual int close_device();
+
+ /// Send a SOPAS command to the device and print out the response to the console.
+ virtual int sendSOPASCommand(const char* request, std::vector<unsigned char>* reply, int cmdLen);
+
+ /// Read a datagram from the device.
+ /**
+ * \param [out] recvTimeStamp timestamp of received datagram
+ * \param [in] receiveBuffer data buffer to fill
+ * \param [in] bufferSize max data size to write to buffer (result should be 0 terminated)
+ * \param [out] actual_length the actual amount of data written
+ * \param [in] isBinaryProtocol true=binary False=ASCII
+ */
+ virtual int
+ get_datagram(ros::Time& recvTimeStamp, unsigned char* receiveBuffer, int bufferSize, int* actual_length,
+ bool isBinaryProtocol, int* numberOfRemainingFifoEntries);
+
+ // Helpers for boost asio
+ int readWithTimeout(size_t timeout_ms, char* buffer, int buffer_size, int* bytes_read = 0,
+ bool* exception_occured = 0, bool isBinary = false);
+
+ void handleRead(boost::system::error_code error, size_t bytes_transfered);
+
+ void checkDeadline();
+
+ private:
+
+
+ // Response buffer
+ UINT32 m_numberOfBytesInResponseBuffer; ///< Number of bytes in buffer
+ UINT8 m_responseBuffer[1024]; ///< Receive buffer for everything except scan data and eval case data.
+ Mutex m_receiveDataMutex; ///< Access mutex for buffer
+
+ // Receive buffer
+ UINT32 m_numberOfBytesInReceiveBuffer; ///< Number of bytes in buffer
+ UINT8 m_receiveBuffer[480000]; ///< Low-Level receive buffer for all data
+
+ bool m_beVerbose;
+ bool m_emulSensor;
+
+ boost::asio::io_service io_service_;
+ boost::asio::ip::tcp::socket socket_;
+ boost::asio::deadline_timer deadline_;
+ boost::asio::streambuf input_buffer_;
+ boost::system::error_code ec_;
+ size_t bytes_transfered_;
+
+ std::string hostname_;
+ std::string port_;
+ int timelimit_;
+ int m_replyMode;
+ };
+
+}
+#endif /* SICK_TIM3XX_COMMON_TCP_H */
+
--
GitLab