From 65118eac26903ffdfc0f44ff7924571dca26541f Mon Sep 17 00:00:00 2001
From: Johann Mantel <j-mantel@gmx.net>
Date: Thu, 15 Jul 2021 18:12:30 +0200
Subject: [PATCH] clean up parseDatagram method
---
.../drivers/SickLaserUnit/SickLaserUnit.cpp | 2 -
.../drivers/SickLaserUnit/SickScanAdapter.cpp | 79 ++++---------------
.../drivers/SickLaserUnit/SickScanAdapter.h | 2 +-
3 files changed, 18 insertions(+), 65 deletions(-)
diff --git a/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.cpp b/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.cpp
index 092cecf19..8757c0fef 100644
--- a/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.cpp
+++ b/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.cpp
@@ -44,9 +44,7 @@ namespace armarx
case RunState::scannerRun:
if (result == sick_scan::ExitSuccess) // OK -> loop again
{
- ARMARX_INFO_S << "looping";
result = scanner->loopOnce(scanData);
- ARMARX_INFO_S << "finished looping";
}
else
{
diff --git a/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.cpp b/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.cpp
index ae45d3132..54b608331 100644
--- a/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.cpp
+++ b/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.cpp
@@ -295,8 +295,6 @@ namespace armarx
{
size_t dlength;
int success = -1;
- int numEchos = 1;
- int echoMask = 0;
LaserScannerInfo scanInfo;
// Always Parsing Ascii-Encoding of datagram
@@ -320,15 +318,10 @@ namespace armarx
// HEADER of data followed by DIST1 ... DIST2 ... DIST3 .... RSSI1 ... RSSI2.... RSSI3...
// <frameid>_<sign>00500_DIST[1|2|3]
- //success = parser_ptr->parse_datagram(dstart, dlength, config_, msg, numEchos, echoMask);
- success = parseDatagram(dstart, dlength, config_, scanData, scanInfo, numEchos, echoMask, true);
+ success = parseDatagram(dstart, dlength, config_, scanData, scanInfo, true);
if (success == sick_scan::ExitSuccess)
{
- if (numEchos > 5)
- {
- ARMARX_ERROR_S << "Too many echos";
- }
// cloud_.header.stamp = recvTimeStamp + ros::Duration(config_.time_offset);
// cloud_.header.frame_id = config_.frame_id;
// cloud_.header.seq = 0;
@@ -350,7 +343,7 @@ namespace armarx
cnt++;
if (cnt == 25)
{
- ARMARX_INFO_S << "10th measurement: (" << scanData[10].angle << ", " << scanData[10].distance << ")";
+ ARMARX_INFO_S << "last measurement: (" << scanData.back().angle << ", " << scanData.back().distance << ")";
cnt = 0;
}
}
@@ -367,28 +360,22 @@ namespace armarx
//Adapted from sick_generic_parser
int SickScanAdapter::parseDatagram(char* datagram, size_t datagram_length, sick_scan::SickScanConfig& config,
- LaserScan& scanData, LaserScannerInfo& scanInfo, int& numEchos, int& echoMask, bool updateScannerInfo)
+ LaserScan& scanData, LaserScannerInfo& scanInfo, bool updateScannerInfo)
{
int HEADER_FIELDS = 32;
char* cur_field;
- size_t count;
+ size_t count = 0;
// Reserve sufficient space
std::vector<char*> fields;
fields.reserve(datagram_length / 2);
-
- // ----- only for debug output
std::vector<char> datagram_copy_vec;
datagram_copy_vec.resize(datagram_length + 1); // to avoid using malloc. destructor frees allocated mem.
char* datagram_copy = &(datagram_copy_vec[0]);
-
- ARMARX_INFO_S << "parse_datagram(): Copying";
strncpy(datagram_copy, datagram, datagram_length); // datagram will be changed by strtok
datagram_copy[datagram_length] = 0;
- count = 0;
cur_field = strtok(datagram, " ");
-
while (cur_field != NULL)
{
fields.push_back(cur_field);
@@ -396,7 +383,6 @@ namespace armarx
}
count = fields.size();
- ARMARX_INFO_S << "parse_datagram(): Validate header";
// Validate header. Total number of tokens is highly unreliable as this may
// change when you change the scanning range or the device name using SOPAS ET
// tool. The header remains stable, however.
@@ -424,35 +410,27 @@ namespace armarx
unsigned short int number_of_data = 0;
sscanf(fields[25], "%hx", &number_of_data);
- //int numOfExpectedShots = parser_ptr->basicParams[scannerIdx].getNumberOfShots();
- //if (number_of_data < 1 || number_of_data > numOfExpectedShots)
- //{
- // ARMARX_ERROR_S << "Data length is outside acceptable range 1-%d (%d). Ignoring scan";//, numOfExpectedShots, number_of_data);
- // return sick_scan::ExitError;
- //}
if ((int) count < HEADER_FIELDS + number_of_data)
{
- ARMARX_ERROR_S << "Less fields than expected for %d data points (%zu). Ignoring scan"; //, number_of_data, count);
+ ARMARX_ERROR_S << "Less fields than expected for data points - Ignoring scan";
return sick_scan::ExitError;
}
// Calculate offset of field that contains indicator of whether or not RSSI data is included
size_t rssi_idx = 26 + number_of_data;
bool rssi = false;
+ unsigned short int number_of_rssi_data = 0;
if (strcmp(fields[rssi_idx], "RSSI1") == 0)
{
rssi = true;
}
- unsigned short int number_of_rssi_data = 0;
if (rssi)
{
sscanf(fields[rssi_idx + 5], "%hx", &number_of_rssi_data);
-
// Number of RSSI data should be equal to number of data
if (number_of_rssi_data != number_of_data)
{
- // ARMARX_ERROR("Number of RSSI data is lower than number of range data (%d vs %d", number_of_data,
- // number_of_rssi_data);
+ ARMARX_ERROR_S << "Number of RSSI data is lower than number of range data";
return sick_scan::ExitError;
}
@@ -460,24 +438,16 @@ namespace armarx
// RSSI data size = number of RSSI readings + 6 fields describing the data
if ((int) count < HEADER_FIELDS + number_of_data + number_of_rssi_data + 6)
{
- // ARMARX_ERROR("Less fields than expected for %d data points (%zu). Ignoring scan", number_of_data, count);
+ ARMARX_ERROR_S << "Less fields than expected for %d data points (%zu). Ignoring scan";
return sick_scan::ExitError;
}
if (strcmp(fields[rssi_idx], "RSSI1"))
{
- // ARMARX_ERROR("Field %zu of received data is not equal to RSSI1 (%s). Unexpected data, ignoring scan", rssi_idx + 1,
- // fields[rssi_idx + 1]);
+ ARMARX_ERROR_S << "A Field of received data is not equal to RSSI1 - Unexpected data, ignoring scan";
}
}
- ARMARX_INFO_S << "parse_datagram(): Checks complete";
-
- // ----- read fields into msg
- //msg.header.frame_id = config.frame_id;
- // evtl. debug stream benutzen
- // ROS_DEBUG("publishing with frame_id %s", config.frame_id.c_str());
-
ros::Time start_time = ros::Time::now(); // will be adjusted in the end
// <STX> (\x02)
@@ -516,7 +486,6 @@ namespace armarx
uint starting_angle = (uint) - 1;
sscanf(fields[23], "%x", &starting_angle);
scanInfo.minAngle = (starting_angle / 10000.0) / 180.0 * M_PI - M_PI / 2;
- // ROS_DEBUG("starting_angle: %d, angle_min: %f", starting_angle, msg.angle_min);
// 24: Angular step width (2710)
unsigned short angular_step_width = -1;
@@ -527,23 +496,16 @@ namespace armarx
// 25: Number of data (<= 10F)
// This is already determined above in number_of_data
- //int index_min = 0;
-
- ARMARX_INFO_S << "parse_datagram(): Parsing";
- //param_ptr->checkForDistAndRSSI(fields, number_of_data, distNum, rssiNum, msg.ranges, msg.intensities, echoMask);
int distNum = 0;
int rssiNum = 0;
int baseOffset = 20;
-
- echoMask = 0;
// More in depth checks: check data length and RSSI availability
// 25: Number of data (<= 10F)
unsigned short int curr_number_of_data = 0;
if (strstr(fields[baseOffset], "DIST") != fields[baseOffset]) // First initial check
{
- // ARMARX_ERROR("Field 20 of received data does not start with DIST (is: %s). Unexpected data, ignoring scan",
- // fields[20]);
+ ARMARX_ERROR_S << "Field 20 of received data does not start with DIST - Unexpected data, ignoring scan";
return sick_scan::ExitError;
}
@@ -557,13 +519,8 @@ namespace armarx
{
if (strstr(fields[offset], "DIST") == fields[offset])
{
- distFnd = true;
distNum++;
- int distId = -1;
- if (1 == sscanf(fields[offset], "DIST%d", &distId))
- {
- echoMask |= (1 << (distId - 1)); // set bit regarding to id
- }
+ distFnd = true;
}
else if (strstr(fields[offset], "RSSI") == fields[offset])
{
@@ -608,12 +565,15 @@ namespace armarx
}
else
{
- offset++; // necessary????
+ offset++;
}
}
- numEchos = distNum;
- ARMARX_INFO_S << "numDistanceVals: " << distNum << ", numIntensityVals: " << rssiNum;
+ if (distVal.size() != intensityVal.size())
+ {
+ ARMARX_ERROR_S << "Number of distance measurements does not match number of intensity values - Skipping";
+ return sick_scan::ExitError;
+ }
//TODO: Write ScanSteps with intensity
scanData.reserve(distVal.size());
for (int i = 0; i < (int) distVal.size(); i++)
@@ -630,11 +590,6 @@ namespace armarx
// 26 + n + 4 .. count - 4 = device label
// count - 3 .. count - 1 = unknown (but seems to be 0 always)
// <ETX> (\x03)
-
- // msg.range_min = override_range_min_;
- // msg.range_max = override_range_max_;
-
- ARMARX_INFO_S << "parse_datagram(): Time check";
return sick_scan::ExitSuccess;
}
diff --git a/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.h b/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.h
index 76e880aaa..387a558ab 100644
--- a/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.h
+++ b/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.h
@@ -74,7 +74,7 @@ namespace armarx
int loopOnce(LaserScan& scanData);
int parseDatagram(char* datagram, size_t datagram_length, sick_scan::SickScanConfig& config,
- LaserScan& scanData, LaserScannerInfo& scanInfo, int& numEchos, int& echoMask, bool updateScannerInfo = false);
+ LaserScan& scanData, LaserScannerInfo& scanInfo, bool updateScannerInfo = false);
static void disconnectFunctionS(void* obj);
--
GitLab