From f5afc07397ff1312277f73f2524afec9e823e39b Mon Sep 17 00:00:00 2001 From: ipa-mig Date: Mon, 13 Feb 2017 15:13:54 +0100 Subject: [PATCH 1/8] cob_sick_s300: clarify telegram calculation according to telegram listing --- .../include/cob_sick_s300/ScannerSickS300.h | 2 +- .../include/cob_sick_s300/TelegramS300.h | 93 +++++++++++-------- cob_sick_s300/common/src/ScannerSickS300.cpp | 2 +- 3 files changed, 57 insertions(+), 40 deletions(-) diff --git a/cob_sick_s300/common/include/cob_sick_s300/ScannerSickS300.h b/cob_sick_s300/common/include/cob_sick_s300/ScannerSickS300.h index b8ab93b68..72c43fdc3 100644 --- a/cob_sick_s300/common/include/cob_sick_s300/ScannerSickS300.h +++ b/cob_sick_s300/common/include/cob_sick_s300/ScannerSickS300.h @@ -156,7 +156,7 @@ class ScannerSickS300 //sick_lms.Uninitialize(); // whether the scanner is currently in Standby or not - bool isInStandby() {return m_bInStandby;}; + bool isInStandby() {return m_bInStandby;} void purgeScanBuf(); diff --git a/cob_sick_s300/common/include/cob_sick_s300/TelegramS300.h b/cob_sick_s300/common/include/cob_sick_s300/TelegramS300.h index 1316268f8..6345973d7 100644 --- a/cob_sick_s300/common/include/cob_sick_s300/TelegramS300.h +++ b/cob_sick_s300/common/include/cob_sick_s300/TelegramS300.h @@ -62,7 +62,7 @@ class TelegramParser { struct { uint32_t reply_telegram; uint16_t trigger_result; - uint16_t size; + uint16_t size; // in 16bit=2byte words uint8_t coordination_flag; uint8_t device_addresss; }; @@ -211,10 +211,14 @@ class TelegramParser { TELEGRAM_COMMON2 tc2_; TELEGRAM_COMMON3 tc3_; TELEGRAM_DISTANCE td_; - int last_offset_; + int length_count_start_, length_count_crc_, user_data_size_; public: - TelegramParser() : last_offset_(0) {} + TelegramParser() : + length_count_start_(0), + length_count_crc_(0), + user_data_size_(0) + {} bool parseHeader(const unsigned char *buffer, const size_t max_size, const uint8_t DEVICE_ADDR, const bool debug) { @@ -229,47 +233,62 @@ class TelegramParser { ntoh(tc1_); if(debug) print(tc1_); - if(tc1_.size*2+JUNK_SIZE>(int)max_size) { + tc2_ = *((TELEGRAM_COMMON2*)(buffer+sizeof(TELEGRAM_COMMON1))); + tc3_ = *((TELEGRAM_COMMON3*)(buffer+(sizeof(TELEGRAM_COMMON1)+sizeof(TELEGRAM_COMMON2)))); + + // The length reported by the protocol varies depending on the calculation which is different depending + // on several factors. + // The calculation is described on pp. 70-73 in: + // https://www.sick.com/media/dox/1/91/891/Telegram_listing_S3000_Expert_Anti_Collision_S300_Expert_de_en_IM0022891.PDF + // + // For the old protocol/compatability mode: + // "The telegram size is calculated ... starting with the ... 5. byte ... up to and including the ... CRC." + if(tc2_.protocol_version==0x102) + { + length_count_start_ = 4; // start at 5th byte + length_count_crc_ = 2; // include 2 bytes CRC + } + // + // If NO I/O or measuring fields are configured: + // "The telegram size is calculated ... starting with the ... 5. byte ... up to and including the ... CRC." + else if (tc3_.type != IO && tc3_.type != DISTANCE) + { + length_count_start_ = 4; // start at 5th byte + length_count_crc_ = 2; // include 2 bytes CRC + } + // If any I/O or measuring field is configured: + // "The telegram size is calculated ... starting with the ... 13. byte ... up to and including the last byte ... bevore (sic!) the CRC." + else + { + length_count_start_ = 12; // start at 13th byte + length_count_crc_ = 0; // do NOT include 2 bytes CRC + } + + // the user_data_size is the size of the actual payload data, i.e. all data except of the CRC and the first two common telegrams + // Accordingly, the full size of the telegramm including any bytes is + // sizeof(TELEGRAM_COMMON1)+sizeof(TELEGRAM_COMMON2)+user_data_size_+sizeof(TELEGRAM_TAIL) + user_data_size_ = + 2*tc1_.size-(sizeof(TELEGRAM_COMMON1)+sizeof(TELEGRAM_COMMON2)-length_count_start_+length_count_crc_); // in bytes + + if( (sizeof(TELEGRAM_COMMON1)+sizeof(TELEGRAM_COMMON2)+user_data_size_+sizeof(TELEGRAM_TAIL)) > (int)max_size) + { if(debug) std::cout<<"invalid header size"< &res) const + void readDistRaw(const unsigned char *buffer, std::vector &res, bool debug) const { res.clear(); if(!isDist()) return; - size_t num_points = (2*tc1_.size - (sizeof(tc1_)+sizeof(tc2_)+sizeof(tc3_)+sizeof(td_)+sizeof(TELEGRAM_TAIL)-JUNK_SIZE-last_offset_)); - //std::cout<<"num_points: "< &vdDistanceM, std::vector0) { // Scan was succesfully read from buffer bRet = true; From 59c486fdf5d479978cf27b9e185660aa3026bce7 Mon Sep 17 00:00:00 2001 From: ipa-mig Date: Mon, 13 Feb 2017 15:21:59 +0100 Subject: [PATCH 2/8] cob_sick_s300: move telegram description to telegram parser file --- .../include/cob_sick_s300/ScannerSickS300.h | 32 +------------------ .../include/cob_sick_s300/TelegramS300.h | 22 +++++++++++++ 2 files changed, 23 insertions(+), 31 deletions(-) diff --git a/cob_sick_s300/common/include/cob_sick_s300/ScannerSickS300.h b/cob_sick_s300/common/include/cob_sick_s300/ScannerSickS300.h index 72c43fdc3..48daa3f4f 100644 --- a/cob_sick_s300/common/include/cob_sick_s300/ScannerSickS300.h +++ b/cob_sick_s300/common/include/cob_sick_s300/ScannerSickS300.h @@ -70,37 +70,7 @@ * Driver class for the laser scanner SICK S300 Professional. * This driver only supports use with 500KBaud in cont. mode * - * S300 header format in continuous mode: - * - * | 00 00 00 00 | 4 byte reply header - * - * Now starts the actual telegram - * - * | 00 00 | data block number (fixed) - * | xx xx | size of data telegram (should be dec 1104) - * | FF xx | last byte decides scanner id, 07 in most cases, but 08 for slave configured scanners - * | xx xx | protocol version - * | 0x 00 | status: 00 00 = normal, 01 00 = lockout - * | xx xx xx xx | scan number - * | xx xx | telegram number - * | BB BB | fixed - * | 11 11 | fixed - * ... data - * | xx xx | CRC - * - * Readout of buffer starts with Reply-Header - * Reply-Header:byte 0 to 3 = 4 bytes - * Telegram (as it is stored in the Buffer): Position in the telegram - * Header: bytes 4 to 23 = 20 bytes bytes 0 to 19 - * Data: bytes 24 to 1105 = 1082 bytes bytes 20 to 1101 - * CRC: bytes 1106, 1107 = 2 bytes bytes 1102, 1103 - * - * for easier parsing Reply-Header and Telegram-Header are combined in the folllowing - * --> Headerlength = 24 bytes (iHeaderLength) - * --> Total length in buffer is 1108 bytes - * --> Telegram length (read from telegram) is 1104 bytes (iDataLength) - * - * if the scanner is in standby, the measurements are 0x4004 according to the Sick Support + * if the scanner is in standby, the measurements are 0x4004 according to the Sick Support */ class ScannerSickS300 diff --git a/cob_sick_s300/common/include/cob_sick_s300/TelegramS300.h b/cob_sick_s300/common/include/cob_sick_s300/TelegramS300.h index 6345973d7..c46ff210b 100644 --- a/cob_sick_s300/common/include/cob_sick_s300/TelegramS300.h +++ b/cob_sick_s300/common/include/cob_sick_s300/TelegramS300.h @@ -55,6 +55,28 @@ #include +/* +* S300 header format in continuous mode: +* +* | 00 00 00 00 | 4 byte reply header +* | 00 00 | data block number (fixed for continuous output) +* | xx xx | size of data telegram in 16-bit data words +* | FF xx | coordination flag and device address (07 in most cases, but 08 for slave configured scanners) +* | xx xx | protocol version (02 01 for old protocol,.otherwise 03 01) +* | 0x 00 | status: 00 00 = normal, 01 00 = lockout +* | xx xx xx xx | scan number (time stamp) +* | xx xx | telegram number +* | BB BB | ID of output (AAAA=I/O, BBBB=range measruements, CCCC=reflector measurements) +* | 11 11 | number of configures measurement field (1111, 2222, 3333, 4444 or 5555) +* ... data +* | xx xx | CRC +* +* in this parser, user_data_ denotes all but the first 20 bytes (up to and including telegram number above) +* and the last two bytes (CRC) +* +*/ + + class TelegramParser { #pragma pack(push,1) From 57ae0bcb8c543397d137650b6f1f4e15c8fd58e0 Mon Sep 17 00:00:00 2001 From: ipa-mig Date: Mon, 13 Feb 2017 14:33:47 +0000 Subject: [PATCH 3/8] cob_sick_s300: comment warning about field parameters --> this is currently buggy, old version works better --- cob_sick_s300/common/include/cob_sick_s300/TelegramS300.h | 2 +- cob_sick_s300/ros/src/cob_sick_s300.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/cob_sick_s300/common/include/cob_sick_s300/TelegramS300.h b/cob_sick_s300/common/include/cob_sick_s300/TelegramS300.h index c46ff210b..5d58787bb 100644 --- a/cob_sick_s300/common/include/cob_sick_s300/TelegramS300.h +++ b/cob_sick_s300/common/include/cob_sick_s300/TelegramS300.h @@ -357,7 +357,7 @@ class TelegramParser { if(!isDist()) return; size_t num_points = (user_data_size_-sizeof(TELEGRAM_COMMON3)-sizeof(TELEGRAM_DISTANCE))/sizeof(TELEGRAM_S300_DIST_2B); - //if (debug) std::cout << "Number of points: " << std::dec << num_points << std::endl; + if (debug) std::cout << "Number of points: " << std::dec << num_points << std::endl; for(size_t i=0; i will using default, but it's deprecated now, please adjust parameters!!!"); + //ROS_WARN("No params for the Sick S300 fieldset were specified --> will using default, but it's deprecated now, please adjust parameters!!!"); //setting defaults to be backwards compatible ScannerSickS300::ParamType param; From 3cfe7bb482062f0078ca5b62fd3db96476191f2d Mon Sep 17 00:00:00 2001 From: ipa-mig Date: Mon, 27 Feb 2017 10:11:11 +0100 Subject: [PATCH 4/8] cob_sick_s300: properly get offsets and add corresponding docu to source code --- .../include/cob_sick_s300/TelegramS300.h | 97 ++++++++++++------- 1 file changed, 63 insertions(+), 34 deletions(-) diff --git a/cob_sick_s300/common/include/cob_sick_s300/TelegramS300.h b/cob_sick_s300/common/include/cob_sick_s300/TelegramS300.h index 5d58787bb..21f39ab63 100644 --- a/cob_sick_s300/common/include/cob_sick_s300/TelegramS300.h +++ b/cob_sick_s300/common/include/cob_sick_s300/TelegramS300.h @@ -233,12 +233,12 @@ class TelegramParser { TELEGRAM_COMMON2 tc2_; TELEGRAM_COMMON3 tc3_; TELEGRAM_DISTANCE td_; - int length_count_start_, length_count_crc_, user_data_size_; + int size_field_start_byte_, crc_bytes_in_size_, user_data_size_; public: TelegramParser() : - length_count_start_(0), - length_count_crc_(0), + size_field_start_byte_(0), + crc_bytes_in_size_(0), user_data_size_(0) {} @@ -258,59 +258,84 @@ class TelegramParser { tc2_ = *((TELEGRAM_COMMON2*)(buffer+sizeof(TELEGRAM_COMMON1))); tc3_ = *((TELEGRAM_COMMON3*)(buffer+(sizeof(TELEGRAM_COMMON1)+sizeof(TELEGRAM_COMMON2)))); - // The length reported by the protocol varies depending on the calculation which is different depending + TELEGRAM_TAIL tt; + uint16_t crc; + int full_data_size = 0; + // The size reported by the protocol varies depending on the calculation which is different depending // on several factors. // The calculation is described on pp. 70-73 in: // https://www.sick.com/media/dox/1/91/891/Telegram_listing_S3000_Expert_Anti_Collision_S300_Expert_de_en_IM0022891.PDF - // + // + // Also, the size is reported as 16bit-words = 2 bytes... + // // For the old protocol/compatability mode: // "The telegram size is calculated ... starting with the ... 5. byte ... up to and including the ... CRC." if(tc2_.protocol_version==0x102) { - length_count_start_ = 4; // start at 5th byte - length_count_crc_ = 2; // include 2 bytes CRC + size_field_start_byte_ = 4; // start at 5th byte (started numbering at 0) + crc_bytes_in_size_ = 2; // include 2 bytes CRC + + // the user_data_size is the size of the actual payload data in bytes, + //i.e. all data except of the CRC and the first two common telegrams + user_data_size_ = + 2*tc1_.size - + (sizeof(TELEGRAM_COMMON1) + sizeof(TELEGRAM_COMMON2) - size_field_start_byte_ + crc_bytes_in_size_); + full_data_size = sizeof(TELEGRAM_COMMON1)+sizeof(TELEGRAM_COMMON2)+user_data_size_+sizeof(TELEGRAM_TAIL); + + tt = *((TELEGRAM_TAIL*) (buffer+(sizeof(TELEGRAM_COMMON1)+sizeof(TELEGRAM_COMMON2)+user_data_size_)) ); + ntoh(tt); + crc = createCRC((uint8_t*)buffer+JUNK_SIZE, full_data_size-JUNK_SIZE-sizeof(TELEGRAM_TAIL)); } - // - // If NO I/O or measuring fields are configured: - // "The telegram size is calculated ... starting with the ... 5. byte ... up to and including the ... CRC." - else if (tc3_.type != IO && tc3_.type != DISTANCE) - { - length_count_start_ = 4; // start at 5th byte - length_count_crc_ = 2; // include 2 bytes CRC - } - // If any I/O or measuring field is configured: - // "The telegram size is calculated ... starting with the ... 13. byte ... up to and including the last byte ... bevore (sic!) the CRC." + // Special handling for the new protocol, as this cannot be deduced from the protocol itself + // Thus, we have to try both possibilities and check against the CRC... else { - length_count_start_ = 12; // start at 13th byte - length_count_crc_ = 0; // do NOT include 2 bytes CRC + // If NO I/O or measuring fields are configured: + // "The telegram size is calculated ... starting with the ... 9. byte ... up to and including the ... CRC." + size_field_start_byte_ = 8; // start at 9th byte (started numbering at 0) + crc_bytes_in_size_ = 2; // include 2 bytes CRC + user_data_size_ = + 2*tc1_.size - + (sizeof(TELEGRAM_COMMON1) + sizeof(TELEGRAM_COMMON2) - size_field_start_byte_ + crc_bytes_in_size_); + full_data_size = sizeof(TELEGRAM_COMMON1)+sizeof(TELEGRAM_COMMON2)+user_data_size_+sizeof(TELEGRAM_TAIL); + + tt = *((TELEGRAM_TAIL*) (buffer+(sizeof(TELEGRAM_COMMON1)+sizeof(TELEGRAM_COMMON2)+user_data_size_)) ); + ntoh(tt); + crc = createCRC((uint8_t*)buffer+JUNK_SIZE, full_data_size-JUNK_SIZE-sizeof(TELEGRAM_TAIL)); + + if(tt.crc!=crc) + { + // If any I/O or measuring field is configured: + // "The telegram size is calculated ... starting with the ... 13. byte ... up to and including the + // last byte ... bevore (sic!) the CRC." + size_field_start_byte_ = 12; // start at 13th byte (started numbering at 0) + crc_bytes_in_size_ = 0; // do NOT include 2 bytes CRC + user_data_size_ = + 2*tc1_.size - + (sizeof(TELEGRAM_COMMON1) + sizeof(TELEGRAM_COMMON2) - size_field_start_byte_ + crc_bytes_in_size_); + full_data_size = + sizeof(TELEGRAM_COMMON1)+sizeof(TELEGRAM_COMMON2)+user_data_size_+sizeof(TELEGRAM_TAIL); + + tt = *((TELEGRAM_TAIL*) (buffer+(sizeof(TELEGRAM_COMMON1)+sizeof(TELEGRAM_COMMON2)+user_data_size_)) ); + ntoh(tt); + crc = createCRC((uint8_t*)buffer+JUNK_SIZE, full_data_size-JUNK_SIZE-sizeof(TELEGRAM_TAIL)); + } } - // the user_data_size is the size of the actual payload data, i.e. all data except of the CRC and the first two common telegrams - // Accordingly, the full size of the telegramm including any bytes is - // sizeof(TELEGRAM_COMMON1)+sizeof(TELEGRAM_COMMON2)+user_data_size_+sizeof(TELEGRAM_TAIL) - user_data_size_ = - 2*tc1_.size-(sizeof(TELEGRAM_COMMON1)+sizeof(TELEGRAM_COMMON2)-length_count_start_+length_count_crc_); // in bytes - + std::cout << std::dec << user_data_size_ << " " << full_data_size << " " << 2*tc1_.size << std::hex << std::endl; if( (sizeof(TELEGRAM_COMMON1)+sizeof(TELEGRAM_COMMON2)+user_data_size_+sizeof(TELEGRAM_TAIL)) > (int)max_size) { if(debug) std::cout<<"invalid header size"< Date: Mon, 27 Feb 2017 11:00:21 +0100 Subject: [PATCH 5/8] cob_sick_s300: remove debug print out --- cob_sick_s300/common/include/cob_sick_s300/TelegramS300.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/cob_sick_s300/common/include/cob_sick_s300/TelegramS300.h b/cob_sick_s300/common/include/cob_sick_s300/TelegramS300.h index 21f39ab63..5a04b2a0b 100644 --- a/cob_sick_s300/common/include/cob_sick_s300/TelegramS300.h +++ b/cob_sick_s300/common/include/cob_sick_s300/TelegramS300.h @@ -261,6 +261,7 @@ class TelegramParser { TELEGRAM_TAIL tt; uint16_t crc; int full_data_size = 0; + // The size reported by the protocol varies depending on the calculation which is different depending // on several factors. // The calculation is described on pp. 70-73 in: @@ -286,7 +287,7 @@ class TelegramParser { ntoh(tt); crc = createCRC((uint8_t*)buffer+JUNK_SIZE, full_data_size-JUNK_SIZE-sizeof(TELEGRAM_TAIL)); } - // Special handling for the new protocol, as this cannot be deduced from the protocol itself + // Special handling for the new protocol, as the settings cannot be fully deduced from the protocol itself // Thus, we have to try both possibilities and check against the CRC... else { @@ -322,7 +323,6 @@ class TelegramParser { } } - std::cout << std::dec << user_data_size_ << " " << full_data_size << " " << 2*tc1_.size << std::hex << std::endl; if( (sizeof(TELEGRAM_COMMON1)+sizeof(TELEGRAM_COMMON2)+user_data_size_+sizeof(TELEGRAM_TAIL)) > (int)max_size) { if(debug) std::cout<<"invalid header size"< Date: Mon, 27 Feb 2017 11:25:16 +0100 Subject: [PATCH 6/8] cob_sick_s300: add readme --- cob_sick_s300/README.md | 22 ++++++++++++++++++++++ 1 file changed, 22 insertions(+) create mode 100644 cob_sick_s300/README.md diff --git a/cob_sick_s300/README.md b/cob_sick_s300/README.md new file mode 100644 index 000000000..5ec636bc8 --- /dev/null +++ b/cob_sick_s300/README.md @@ -0,0 +1,22 @@ +# cob_sick_s300 +This package implements a driver for the Sick S300 Safety laser scanners. +It provides an implementation for both, the old (1.40) and the new (2.10) protocol. +Thus, the old Sick S300 Professional CMS as well as the new Sick S300 Expert are supported. + +However, it does not cover the full functionality of the protocol: +- It only handle distance measurements properly +- It only handles no or only one configured measurement range field properly +- It does not handle I/O-data or reflector data +(though it reads the reflector marker field in the distance measurements) + +See http://wiki.ros.org/cob_sick_s300 for more details. + +## S300 Configuration +Here are a few notes about how to best configure the S300: +- Configure the RS422 output to 500kBaud (otherwise, the scanner only provides a lower frequency) +- Configure the scanner to Continuous Data Output +- Send data via one telegram +- Only configure distances, no I/O or reflector data (otherwise, the scanner only provides a lower frequency). +- Do not configure a measurement range field (otherwise, the scanner only provides a lower frequency). +If you want to only use certain measurement ranges, do this on the ROS side using e.g. the `cob_scan_filter` +located in this package as well. From 15024ba6763dd28fb84d57a05331ab14cc159dfa Mon Sep 17 00:00:00 2001 From: ipa-mig Date: Mon, 27 Feb 2017 11:48:54 +0100 Subject: [PATCH 7/8] cob_sick_s300: clarify some points about the measurement range fields in the readme --- cob_sick_s300/README.md | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/cob_sick_s300/README.md b/cob_sick_s300/README.md index 5ec636bc8..6ec38d4cb 100644 --- a/cob_sick_s300/README.md +++ b/cob_sick_s300/README.md @@ -17,6 +17,9 @@ Here are a few notes about how to best configure the S300: - Configure the scanner to Continuous Data Output - Send data via one telegram - Only configure distances, no I/O or reflector data (otherwise, the scanner only provides a lower frequency). -- Do not configure a measurement range field (otherwise, the scanner only provides a lower frequency). -If you want to only use certain measurement ranges, do this on the ROS side using e.g. the `cob_scan_filter` +- Configuration of the measurement ranges + - For protocol 1.40: only configure one measurement range field with the full range (-45° to 225°) with all values. + - For protocol 2.10: do not configure a measurement range field + (otherwise, the scanner only provides a lower frequency). +- If you want to only use certain measurement ranges, do this on the ROS side using e.g. the `cob_scan_filter` located in this package as well. From 0c68fa8851b1ef7e52188b4aff6c93b00ae39619 Mon Sep 17 00:00:00 2001 From: Matthias Gruhler Date: Fri, 3 Mar 2017 09:24:27 +0100 Subject: [PATCH 8/8] [cob_sick_s300] fix typo in Readme --- cob_sick_s300/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cob_sick_s300/README.md b/cob_sick_s300/README.md index 6ec38d4cb..f92c1c21a 100644 --- a/cob_sick_s300/README.md +++ b/cob_sick_s300/README.md @@ -4,7 +4,7 @@ It provides an implementation for both, the old (1.40) and the new (2.10) protoc Thus, the old Sick S300 Professional CMS as well as the new Sick S300 Expert are supported. However, it does not cover the full functionality of the protocol: -- It only handle distance measurements properly +- It only handles distance measurements properly - It only handles no or only one configured measurement range field properly - It does not handle I/O-data or reflector data (though it reads the reflector marker field in the distance measurements)