From 65aa38789c3b8da18c04b4d15c5a2d0b52bd8b93 Mon Sep 17 00:00:00 2001
From: Johann Mantel <j-mantel@gmx.net>
Date: Thu, 8 Jul 2021 19:33:57 +0200
Subject: [PATCH] Integrate and shorten the inherited loopOnce method of
SickScanAdapter
---
etc/cmake/Findsick_scan_base.cmake | 11 +-
.../config/SickLaserUnit.cfg | 33 +-
.../drivers/SickLaserUnit/CMakeLists.txt | 4 +-
.../drivers/SickLaserUnit/SickLaserUnit.cpp | 96 +-
.../drivers/SickLaserUnit/SickLaserUnit.h | 16 +-
.../drivers/SickLaserUnit/SickScanAdapter.cpp | 3598 ++++++++---------
.../drivers/SickLaserUnit/SickScanAdapter.h | 6 +
7 files changed, 1704 insertions(+), 2060 deletions(-)
diff --git a/etc/cmake/Findsick_scan_base.cmake b/etc/cmake/Findsick_scan_base.cmake
index 36714c613..391b22908 100644
--- a/etc/cmake/Findsick_scan_base.cmake
+++ b/etc/cmake/Findsick_scan_base.cmake
@@ -20,8 +20,16 @@ set(HEADER_SEARCH_PATHS
${sick_scan_base_DIR}/roswrap/src/cfgsimu/
ENV CPATH
/usr/include/
+ #/opt/ros/melodic/include/
+ #/opt/ros/melodic/include/sick_scan/
)
+#find_path(sick_scan_base_INCLUDE_DIR_7 NAMES sick_scan_common.h PATHS ${HEADER_SEARCH_PATHS} NO_DEFAULT_PATH)
+#if(sick_scan_base_INCLUDE_DIR_7)
+# set(sick_scan_base_INCLUDE_DIRS ${sick_scan_base_INCLUDE_DIR_7})
+#endif()
+#find_library(sick_scan_base_LIBRARIES NAMES libsick_scan_lib.so PATHS /opt/ros/melodic/lib/ NO_DEFAULT_PATH)
+
find_library(sick_scan_base_LIBRARIES NAMES libsick_scan_generic.so PATHS ${sick_scan_base_DIR}/build NO_DEFAULT_PATH)
message(STATUS "sick_scan_base_LIBRARIES:${sick_scan_base_LIBRARIES}")
@@ -33,10 +41,11 @@ find_path(sick_scan_base_INCLUDE_DIR_4 NAMES dynamic_reconfigure/config_tools.h
find_path(sick_scan_base_INCLUDE_DIR_5 NAMES sick_scan/SickScanConfig.h PATHS ${HEADER_SEARCH_PATHS} NO_DEFAULT_PATH)
find_path(sick_scan_base_INCLUDE_DIR_6 NAMES sick_scan/rosconsole_simu.hpp PATHS ${HEADER_SEARCH_PATHS} NO_DEFAULT_PATH)
-
if(sick_scan_base_INCLUDE_DIR_0 AND sick_scan_base_INCLUDE_DIR_1 AND sick_scan_base_INCLUDE_DIR_2 AND sick_scan_base_INCLUDE_DIR_3 AND sick_scan_base_INCLUDE_DIR_4 AND sick_scan_base_INCLUDE_DIR_5 AND sick_scan_base_INCLUDE_DIR_6)
set(sick_scan_base_INCLUDE_DIRS ${sick_scan_base_INCLUDE_DIR_0} ${sick_scan_base_INCLUDE_DIR_1} ${sick_scan_base_INCLUDE_DIR_2} ${sick_scan_base_INCLUDE_DIR_3} ${sick_scan_base_INCLUDE_DIR_4} ${sick_scan_base_INCLUDE_DIR_5} ${sick_scan_base_INCLUDE_DIR_6})
endif()
+
+
message(STATUS "sick_scan_base_INCLUDE_DIRS:${sick_scan_base_INCLUDE_DIRS}")
find_package_handle_standard_args(sick_scan_base DEFAULT_MSG sick_scan_base_INCLUDE_DIRS sick_scan_base_LIBRARIES)
diff --git a/scenarios/SickLaserUnitTest/config/SickLaserUnit.cfg b/scenarios/SickLaserUnitTest/config/SickLaserUnit.cfg
index 19f03edb0..5d6d74f97 100644
--- a/scenarios/SickLaserUnitTest/config/SickLaserUnit.cfg
+++ b/scenarios/SickLaserUnitTest/config/SickLaserUnit.cfg
@@ -167,21 +167,12 @@ ArmarX.SickLaserUnit.angleOffset = 0.0
ArmarX.SickLaserUnit.devices = Device1; Scanner2
-# ArmarX.SickLaserUnit.emulateSensor: overwrite the default Settings and don't connect to Scanner
-# Attributes:
-# - Default: false
-# - Case sensitivity: yes
-# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.SickLaserUnit.emulateSensor = false
-
-
# ArmarX.SickLaserUnit.hostname: Hostname of the LaserScanner
# Attributes:
# - Default: 192.168.8.133
# - Case sensitivity: yes
# - Required: no
-# ArmarX.SickLaserUnit.hostname = 192.168.8.133
+ArmarX.SickLaserUnit.hostname = 192.168.8.133
# ArmarX.SickLaserUnit.laserScannerTopicName: No Description
@@ -192,14 +183,6 @@ ArmarX.SickLaserUnit.devices = Device1; Scanner2
ArmarX.SickLaserUnit.laserScannerTopicName = "SickTopic"
-# ArmarX.SickLaserUnit.newIpAddress: New IP address for the LaserScanner
-# Attributes:
-# - Default: ""
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.SickLaserUnit.newIpAddress = ""
-
-
# ArmarX.SickLaserUnit.port: port to use on the LaserScanner
# Attributes:
# - Default: 2112
@@ -208,12 +191,11 @@ ArmarX.SickLaserUnit.laserScannerTopicName = "SickTopic"
ArmarX.SickLaserUnit.port = 2112
-# ArmarX.SickLaserUnit.protocol: Either use ASCII or Binary protocol
+# ArmarX.SickLaserUnit.protocol: No Description
# Attributes:
# - Default: ASCII
-# - Case sensitivity: yes
+# - Case sensitivity: no
# - Required: no
-# - Possible values: {ASCII, Binary}
ArmarX.SickLaserUnit.protocol = ASCII
@@ -240,15 +222,6 @@ ArmarX.SickLaserUnit.rangeMin = 0.01
ArmarX.SickLaserUnit.scannerType = sick_tim_5xx
-# ArmarX.SickLaserUnit.sopasProtocolType: Automatically set to true if the Scanner does not support ASCII communication
-# Attributes:
-# - Default: false
-# - Case sensitivity: yes
-# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.SickLaserUnit.sopasProtocolType = false
-
-
# ArmarX.SickLaserUnit.subscribeDatagram: subscribe to Datagram in communication or not
# Attributes:
# - Default: false
diff --git a/source/RobotAPI/drivers/SickLaserUnit/CMakeLists.txt b/source/RobotAPI/drivers/SickLaserUnit/CMakeLists.txt
index 4f22c48fb..15153b3dc 100644
--- a/source/RobotAPI/drivers/SickLaserUnit/CMakeLists.txt
+++ b/source/RobotAPI/drivers/SickLaserUnit/CMakeLists.txt
@@ -45,11 +45,11 @@ armarx_add_component(
SOURCES
SickLaserUnit.cpp
- #SickScanAdapter.cpp
+ SickScanAdapter.cpp
HEADERS
SickLaserUnit.h
- #SickScanAdapter.h
+ SickScanAdapter.h
)
diff --git a/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.cpp b/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.cpp
index c706c5162..1897cbf13 100644
--- a/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.cpp
+++ b/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.cpp
@@ -32,22 +32,6 @@
namespace armarx
{
- std::string protocolToString(ScanProtocol protocol)
- {
- std::string protocolStr;
- switch (protocol)
- {
- case ScanProtocol::ASCII:
- protocolStr = "ASCII";
- break;
- case ScanProtocol::Binary:
- protocolStr = "Binary";
- break;
- }
- return protocolStr;
- }
-
-
void SickLaserScanDevice::run()
{
while (!task->isStopped())
@@ -87,37 +71,26 @@ namespace armarx
delete this->scanner; // disconnect scanner
if (this->useTcp)
{
- //this->scanner = new SickScanAdapter(this->ip, this->port, this->timelimit, this->parser, this->colaDialectId);
- this->scanner = new sick_scan::SickScanCommonTcp(this->ip, this->port, this->timelimit, this->parser, this->colaDialectId);
+ this->scanner = new SickScanAdapter(this->ip, this->port, this->timelimit, this->parser, 'A');
+ //this->scanner = new sick_scan::SickScanCommonTcp(this->ip, this->port, this->timelimit, this->parser, 'A');
}
else
{
ARMARX_ERROR_S << "TCP is not switched on. Probably hostname or port not set.\n";
return;
}
-
- if (this->emulSensor)
- {
- this->scanner->setEmulSensor(true);
- }
result = this->scanner->init();
- this->isSensorInitialized = true;
- ARMARX_INFO_S << "Scanner initialized.";
+
if (result == sick_scan::ExitSuccess) // OK -> loop again
{
- if (this->changeIP)
- {
- this->runState = RunState::scannerFinalize;
- }
- else
- {
- this->runState = RunState::scannerRun; // after initialising switch to run state
- }
-
+ this->isSensorInitialized = true;
+ ARMARX_INFO_S << "Scanner initialized.";
+ this->runState = RunState::scannerRun; // after initialising switch to run state
}
else
{
+ ARMARX_INFO_S << "Reinitializing scanner.";
this->runState = RunState::scannerInit; // If there was an error, try to restart scanner
}
}
@@ -138,14 +111,9 @@ namespace armarx
//communication parameters
def->optional(properties.hostname, "hostname", "Hostname of the LaserScanner");
- def->optional(properties.newIpAddress, "newIpAddress", "New IP address for the LaserScanner");
def->optional(properties.port, "port", "port to use on the LaserScanner");
def->optional(properties.timelimit, "timelimit", "timelimit for communication");
def->optional(properties.subscribeDatagram, "subscribeDatagram", "subscribe to Datagram in communication or not");
- def->optional(properties.protocol, "protocol", "Either use ASCII or Binary protocol")
- .map(protocolToString(ScanProtocol::ASCII), ScanProtocol::ASCII)
- .map(protocolToString(ScanProtocol::Binary), ScanProtocol::Binary);
- def->optional(properties.sopasProtocolType, "sopasProtocolType", "Automatically set to true if the Scanner does not support ASCII communication");
//Scanner parameters
def->required(properties.scannerType, "scannerType", "Name of the LaserScanner");
def->optional(properties.deviceNumber, "deviceNumber", "number of the LaserScanner Device");
@@ -153,8 +121,6 @@ namespace armarx
def->optional(properties.rangeMin, "rangeMin", "minimum Range of the Scanner");
def->optional(properties.rangeMax, "rangeMax", "maximum Range of the Scanner");
def->optional(properties.timeIncrement, "timeIncrement", "timeIncrement??");
- //Additional configuration
- def->optional(properties.emulSensor, "emulateSensor", "overwrite the default Settings and don't connect to Scanner");
return def;
}
@@ -178,11 +144,6 @@ namespace armarx
scanDevice.angleOffset = properties.angleOffset;
scanDevice.timelimit = properties.timelimit;
scanDevice.subscribeDatagram = properties.subscribeDatagram;
- if (properties.newIpAddress != "")
- {
- scanDevice.changeIP = true;
- scanDevice.newIpAddress = properties.newIpAddress;
- }
scanDevice.scannerType = properties.scannerType;
scanDevice.deviceNumber = properties.deviceNumber;
scanDevice.rangeMin = properties.rangeMin;
@@ -201,47 +162,10 @@ namespace armarx
ARMARX_ERROR_S << "Could not create parser. Wrong Scanner name.";
return;
}
- if (properties.emulSensor)
- {
- ARMARX_INFO_S << "Found paraemter emulSensor overwriting default settings. Emulation: True";
- scanDevice.emulSensor = true;
- }
- switch (properties.protocol)
- {
- case ScanProtocol::ASCII:
- if (scanDevice.parser->getCurrentParamPtr()->getNumberOfLayers() > 4)
- {
- ARMARX_WARNING_S << "This scanner type does not support ASCII communication.\n"
- << "Binary communication has been activated.\n"
- << "The parameter \"sopasProtocolType\" has been set to \"true\".";
- scanDevice.sopasProtocolType = true;
- scanDevice.protocol = ScanProtocol::Binary;
- }
- else
- {
- ARMARX_INFO_S << "ASCII protocol activated.";
- scanDevice.protocol = ScanProtocol::ASCII;
- }
- break;
- case ScanProtocol::Binary:
- ARMARX_INFO_S << "Binary protocol activated.";
- scanDevice.protocol = ScanProtocol::Binary;
- break;
- default:
- ARMARX_WARNING_S << "Unknown protocol type. Defaulting to Binary protocol.";
- scanDevice.protocol = ScanProtocol::Binary;
- }
+ scanDevice.parser->getCurrentParamPtr()->setUseBinaryProtocol(false);
+ scanDevice.colaDialectId = 'A';
+ ARMARX_INFO_S << "ASCII protocol activated.";
- if (scanDevice.protocol == ScanProtocol::ASCII)
- {
- scanDevice.parser->getCurrentParamPtr()->setUseBinaryProtocol(false);
- scanDevice.colaDialectId = 'A';
- }
- else
- {
- scanDevice.parser->getCurrentParamPtr()->setUseBinaryProtocol(true);
- scanDevice.colaDialectId = 'B';
- }
ARMARX_INFO_S << "SickLaserUnit initialisation complete.";
}
diff --git a/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.h b/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.h
index e9a038959..4990eb80c 100644
--- a/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.h
+++ b/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.h
@@ -34,7 +34,7 @@
#include <vector>
-//#include "SickScanAdapter.h"
+#include "SickScanAdapter.h"
#include <sick_scan/sick_scan_common.h>
#include <sick_scan/sick_scan_common_tcp.h>
#include <sick_scan/sick_generic_laser.h>
@@ -66,23 +66,18 @@ namespace armarx
double timeIncrement;
//communication parameters
std::string ip;
- std::string newIpAddress = "";
std::string port;
int timelimit = 5;
bool subscribeDatagram = false;
- ScanProtocol protocol = ScanProtocol::ASCII;
- bool sopasProtocolType = false;
bool useTcp = false;
- bool changeIP = false;
- bool emulSensor = false;
- char colaDialectId = 'B';
+ char colaDialectId = 'A';
//data and task pointers
std::vector<long> scanData;
RunState runState = RunState::scannerFinalize;
RunningTask<SickLaserScanDevice>::pointer_type task;
sick_scan::SickScanConfig cfg;
- //SickScanAdapter* scanner;
- sick_scan::SickScanCommonTcp* scanner;
+ SickScanAdapter* scanner;
+ //sick_scan::SickScanCommonTcp* scanner;
sick_scan::SickGenericParser* parser;
int result = sick_scan::ExitError;
bool isSensorInitialized = false;
@@ -156,12 +151,9 @@ namespace armarx
{
//communication parameters
std::string hostname = "192.168.8.133";
- std::string newIpAddress = "";
std::string port = "2112";
int timelimit = 5;
bool subscribeDatagram = false;
- ScanProtocol protocol = ScanProtocol::ASCII;
- bool sopasProtocolType = false;
//scanner parameters
std::string scannerType = "sick_tim_5xx";
int deviceNumber = 0;
diff --git a/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.cpp b/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.cpp
index 7bdb37dec..446676544 100644
--- a/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.cpp
+++ b/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.cpp
@@ -59,6 +59,17 @@
#include <sick_scan/tcp/colaa.hpp>
#include <sick_scan/tcp/colab.hpp>
#include <sick_scan/sick_generic_radar.h>
+#include <sick_scan/helper/angle_compensator.h>
+#include <sick_scan/sick_scan_config_internal.h>
+#include <sick_scan/sick_generic_imu.h>
+
+#ifdef ROSSIMU
+#include <sick_scan/rosconsole_simu.hpp>
+#endif
+
+#include <sick_scan/binScanf.hpp>
+#include <sick_scan/RadarScan.h>
+#include <sick_scan/Encoder.h>
#include <boost/asio.hpp>
#include <boost/lambda/lambda.hpp>
@@ -67,11 +78,11 @@
#include <boost/lexical_cast.hpp>
#include <vector>
-#ifdef ROSSIMU
-
-#include <sick_scan/rosconsole_simu.hpp>
-
+#ifndef rad2deg
+#define rad2deg(x) ((x) / M_PI * 180.0)
#endif
+#define deg2rad_const (0.017453292519943295769236907684886f)
+
std::vector<unsigned char> exampleData(65536);
std::vector<unsigned char> receivedData(65536);
@@ -91,155 +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 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);
+ 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
+ };
+ // 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]);
+ }
+
+ 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)
+ 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;
@@ -253,1277 +243,1043 @@ namespace armarx
*/
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;
+ 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);
+ 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
- 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);
+ 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++;
- }
- return errorCode; // return success to continue looping
+ printf("PUBLISH_DATA:\n");
+ unsigned char* cloudDataPtr = &(cloud_.data[0]);
+ int w = cloud_.width;
+ int h = cloud_.height;
- }
- else
- {
+ int numShots = w * h;
- 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];
+ float* ptr = (float*) cloudDataPtr;
-#ifdef _MSC_VER
- sprintf(szFileName, "c:\\temp\\dump%05d.bin", cnt);
+ if (cnt == 25)
+ {
+ char jpgFileName_tmp[255] = { 0 };
+#if linux
+ strcpy(jpgFileName_tmp, "./demo/scan.jpg_tmp");
#else
- sprintf(szFileName, "/tmp/dump%05d.txt", cnt);
+ strcpy(jpgFileName_tmp, "..\\demo\\scan.jpg_tmp");
#endif
- FILE* fout;
- fout = fopen(szFileName, "wb");
- fwrite(receiveBuffer, actual_length, 1, fout);
- fclose(fout);
- cnt++;
- }
+ 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
- // 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());
+ 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;
+ }
- memcpy(&scanFrequencyX100, receiveBuffer + 52, 4);
- swap_endian((unsigned char*) &scanFrequencyX100, 4);
-
- memcpy(&measurementFrequencyDiv100, receiveBuffer + 56, 4);
- swap_endian((unsigned char*) &measurementFrequencyDiv100, 4);
+#else
+ if (config_.cloud_output_mode == 0)
+ {
+ // standard handling of scans
+ cloud_pub_.publish(cloud_);
+ }
- msg.scan_time = 1.0 / (scanFrequencyX100 / 100.0);
+ // 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
+ }
- //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();
+ /*
+ //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.");
+ }
- memcpy(&numOfEncoders, receiveBuffer + 60, 2);
- swap_endian((unsigned char*) &numOfEncoders, 2);
- int encoderDataOffset = 6 * numOfEncoders;
- int32_t EncoderPosTicks[4] = {0};
- int16_t EncoderSpeed[4] = {0};
+ if (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;
-#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
+ count = 0;
+ cur_field = strtok(datagram, " ");
- 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];
+ while (cur_field != NULL)
+ {
+ fields.push_back(cur_field);
+ cur_field = strtok(NULL, " ");
+ }
+ count = fields.size();
-#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
+ 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++;
+ }
- 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");
+ 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;
+ }
- unsigned char* cloudDataPtr = &(cloud_.data[0]);
- int w = cloud_.width;
- int h = cloud_.height;
+ 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;
+ }
- int numShots = w * h;
+ // 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);
- float* ptr = (float*) cloudDataPtr;
+ 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);
- 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);
+ // 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);
-#if linux
- rename(jpgFileName_tmp, "./demo/scan.jpg");
-#else
- _unlink("..\\demo\\scan.jpg");
- rename(jpgFileName_tmp, "..\\demo\\scan.jpg");
-#endif
+ // 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;
+ }
- }
- // 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;
- }
+ // 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;
+ }
-#else
- if (config_.cloud_output_mode == 0)
- {
- // standard handling of scans
- cloud_pub_.publish(cloud_);
+ 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)
+ {
- // 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
}
+ 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()
{
@@ -1532,41 +1288,27 @@ 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);
- }
-
-#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;
- }
+ return (m_replyMode);
}
-#endif
/*!
\brief Set emulation flag (using emulation instead of "real" scanner - currently implemented for radar
@@ -1575,7 +1317,7 @@ namespace armarx
*/
void SickScanAdapter::setEmulSensor(bool _emulFlag)
{
- m_emulSensor = _emulFlag;
+ m_emulSensor = _emulFlag;
}
/*!
@@ -1585,7 +1327,7 @@ namespace armarx
*/
bool SickScanAdapter::getEmulSensor()
{
- return (m_emulSensor);
+ return (m_emulSensor);
}
//
@@ -1597,173 +1339,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();
}
@@ -1775,195 +1517,195 @@ 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())
- {
- ROS_INFO("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()
{
- ROS_WARN("Disconnecting TCP-Connection.");
- m_nw.disconnect();
- return 0;
+ ARMARX_WARNING_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));
+ 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);
}
/**
@@ -1972,240 +1714,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)
- {
- 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;
+ }
+
+ // 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)
- {
- 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;
-
- }
+ 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;
+
+ }
#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
- ROS_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 08daf8276..d5ddd955e 100644
--- a/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.h
+++ b/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.h
@@ -35,6 +35,8 @@
#ifndef SICK_TIM3XX_COMMON_TCP_H
#define SICK_TIM3XX_COMMON_TCP_H
+#include <ArmarXCore/core/Component.h>
+
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
@@ -70,6 +72,9 @@ namespace armarx
public:
int loopOnce();
+ int parse_datagram(char* datagram, size_t datagram_length, sick_scan::SickScanConfig& config,
+ sensor_msgs::LaserScan& msg, int& numEchos, int& echoMask);
+
static void disconnectFunctionS(void* obj);
SickScanAdapter(const std::string& hostname, const std::string& port, int& timelimit, sick_scan::SickGenericParser* parser,
@@ -161,6 +166,7 @@ namespace armarx
boost::system::error_code ec_;
size_t bytes_transfered_;
+ sick_scan::SickGenericParser* parser_ptr;
std::string hostname_;
std::string port_;
int timelimit_;
--
GitLab