cola_converter.cpp
Go to the documentation of this file.
1 /*
2  * @brief sim_loc_cola_converter converts between Cola-ASCII and Cola-Binary telegrams.
3  * See Operation-Instruction-v1.1.0.241R.pdf, chapter 5.8 "About CoLa-A telegrams", page 46-48,
4  * Telegram-Listing-v1.1.0.241R.pdf, chapter 2.3.9 "Command: LocRequestTimestamp", page 21, and
5  * Technical_information_Telegram_Listing_NAV_LOC_en_IM0076556.PDF for further details about
6  * Cola telegrams.
7  *
8  * Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim
9  * Copyright (C) 2019 SICK AG, Waldkirch
10  *
11  * Licensed under the Apache License, Version 2.0 (the "License");
12  * you may not use this file except in compliance with the License.
13  * You may obtain a copy of the License at
14  *
15  * http://www.apache.org/licenses/LICENSE-2.0
16  *
17  * Unless required by applicable law or agreed to in writing, software
18  * distributed under the License is distributed on an "AS IS" BASIS,
19  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
20  * See the License for the specific language governing permissions and
21  * limitations under the License.
22  *
23  * All rights reserved.
24  *
25  * Redistribution and use in source and binary forms, with or without
26  * modification, are permitted provided that the following conditions are met:
27  *
28  * * Redistributions of source code must retain the above copyright
29  * notice, this list of conditions and the following disclaimer.
30  * * Redistributions in binary form must reproduce the above copyright
31  * notice, this list of conditions and the following disclaimer in the
32  * documentation and/or other materials provided with the distribution.
33  * * Neither the name of SICK AG nor the names of its
34  * contributors may be used to endorse or promote products derived from
35  * this software without specific prior written permission
36  * * Neither the name of Ing.-Buero Dr. Michael Lehning nor the names of its
37  * contributors may be used to endorse or promote products derived from
38  * this software without specific prior written permission
39  *
40  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
41  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
42  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
43  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
44  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
45  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
46  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
47  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
48  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
49  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
50  * POSSIBILITY OF SUCH DAMAGE.
51  *
52  * Authors:
53  * Michael Lehning <michael.lehning@lehning.de>
54  *
55  * Copyright 2019 SICK AG
56  * Copyright 2019 Ing.-Buero Dr. Michael Lehning
57  *
58  */
59 #include "sick_scan/ros_wrapper.h"
60 #include <sstream>
61 #include <string.h>
62 
63 #include "sick_scan/cola_converter.h"
64 #include "sick_scan/cola_parser.h"
65 
69 const std::string sick_scan::ColaAsciiBinaryConverter::s_ascii_table[256] =
70  {
71  "", "<SOH>", "<STX>", "<ETX>", "<EOT>", "<ENQ>", "<ACK>", "<BEL>", "\b", "<HT>", "\n", "<VT>", "<FF>", "\n",
72  "<SO>", "<SI>", "<DLE>", "<DC1>", "<DC2>", "<DC3>", "<DC4>", "<NAK>", "<SYN>", "<ETB>", "<CAN>", "<EM>", "<SUB>", "<ESC>", "<FS>", "<GS>", "<RS>", "<US>",
73  " ", "!", "\"", "#", "$", "%", "&", "'", "(", ")", "*", "+", ",", "-", ".", "/",
74  "0", "1", "2", "3", "4", "5", "6", "7", "8", "9", ":", ";", "<", "=", ">", "?", "@",
75  "A", "B", "C", "D", "E", "F", "G", "H", "I", "J", "K", "L", "M", "N", "O", "P", "Q", "R", "S", "T", "U", "V", "W", "X", "Y", "Z", "[", "\\", "]", "^", "_", "`",
76  "a", "b", "c", "d", "e", "f", "g", "h", "i", "j", "k", "l", "m", "n", "o", "p", "q", "r", "s", "t", "u", "v", "w", "x", "y", "z", "{", "|", "}", "~", "<DEL>",
77  "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "",
78  "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "",
79  "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "",
80  "", "", "", "", "", "", "", ""
81  };
82 
86 const std::map<std::string, uint8_t> sick_scan::ColaAsciiBinaryConverter::s_ascii_map =
87  {
88  {"<SOH>", 0x01}, {"<STX>", 0x02}, {"<ETX>", 0x03}, {"<EOT>", 0x04}, {"<ENQ>", 0x05}, {"<ACK>", 0x06}, {"<BEL>", 0x07}, {"\b", 0x08}, {"<HT>", 0x09},
89  {"\n", 0x0A}, {"<VT>", 0x0B}, {"<FF>", 0x0C}, {"\n", 0x0D}, {"<SO>", 0x0E}, {"<SI>", 0x0F}, {"<DLE>", 0x10}, {"<DC1>", 0x11}, {"<DC2>", 0x12},
90  {"<DC3>", 0x13}, {"<DC4>", 0x14}, {"<NAK>", 0x15}, {"<SYN>", 0x16}, {"<ETB>", 0x17}, {"<CAN>", 0x18}, {"<EM>", 0x19}, {"<SUB>", 0x1A}, {"<ESC>", 0x1B},
91  {"<FS>", 0x1C}, {"<GS>", 0x1D}, {"<RS>", 0x1E}, {"<US>", 0x1F}, {" ", 0x20}, {"!", 0x21}, {"\"", 0x22}, {"#", 0x23}, {"$", 0x24}, {"%", 0x25}, {"&", 0x26},
92  {"'", 0x27}, {"(", 0x28}, {")", 0x29}, {"*", 0x2A}, {"+", 0x2B}, {",", 0x2C}, {"-", 0x2D}, {".", 0x2E}, {"/", 0x2F},
93  {"0", 0x30}, {"1", 0x31}, {"2", 0x32}, {"3", 0x33}, {"4", 0x34}, {"5", 0x35}, {"6", 0x36}, {"7", 0x37}, {"8", 0x38}, {"9", 0x39}, {":", 0x3A}, {";", 0x3B}, {"=", 0x3D}, {"?", 0x3F}, {"@", 0x40},
94  {"A", 0x41}, {"B", 0x42}, {"C", 0x43}, {"D", 0x44}, {"E", 0x45}, {"F", 0x46}, {"G", 0x47}, {"H", 0x48}, {"I", 0x49}, {"J", 0x4A}, {"K", 0x4B}, {"L", 0x4C}, {"M", 0x4D}, {"N", 0x4E}, {"O", 0x4F},
95  {"P", 0x50}, {"Q", 0x51}, {"R", 0x52}, {"S", 0x53}, {"T", 0x54}, {"U", 0x55}, {"V", 0x56}, {"W", 0x57}, {"X", 0x58}, {"Y", 0x59}, {"Z", 0x5A}, {"[", 0x5B}, {"\\", 0x5C}, {"]", 0x5D}, {"^", 0x5E}, {"_", 0x5F}, {"`", 0x60},
96  {"a", 0x61}, {"b", 0x62}, {"c", 0x63}, {"d", 0x64}, {"e", 0x65}, {"f", 0x66}, {"g", 0x67}, {"h", 0x68}, {"i", 0x69}, {"j", 0x6A}, {"k", 0x6B}, {"l", 0x6C}, {"m", 0x6D}, {"n", 0x6E}, {"o", 0x6F},
97  {"p", 0x70}, {"q", 0x71}, {"r", 0x72}, {"s", 0x73}, {"t", 0x74}, {"u", 0x75}, {"v", 0x76}, {"w", 0x77}, {"x", 0x78}, {"y", 0x79}, {"z", 0x7A}, {"{", 0x7B}, {"|", 0x7C}, {"}", 0x7D}, {"~", 0x7E}, {"<DEL>", 0x7F}
98  };
99 
105 std::string sick_scan::ColaAsciiBinaryConverter::ConvertColaAscii(const std::vector<uint8_t> & cola_telegram)
106 {
107  std::stringstream cola_ascii;
108  for(std::vector<uint8_t>::const_iterator iter = cola_telegram.cbegin(); iter != cola_telegram.cend(); iter++)
109  {
110  // Convert binary to ascii by lookup table
111  cola_ascii << s_ascii_table[((*iter) & 0xFF)];
112  }
113  return cola_ascii.str();
114 }
115 
122 std::vector<uint8_t> sick_scan::ColaAsciiBinaryConverter::ConvertColaAscii(const std::string & cola_telegram)
123 {
124  std::vector<uint8_t> cola_ascii;
125  cola_ascii.reserve(cola_telegram.size());
126  for(size_t char_cnt = 0; char_cnt < cola_telegram.size(); char_cnt++)
127  {
128  cola_ascii.push_back(0); // default, overwrite by value converted from ascii
129  // Convert ascii string to binary by lookup table
130  for(std::map<std::string, uint8_t>::const_iterator map_iter = s_ascii_map.cbegin(); map_iter != s_ascii_map.cend(); map_iter++)
131  {
132  if(strncmp(map_iter->first.c_str(), cola_telegram.c_str() + char_cnt, map_iter->first.size()) == 0)
133  {
134  cola_ascii.back() = map_iter->second;
135  char_cnt += (map_iter->first.size() - 1);
136  break;
137  }
138  }
139  }
140  return cola_ascii;
141 }
142 
143 static uint8_t CRC8XOR(uint8_t* msgBlock, size_t len)
144 {
145  uint8_t xorVal = 0x00;
146  for (size_t i = 0; i < len; i++)
147  xorVal ^= (msgBlock[i]);
148  return xorVal;
149 }
150 
162 std::vector<uint8_t> sick_scan::ColaAsciiBinaryConverter::ColaAsciiToColaBinary(const std::vector<uint8_t> & cola_telegram, int parameter_is_ascii)
163 {
164  // Split telegram in command_type (sRN,sRA,sMN,sMA,sWN), command_name ("SetAccessMode", "LocSetResultPoseEnabled",
165  // "LocRequestTimestamp", etc.) and command parameter
166  sick_scan::SickLocColaTelegramMsg cola_msg = sick_scan::ColaParser::decodeColaTelegram(cola_telegram);
167  if(cola_msg.command_type < 0 || cola_msg.command_type >= sick_scan::ColaParser::MAX_COLA_COMMAND_NUMBER)
168  {
169  ROS_ERROR_STREAM("## ERROR in ColaAsciiToColaBinary(): invalid SOPAS command type " << cola_msg.command_type);
170  return std::vector<uint8_t>();
171  }
172  return ColaTelegramToColaBinary(cola_msg, parameter_is_ascii);
173 }
174 
185 std::vector<uint8_t> sick_scan::ColaAsciiBinaryConverter::ColaTelegramToColaBinary(const sick_scan::SickLocColaTelegramMsg & cola_msg, int parameter_is_ascii)
186 {
187  std::vector<uint8_t> cola_binary;
188  cola_binary.reserve(64*1024);
189  const uint8_t binary_stx = 0x02;
190  const uint8_t binary_separator = 0x20;
191  std::string command_type = sick_scan::ColaParser::convertSopasCommand((sick_scan::ColaParser::COLA_SOPAS_COMMAND)cola_msg.command_type);
192 
193  // Encode the payload
194  std::vector<uint8_t> binary_payload;
195  binary_payload.reserve(64*1024);
196  for(size_t n = 0; n < command_type.size(); n++)
197  binary_payload.push_back(command_type[n]);
198  binary_payload.push_back(binary_separator);
199  for(size_t n = 0; n < cola_msg.command_name.size(); n++)
200  binary_payload.push_back(cola_msg.command_name[n]);
201  if(cola_msg.parameter.size() > 0)
202  {
203  binary_payload.push_back(binary_separator);
204  for(size_t n = 0; n < cola_msg.parameter.size(); n++)
205  {
206  std::string parameter = cola_msg.parameter[n];
207  if(parameter_is_ascii != 0) // parameter given in ascii, default: true
208  {
209  if ((parameter.size() % 2) != 0)
210  parameter = "0" + cola_msg.parameter[n];
211  for (size_t m = 1; m < parameter.size(); m += 2)
212  {
213  try
214  {
215  uint8_t val = static_cast<uint8_t>(std::stoul(parameter.substr(m - 1, 2), 0, 16) & 0xFF);
216  binary_payload.push_back(val);
217  }
218  catch (const std::exception & exc)
219  {
220  if(parameter_is_ascii < 0) // auto determination, 1. assumption is true, 2. assumption is false in case of errors
221  {
222  return ColaTelegramToColaBinary(cola_msg, 0);
223  }
224  else // parameter_is_ascii is true, but parameter aren't -> error
225  {
226  ROS_ERROR_STREAM("## ERROR in ColaTelegramToColaBinary(): can't convert parameter value " << parameter.substr(m - 1, 2) << " to hex, exception " << exc.what());
227  return cola_binary;
228  }
229  }
230  }
231  }
232  else // copy parameter (parameter is byte array, default: false)
233  {
234  if(n > 0)
235  binary_payload.push_back(binary_separator); // parameter is byte array -> payload including 0x20
236  for (size_t m = 0; m < parameter.size(); m++)
237  {
238  uint8_t val = static_cast<uint8_t>(parameter[m] & 0xFF);
239  binary_payload.push_back(val);
240  }
241  }
242  }
243  }
244 
245  // Concatenate Cola binary telegram: { 4 byte STX } + { 4 byte length } + { payload } + { checksum }
246  for(size_t n = 0; n < 4; n++)
247  cola_binary.push_back(binary_stx); // 4 byte STX
248  uint32_t payload_length = binary_payload.size();
249  for(int n = 24; n >= 0; n-=8)
250  cola_binary.push_back((payload_length >> n) & 0xFF); // 4 byte payload length, big endian
251  for(size_t n = 0; n < binary_payload.size(); n++)
252  cola_binary.push_back(binary_payload[n]); // payload
253  uint8_t checksum = CRC8XOR(binary_payload.data(), binary_payload.size());
254  cola_binary.push_back(checksum); // CRC8XOR checksum
255 
256  return cola_binary;
257 }
258 
267 std::vector<uint8_t> sick_scan::ColaAsciiBinaryConverter::ColaBinaryToColaAscii(const std::vector<uint8_t> & cola_telegram, bool parameter_to_ascii)
268 {
269  const uint8_t binary_separator = 0x20;
270  std::vector<uint8_t> cola_ascii;
271  // Decode header
272  if(!IsColaBinary(cola_telegram))
273  {
274  ROS_ERROR_STREAM("## ERROR in ColaBinaryToColaAscii(): cola telegram is not Cola-Binary");
275  return cola_ascii;
276  }
277  uint32_t telegram_length = ColaBinaryTelegramLength(cola_telegram);
278  if(telegram_length != cola_telegram.size())
279  {
280  ROS_ERROR_STREAM("## ERROR in ColaBinaryToColaAscii(): invalid cola telegram length (" << cola_telegram.size() << " byte, expected " << telegram_length << " byte)");
281  return cola_ascii;
282  }
283  // Extract payload
284  std::vector<uint8_t> binary_payload;
285  binary_payload.reserve(cola_telegram.size());
286  for(size_t n = 8; n < telegram_length - 1; n++)
287  binary_payload.push_back(cola_telegram[n]);
288  // Check crc checksum
289  uint8_t checksum = CRC8XOR(binary_payload.data(), binary_payload.size());
290  if(checksum != cola_telegram.back())
291  {
292  ROS_ERROR_STREAM("## ERROR in ColaBinaryToColaAscii(): invalid checksum (expected: " << checksum << ", received: " << cola_telegram.back() << ")");
293  return cola_ascii;
294  }
295  // Find start of command parameter
296  size_t parameter_start_idx = 0;
297  while(parameter_start_idx < binary_payload.size() && binary_payload[parameter_start_idx] != binary_separator)
298  parameter_start_idx++;
299  parameter_start_idx++;
300  while(parameter_start_idx < binary_payload.size() && binary_payload[parameter_start_idx] != binary_separator)
301  parameter_start_idx++;
302  parameter_start_idx++;
303  // Concatenate Cola-ASCII telegram
304  std::vector<uint8_t> stx = sick_scan::ColaParser::binarySTX();
305  std::vector<uint8_t> etx = sick_scan::ColaParser::binaryETX();
306  cola_ascii.reserve(2 * cola_telegram.size());
307  // Copy STX
308  for(size_t n = 0; n < stx.size(); n++)
309  cola_ascii.push_back(stx[n]);
310  if(parameter_to_ascii) // Convert command parameter to ASCII, default: true
311  {
312  // Copy command type and name
313  for (size_t n = 0; n < binary_payload.size() && n < parameter_start_idx; n++)
314  cola_ascii.push_back(binary_payload[n]);
315  // Convert command parameter to ASCII
316  for (size_t n = parameter_start_idx; n < binary_payload.size(); n++)
317  {
318  std::stringstream hexstring;
319  hexstring << std::hex << (uint32_t) binary_payload[n];
320  for (size_t m = 0; m < hexstring.str().size(); m++)
321  cola_ascii.push_back(hexstring.str()[m]);
322  }
323  }
324  else
325  {
326  // Copy command type, name and parameter
327  for (size_t n = 0; n < binary_payload.size(); n++)
328  cola_ascii.push_back(binary_payload[n]);
329  }
330  // Copy ETX
331  for(size_t n = 0; n < etx.size(); n++)
332  cola_ascii.push_back(etx[n]);
333  return cola_ascii;
334 }
335 
342 bool sick_scan::ColaAsciiBinaryConverter::IsColaBinary(const std::vector<uint8_t> & cola_telegram)
343 {
344  const uint8_t binary_stx = 0x02;
345  return cola_telegram.size() >= 4 && cola_telegram[0] == binary_stx && cola_telegram[1] == binary_stx
346  && cola_telegram[2] == binary_stx && cola_telegram[3] == binary_stx;
347 }
348 
354 uint32_t sick_scan::ColaAsciiBinaryConverter::ColaBinaryTelegramLength(const std::vector<uint8_t> & cola_telegram)
355 {
356  if(cola_telegram.size() >= 8 && IsColaBinary(cola_telegram))
357  {
358  uint32_t payload_length = 0;
359  for(size_t n = 4; n < 8; n++)
360  {
361  payload_length = payload_length << 8;
362  payload_length |= ((cola_telegram[n]) & 0xFF);
363  }
364  return payload_length + 9; // 8 byte header + payload_length + 1 byte checksum
365  }
366  return 0;
367 }
sick_scan::ColaAsciiBinaryConverter::ColaBinaryTelegramLength
static uint32_t ColaBinaryTelegramLength(const std::vector< uint8_t > &cola_telegram)
Decodes the header and returns the length of a Cola-Binary telegram.
Definition: cola_converter.cpp:354
sick_scan::ColaParser::binaryETX
static std::vector< uint8_t > binaryETX(void)
Returns the binary "end of text" tag in a Cola-Binary command, i.e. {0x03}.
Definition: cola_parser.h:176
pcap_json_converter.cola_ascii
bool cola_ascii
Definition: pcap_json_converter.py:132
ROS_ERROR_STREAM
#define ROS_ERROR_STREAM(args)
sick_scan::ColaParser::COLA_SOPAS_COMMAND
enum sick_scan::ColaParser::COLA_SOPAS_COMMAND_ENUM COLA_SOPAS_COMMAND
Enumeration of SOPAS commands in cola telegrams: The command_type in SickLocColaTelegramMsg is one of...
sick_scan::ColaAsciiBinaryConverter::IsColaBinary
static bool IsColaBinary(const std::vector< uint8_t > &cola_telegram)
Returns true for Cola-Binary, if a given telegram is Cola-Binary encoded and starts with 4 Bytes { 0x...
Definition: cola_converter.cpp:342
cola_converter.h
sick_scan::ColaAsciiBinaryConverter::s_ascii_map
static const std::map< std::string, uint8_t > s_ascii_map
static ascii map to convert ascii to binary, f.e. s_ascii_map["<STX>"]:=0x02, s_ascii_map["<ETX>"]:=0...
Definition: cola_converter.h:155
sick_scan::ColaAsciiBinaryConverter::ColaAsciiToColaBinary
static std::vector< uint8_t > ColaAsciiToColaBinary(const std::vector< uint8_t > &cola_telegram, int parameter_is_ascii=-1)
Converts and returns a Cola telegram from Cola-ASCII to Cola-Binary.
Definition: cola_converter.cpp:162
sick_scan::ColaAsciiBinaryConverter::ColaBinaryToColaAscii
static std::vector< uint8_t > ColaBinaryToColaAscii(const std::vector< uint8_t > &cola_telegram, bool parameter_to_ascii=true)
Converts and returns a Cola telegram from Cola-ASCII to Cola-Binary.
Definition: cola_converter.cpp:267
sick_scan::ColaParser::MAX_COLA_COMMAND_NUMBER
@ MAX_COLA_COMMAND_NUMBER
Number of possible COLA_SOPAS_COMMANDs incl. invalid command.
Definition: cola_parser.h:106
sick_scan::ColaParser::binarySTX
static std::vector< uint8_t > binarySTX(void)
Returns the binary "start of text" tag in a Cola-Binary command, i.e. {0x02}.
Definition: cola_parser.h:170
sick_scan::ColaAsciiBinaryConverter::s_ascii_table
static const std::string s_ascii_table[256]
static ascii table to convert binary to ascii, f.e. s_ascii_table[0x02]:="<STX>", s_ascii_table[0x03]...
Definition: cola_converter.h:154
CRC8XOR
static uint8_t CRC8XOR(uint8_t *msgBlock, size_t len)
Definition: cola_converter.cpp:143
sick_scan::ColaParser::convertSopasCommand
static std::string convertSopasCommand(COLA_SOPAS_COMMAND command_type)
Converts and returns a COLA_SOPAS_COMMAND to string. Example: convertSopasCommand(sMN) returns "sMN".
Definition: cola_parser.cpp:220
sick_scan::ColaParser::decodeColaTelegram
static sick_scan::SickLocColaTelegramMsg decodeColaTelegram(const std::vector< uint8_t > &cola_binary)
Decodes and returns a Cola message of type SickLocColaTelegramMsg from a Cola-Binary telegram....
Definition: cola_parser.cpp:119
ros_wrapper.h
sick_scan::ColaAsciiBinaryConverter::ConvertColaAscii
static std::string ConvertColaAscii(const std::vector< uint8_t > &cola_telegram)
Converts and returns a Cola-ASCII telegram to string.
Definition: cola_converter.cpp:105
cola_parser.h
sick_scan::ColaAsciiBinaryConverter::ColaTelegramToColaBinary
static std::vector< uint8_t > ColaTelegramToColaBinary(const sick_scan::SickLocColaTelegramMsg &cola_telegram, int parameter_is_ascii=-1)
Converts and returns a Cola telegram to Cola-Binary.
Definition: cola_converter.cpp:185

sick_scan
Author(s): Michael Lehning , Jochen Sprickerhof , Martin Günther
autogenerated on Thu Sep 8 2022 02:30:19