Skip to content
This repository was archived by the owner on Mar 8, 2023. It is now read-only.

Commit d3634ac

Browse files
rostestrostest
committed
Release 1.8.2, activated OUT4
1 parent 40d3205 commit d3634ac

File tree

10 files changed

+105
-49
lines changed

10 files changed

+105
-49
lines changed

‎cfg/SickScan.cfg‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -74,5 +74,5 @@ gen.add("scan_freq", double_t, 0, "Scan frequency set to 0 to use scanner defaul
7474
gen.add("encoder_mode", int_t, 0, "-1:No Encoder, 0:Off, 1:Single increment, 2:Direction Phase, 3:Direction Level",-1 ,-1,3)
7575
# gen.add("mean_filter", int_t, 0, "Number of averages for mean filter 0 means filter is disabled", 0, 0, 100)
7676
# gen.add("mirror_scan",bool_t, 0, "Scan direction's changed. E.g. for overhead mounting or NAV 310 ( in contrast to other sick scanners NAV 310 is clockwise rotating ).",False)
77-
gen.add("use_safty_fields", bool_t, 0, "Whether or not to use safty fields. Only tim 7xx5 supported at the moment", True)
77+
# gen.add("use_safety_fields", bool_t, 0, "Whether or not to use safety fields. Only tim 7xxS supported at the moment", False) # True)
7878
exit(gen.generate(PACKAGE, "sick_scan", "SickScan"))

‎doc/tim7xxs_screenshot01.png‎

-45 KB
Loading

‎doc/tim7xxs_screenshot02.png‎

-11.3 KB
Loading

‎doc/tim7xxs_screenshot03.png‎

184 KB
Loading

‎driver/src/sick_generic_parser.cpp‎

Lines changed: 21 additions & 18 deletions
Original file line numberDiff line numberDiff line change
@@ -332,18 +332,21 @@ namespace sick_scan
332332
return (useSafetyPasWD);
333333
}
334334

335-
bool ScannerBasicParam::getUseSaftyFields(){
336-
return(useSaftyFields);
335+
bool ScannerBasicParam::getUseSafetyFields(){
336+
return(useSafetyFields);
337337
}
338338

339-
void ScannerBasicParam::setUseSaftyFields(bool _useSaftyFields){
340-
this->useSaftyFields=_useSaftyFields;
339+
void ScannerBasicParam::setUseSafetyFields(bool _useSafetyFields){
340+
this->useSafetyFields=_useSafetyFields;
341341
}
342342
/*!
343343
\brief Construction of parameter object
344344
345345
*/
346346
ScannerBasicParam::ScannerBasicParam()
347+
: numberOfLayers(0), numberOfShots(0), numberOfMaximumEchos(0), elevationDegreeResolution(0), angleDegressResolution(0), expectedFrequency(0),
348+
useBinaryProtocol(false), IntensityResolutionIs16Bit(false), deviceIsRadar(false), useSafetyPasWD(false), encoderMode(0),
349+
CartographerCompatibility(false), scanMirroredAndShifted(false), useSafetyFields(false)
347350
{
348351
this->elevationDegreeResolution = 0.0;
349352
this->setUseBinaryProtocol(false);
@@ -417,7 +420,7 @@ namespace sick_scan
417420
basicParams[i].setUseSafetyPasWD(false); // Default
418421
basicParams[i].setEncoderMode(-1); // Default
419422
basicParams[i].setScanMirroredAndShifted(false);
420-
basicParams[i].setUseSaftyFields(false);
423+
basicParams[i].setUseSafetyFields(false);
421424
}
422425
if (basicParams[i].getScannerName().compare(SICK_SCANNER_LMS_1XXX_NAME) ==
423426
0) // LMS1000 - 4 layer, 1101 shots per scan
@@ -433,7 +436,7 @@ namespace sick_scan
433436
basicParams[i].setUseSafetyPasWD(false); // Default
434437
basicParams[i].setEncoderMode(-1); // Default
435438
basicParams[i].setScanMirroredAndShifted(false);
436-
basicParams[i].setUseSaftyFields(false);
439+
basicParams[i].setUseSafetyFields(false);
437440
}
438441
if (basicParams[i].getScannerName().compare(SICK_SCANNER_TIM_240_NAME) ==
439442
0) // TIM_5xx - 1 Layer, max. 811 shots per scan
@@ -448,7 +451,7 @@ namespace sick_scan
448451
basicParams[i].setUseSafetyPasWD(false); // Default
449452
basicParams[i].setEncoderMode(-1); // Default
450453
basicParams[i].setScanMirroredAndShifted(false);
451-
basicParams[i].setUseSaftyFields(false);
454+
basicParams[i].setUseSafetyFields(false);
452455

453456
}
454457
if (basicParams[i].getScannerName().compare(SICK_SCANNER_TIM_5XX_NAME) ==
@@ -464,7 +467,7 @@ namespace sick_scan
464467
basicParams[i].setUseSafetyPasWD(false); // Default
465468
basicParams[i].setEncoderMode(-1); // Default
466469
basicParams[i].setScanMirroredAndShifted(false);
467-
basicParams[i].setUseSaftyFields(false);
470+
basicParams[i].setUseSafetyFields(false);
468471

469472
}
470473
if (basicParams[i].getScannerName().compare(SICK_SCANNER_LMS_4XXX_NAME) == 0) // LMS_4xxx - 1 Layer, 600 Hz
@@ -479,7 +482,7 @@ namespace sick_scan
479482
basicParams[i].setUseSafetyPasWD(false); // Default
480483
basicParams[i].setEncoderMode(-1); // Default
481484
basicParams[i].setScanMirroredAndShifted(false);
482-
basicParams[i].setUseSaftyFields(false);
485+
basicParams[i].setUseSafetyFields(false);
483486
}
484487
if (basicParams[i].getScannerName().compare(SICK_SCANNER_TIM_7XX_NAME) == 0) // TIM_7xx - 1 Layer Scanner
485488
{
@@ -493,7 +496,7 @@ namespace sick_scan
493496
basicParams[i].setUseSafetyPasWD(false); // Default
494497
basicParams[i].setEncoderMode(-1); // Default
495498
basicParams[i].setScanMirroredAndShifted(false);
496-
basicParams[i].setUseSaftyFields(false);
499+
basicParams[i].setUseSafetyFields(false);
497500
}
498501
if (basicParams[i].getScannerName().compare(SICK_SCANNER_TIM_7XXS_NAME) == 0) // TIM_7xxS - 1 layer Safety Scanner
499502
{
@@ -507,7 +510,7 @@ namespace sick_scan
507510
basicParams[i].setUseSafetyPasWD(true); // Safety scanner
508511
basicParams[i].setEncoderMode(-1); // Default
509512
basicParams[i].setScanMirroredAndShifted(false);
510-
basicParams[i].setUseSaftyFields(true);
513+
basicParams[i].setUseSafetyFields(true);
511514
}
512515
if (basicParams[i].getScannerName().compare(SICK_SCANNER_LMS_5XX_NAME) == 0) // LMS_5xx - 1 Layer
513516
{
@@ -521,7 +524,7 @@ namespace sick_scan
521524
basicParams[i].setUseSafetyPasWD(false); // Default
522525
basicParams[i].setEncoderMode(-1); // Default
523526
basicParams[i].setScanMirroredAndShifted(false);
524-
basicParams[i].setUseSaftyFields(false);
527+
basicParams[i].setUseSafetyFields(false);
525528
}
526529
if (basicParams[i].getScannerName().compare(SICK_SCANNER_LMS_1XX_NAME) == 0) // LMS_1xx - 1 Layer
527530
{
@@ -535,7 +538,7 @@ namespace sick_scan
535538
basicParams[i].setUseSafetyPasWD(false); // Default
536539
basicParams[i].setEncoderMode(-1); // Default
537540
basicParams[i].setScanMirroredAndShifted(false);
538-
basicParams[i].setUseSaftyFields(false);
541+
basicParams[i].setUseSafetyFields(false);
539542
}
540543
if (basicParams[i].getScannerName().compare(SICK_SCANNER_MRS_6XXX_NAME) == 0) //
541544
{
@@ -550,7 +553,7 @@ namespace sick_scan
550553
basicParams[i].setUseSafetyPasWD(false); // Default
551554
basicParams[i].setEncoderMode(-1); // Default
552555
basicParams[i].setScanMirroredAndShifted(false);
553-
basicParams[i].setUseSaftyFields(false);
556+
basicParams[i].setUseSafetyFields(false);
554557
}
555558

556559
if (basicParams[i].getScannerName().compare(SICK_SCANNER_RMS_3XX_NAME) == 0) // Radar
@@ -566,7 +569,7 @@ namespace sick_scan
566569
basicParams[i].setUseSafetyPasWD(false); // Default
567570
basicParams[i].setEncoderMode(-1); // Default
568571
basicParams[i].setScanMirroredAndShifted(false);
569-
basicParams[i].setUseSaftyFields(false);
572+
basicParams[i].setUseSafetyFields(false);
570573
}
571574
if (basicParams[i].getScannerName().compare(SICK_SCANNER_NAV_3XX_NAME) == 0) // Nav 3xx
572575
{
@@ -578,7 +581,7 @@ namespace sick_scan
578581
basicParams[i].setUseBinaryProtocol(true);
579582
basicParams[i].setDeviceIsRadar(false); // Default
580583
basicParams[i].setScanMirroredAndShifted(true);
581-
basicParams[i].setUseSaftyFields(false);
584+
basicParams[i].setUseSafetyFields(false);
582585
}
583586
if (basicParams[i].getScannerName().compare(SICK_SCANNER_NAV_2XX_NAME) == 0) // NAV_2xx - 1 Layer
584587
{
@@ -592,7 +595,7 @@ namespace sick_scan
592595
basicParams[i].setUseSafetyPasWD(false); // Default
593596
basicParams[i].setEncoderMode(-1); // Default
594597
basicParams[i].setScanMirroredAndShifted(false);
595-
basicParams[i].setUseSaftyFields(false);
598+
basicParams[i].setUseSafetyFields(false);
596599
}
597600
if (basicParams[i].getScannerName().compare(SICK_SCANNER_TIM_4XX_NAME) == 0) // TiM433 and TiM443
598601
{
@@ -606,7 +609,7 @@ namespace sick_scan
606609
basicParams[i].setUseSafetyPasWD(false); // Default
607610
basicParams[i].setEncoderMode(-1); // Default
608611
basicParams[i].setScanMirroredAndShifted(false);
609-
basicParams[i].setUseSaftyFields(false);
612+
basicParams[i].setUseSafetyFields(false);
610613
}
611614
}
612615

‎driver/src/sick_scan_common.cpp‎

Lines changed: 4 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -1170,13 +1170,13 @@ namespace sick_scan
11701170

11711171
bool rssiFlag = false;
11721172
bool rssiResolutionIs16Bit = true; //True=16 bit Flase=8bit
1173-
bool useSaftyields=false;
1173+
// bool useSafetyfields=false;
11741174
int activeEchos = 0;
11751175
ros::NodeHandle pn("~");
11761176

11771177
pn.getParam("intensity", rssiFlag);
11781178
pn.getParam("intensity_resolution_16bit", rssiResolutionIs16Bit);
1179-
pn.getParam("use_safty_fields", useSaftyields);
1179+
// pn.getParam("use_safety_fields", useSafetyfields);
11801180
//check new ip adress and add cmds to write ip to comand chain
11811181
std::string sNewIPAddr = "";
11821182
boost::asio::ip::address_v4 ipNewIPAddr;
@@ -1215,7 +1215,7 @@ namespace sick_scan
12151215
}
12161216

12171217
this->parser_->getCurrentParamPtr()->setIntensityResolutionIs16Bit(rssiResolutionIs16Bit);
1218-
this->parser_->getCurrentParamPtr()->setUseSaftyFields(useSaftyields);
1218+
// this->parser_->getCurrentParamPtr()->setUseSafetyFields(useSafetyfields);
12191219
// parse active_echos entry and set flag array
12201220
pn.getParam("active_echos", activeEchos);
12211221

@@ -2106,7 +2106,7 @@ namespace sick_scan
21062106

21072107
}
21082108
//SAFTY FIELD PARSING
2109-
if (this->parser_->getCurrentParamPtr()->getUseSaftyFields())
2109+
if (this->parser_->getCurrentParamPtr()->getUseSafetyFields())
21102110
{
21112111
ROS_INFO("Reading safety fields");
21122112
SickScanFieldMonSingleton *fieldMon = SickScanFieldMonSingleton::getInstance();

‎driver/src/sick_scan_messages.cpp‎

Lines changed: 68 additions & 16 deletions
Original file line numberDiff line numberDiff line change
@@ -85,15 +85,44 @@ bool sick_scan::SickScanMessages::parseLIDoutputstateMsg(const ros::Time& timeSt
8585
if(useBinaryProtocol)
8686
{
8787
// parse and convert LIDoutputstate message
88+
int dbg_telegram_length = receiveLength;
89+
std::string dbg_telegram_hex_copy = DataDumper::binDataToAsciiString(receiveBuffer, receiveLength);
8890
int msg_start_idx = 27; // 4 byte STX + 4 byte payload_length + 19 byte "sSN LIDoutputstate " = 27 byte
8991
int msg_parameter_length = receiveLength - msg_start_idx - 1; // start bytes + 1 byte CRC
90-
int msg_output_states_length = msg_parameter_length - 18; // 2 byte version + 4 byte system + 12 byte timestamp = 18 byte
91-
int number_of_output_states = msg_output_states_length / 5; // each output state has 1 byte state enum + 4 byte counter = 5 byte
92-
if((msg_output_states_length % 5) != 0 || number_of_output_states <= 0)
92+
if(msg_parameter_length < 8) // at least 6 byte (version+system) + 2 byte (timestamp status)
9393
{
94-
ROS_ERROR_STREAM("## ERROR SickScanMessages::parseLIDoutputstateMsg(): received " << receiveLength << " byte with " << msg_output_states_length << " byte states, expected multiple of 5 (" << __FILE__ << ":" << __LINE__ << ")");
94+
ROS_ERROR_STREAM("## ERROR SickScanMessages::parseLIDoutputstateMsg(): received " << receiveLength << " byte, expected at least 8 byte (" << __FILE__ << ":" << __LINE__ << ")");
9595
return false;
9696
}
97+
// The LIDoutputstate parameter may have 11 byte timestamp or not, i.e. the parameter format is either:
98+
// a) msg_parameter_length := (6 byte version+system) + number_of_output_states * (5 byte state+counter per output_state) + (2 byte 0x0000 no timestamp), or
99+
// b) msg_parameter_length := (6 byte version+system) + number_of_output_states * (5 byte state+counter per output_state) + (2 byte 0x0001 timestamp available) + (11 byte timestamp)
100+
bool parameter_format_ok = false;
101+
// Check parameter format a (default, TiM781S: no timestamp)
102+
uint16_t timestamp_status = (receiveBuffer[receiveLength - 3] << 8) | (receiveBuffer[receiveLength - 2]); // 2 byte before receiveBuffer[receiveLength-1] = CRC
103+
int number_of_output_states = (msg_parameter_length - 6 - 2) / 5; // each output state has 1 byte state enum + 4 byte counter = 5 byte
104+
if(msg_parameter_length == 6 + number_of_output_states * 5 + 2 && timestamp_status == 0)
105+
{
106+
parameter_format_ok = true;
107+
ROS_DEBUG_STREAM("SickScanMessages::parseLIDoutputstateMsg(): " << number_of_output_states << " output states, no timestamp" );
108+
}
109+
else // Check parameter format b with timestamp
110+
{
111+
timestamp_status = (receiveBuffer[receiveLength - 14] << 8) | (receiveBuffer[receiveLength - 13]); // 2 byte before receiveBuffer[receiveLength-12] = (11 byte timestamp) + (1 byte CRC)
112+
number_of_output_states = (msg_parameter_length - 6 - 2 - 11) / 5; // each output state has 1 byte state enum + 4 byte counter = 5 byte
113+
if(msg_parameter_length == 6 + number_of_output_states * 5 + 2 + 11 && timestamp_status > 0)
114+
{
115+
parameter_format_ok = true;
116+
ROS_DEBUG_STREAM("SickScanMessages::parseLIDoutputstateMsg(): " << number_of_output_states << " output states and timestamp" );
117+
}
118+
}
119+
if(!parameter_format_ok)
120+
{
121+
ROS_ERROR_STREAM("## ERROR SickScanMessages::parseLIDoutputstateMsg(): received " << receiveLength << " byte with invalid format (" << __FILE__ << ":" << __LINE__ << "): "
122+
<< DataDumper::binDataToAsciiString(receiveBuffer, receiveLength));
123+
return false;
124+
}
125+
// Read 2 byte version + 2 byte system status
97126
receiveBuffer += msg_start_idx;
98127
receiveLength -= msg_start_idx;
99128
if( !readBinaryBuffer(receiveBuffer, receiveLength, output_msg.version_number)
@@ -102,6 +131,7 @@ bool sick_scan::SickScanMessages::parseLIDoutputstateMsg(const ros::Time& timeSt
102131
ROS_ERROR_STREAM("## ERROR SickScanMessages::parseLIDoutputstateMsg(): error parsing version_number and system_counter (" << __FILE__ << ":" << __LINE__ << ")");
103132
return false;
104133
}
134+
// Read N output states
105135
output_msg.output_state.reserve(number_of_output_states);
106136
output_msg.output_count.reserve(number_of_output_states);
107137
for(int state_cnt = 0; state_cnt < number_of_output_states; state_cnt++)
@@ -114,29 +144,51 @@ bool sick_scan::SickScanMessages::parseLIDoutputstateMsg(const ros::Time& timeSt
114144
ROS_ERROR_STREAM("## ERROR SickScanMessages::parseLIDoutputstateMsg(): error parsing version_number and system_counter (" << __FILE__ << ":" << __LINE__ << ")");
115145
return false;
116146
}
117-
output_msg.output_state.push_back(output_state);
118-
output_msg.output_count.push_back(output_count);
147+
if(output_state == 0 || output_state == 1) // 0: not active, 1: active, 2: not used
148+
{
149+
output_msg.output_state.push_back(output_state);
150+
output_msg.output_count.push_back(output_count);
151+
}
119152
}
120-
if( !readBinaryBuffer(receiveBuffer, receiveLength, output_msg.time_state)
121-
|| !readBinaryBuffer(receiveBuffer, receiveLength, output_msg.year)
122-
|| !readBinaryBuffer(receiveBuffer, receiveLength, output_msg.month)
123-
|| !readBinaryBuffer(receiveBuffer, receiveLength, output_msg.day)
124-
|| !readBinaryBuffer(receiveBuffer, receiveLength, output_msg.hour)
125-
|| !readBinaryBuffer(receiveBuffer, receiveLength, output_msg.minute)
126-
|| !readBinaryBuffer(receiveBuffer, receiveLength, output_msg.second)
127-
|| !readBinaryBuffer(receiveBuffer, receiveLength, output_msg.microsecond))
153+
// Read timestamp state
154+
if( !readBinaryBuffer(receiveBuffer, receiveLength, output_msg.time_state))
128155
{
129-
ROS_ERROR_STREAM("## ERROR SickScanMessages::parseLIDoutputstateMsg(): error parsing timestamp (" << __FILE__ << ":" << __LINE__ << ")");
156+
ROS_ERROR_STREAM("## ERROR SickScanMessages::parseLIDoutputstateMsg(): error parsing time_state (" << __FILE__ << ":" << __LINE__ << ")");
130157
return false;
131158
}
159+
if(output_msg.time_state != timestamp_status) // time_state and previously parsed timestamp_status must be identical
160+
{
161+
ROS_ERROR_STREAM("## ERROR SickScanMessages::parseLIDoutputstateMsg(): time_state mismatch, received " << (int)output_msg.time_state << ", expected " << (int)timestamp_status << " (" << __FILE__ << ":" << __LINE__ << ")");
162+
return false;
163+
164+
}
165+
// Read optional timestamp
166+
if(timestamp_status > 0)
167+
{
168+
if(!readBinaryBuffer(receiveBuffer, receiveLength, output_msg.year)
169+
|| !readBinaryBuffer(receiveBuffer, receiveLength, output_msg.month)
170+
|| !readBinaryBuffer(receiveBuffer, receiveLength, output_msg.day)
171+
|| !readBinaryBuffer(receiveBuffer, receiveLength, output_msg.hour)
172+
|| !readBinaryBuffer(receiveBuffer, receiveLength, output_msg.minute)
173+
|| !readBinaryBuffer(receiveBuffer, receiveLength, output_msg.second)
174+
|| !readBinaryBuffer(receiveBuffer, receiveLength, output_msg.microsecond))
175+
{
176+
ROS_ERROR_STREAM("## ERROR SickScanMessages::parseLIDoutputstateMsg(): error parsing timestamp (" << __FILE__ << ":" << __LINE__ << ")");
177+
return false;
178+
}
179+
}
132180
output_msg.header.stamp = timeStamp;
133181
output_msg.header.seq = 0;
134182
output_msg.header.frame_id = frame_id;
135183

184+
// Debug messages
136185
std::stringstream state_str;
137-
for(int state_cnt = 0; state_cnt < number_of_output_states; state_cnt++)
186+
assert(output_msg.output_state.size() == output_msg.output_state.size());
187+
state_str << number_of_output_states << " output states received" << (timestamp_status?" with":", no") << " timestamp\n";
188+
for(int state_cnt = 0; state_cnt < output_msg.output_count.size(); state_cnt++)
138189
state_str << "state[" << state_cnt << "]: " << (uint32_t)output_msg.output_state[state_cnt] << ", count=" << (uint32_t)output_msg.output_count[state_cnt] << "\n";
139190
ROS_DEBUG_STREAM("SickScanMessages::parseLIDoutputstateMsg():\n"
191+
<< dbg_telegram_length << " byte telegram: " << dbg_telegram_hex_copy << "\n"
140192
<< "version_number: " << (uint32_t)output_msg.version_number << ", system_counter: " << (uint32_t)output_msg.system_counter << "\n"
141193
<< state_str.str()
142194
<< "time state: " << (uint32_t)output_msg.time_state

‎include/sick_scan/sick_generic_parser.h‎

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -115,9 +115,9 @@ namespace sick_scan
115115

116116
int8_t getEncoderMode();
117117

118-
bool getUseSaftyFields();
118+
bool getUseSafetyFields();
119119

120-
void setUseSaftyFields(bool _useSaftyFields);
120+
void setUseSafetyFields(bool _useSafetyFields);
121121

122122
private:
123123
std::string scannerName;
@@ -134,7 +134,7 @@ namespace sick_scan
134134
int8_t encoderMode;
135135
bool CartographerCompatibility;
136136
bool scanMirroredAndShifted;
137-
bool useSaftyFields;
137+
bool useSafetyFields;
138138
};
139139

140140

‎msg/LIDoutputstateMsg.msg‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -12,7 +12,7 @@ uint8[] output_state # output states, each state with value 0 (not active), 1 (
1212
uint32[] output_count # output counter
1313

1414
# Time block (sensortime from the last change of min. one of the outputs)
15-
uint8 time_state # No time data: 0, Time data: 1
15+
uint16 time_state # No time data: 0, Time data: 1
1616
uint16 year # f.e. 2021
1717
uint8 month # 1 ... 12
1818
uint8 day # 1 ... 31

0 commit comments

Comments
 (0)