cola_parser.cpp
Go to the documentation of this file.
1 /*
2  * @brief sim_loc_cola_parser parses and converts binary Cola telegrams to ros messages SickLocColaTelegramMsg
3  * and vice versa.
4  *
5  * See Operation-Instruction-v1.1.0.241R.pdf, chapter 5.8 "About CoLa-A telegrams", page 46-48,
6  * Telegram-Listing-v1.1.0.241R.pdf, chapter 2.3.9 "Command: LocRequestTimestamp", page 21, and
7  * Technical_information_Telegram_Listing_NAV_LOC_en_IM0076556.PDF for further details about
8  * Cola telegrams.
9  *
10  * Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim
11  * Copyright (C) 2019 SICK AG, Waldkirch
12  *
13  * Licensed under the Apache License, Version 2.0 (the "License");
14  * you may not use this file except in compliance with the License.
15  * You may obtain a copy of the License at
16  *
17  * http://www.apache.org/licenses/LICENSE-2.0
18  *
19  * Unless required by applicable law or agreed to in writing, software
20  * distributed under the License is distributed on an "AS IS" BASIS,
21  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
22  * See the License for the specific language governing permissions and
23  * limitations under the License.
24  *
25  * All rights reserved.
26  *
27  * Redistribution and use in source and binary forms, with or without
28  * modification, are permitted provided that the following conditions are met:
29  *
30  * * Redistributions of source code must retain the above copyright
31  * notice, this list of conditions and the following disclaimer.
32  * * Redistributions in binary form must reproduce the above copyright
33  * notice, this list of conditions and the following disclaimer in the
34  * documentation and/or other materials provided with the distribution.
35  * * Neither the name of SICK AG nor the names of its
36  * contributors may be used to endorse or promote products derived from
37  * this software without specific prior written permission
38  * * Neither the name of Ing.-Buero Dr. Michael Lehning nor the names of its
39  * contributors may be used to endorse or promote products derived from
40  * this software without specific prior written permission
41  *
42  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
43  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
44  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
45  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
46  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
47  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
48  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
49  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
50  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
51  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
52  * POSSIBILITY OF SUCH DAMAGE.
53  *
54  * Authors:
55  * Michael Lehning <michael.lehning@lehning.de>
56  *
57  * Copyright 2019 SICK AG
58  * Copyright 2019 Ing.-Buero Dr. Michael Lehning
59  *
60  */
61 #include "sick_scan/ros_wrapper.h"
62 #include <cassert>
63 #include <boost/algorithm/string.hpp>
64 
65 #include "sick_scan/cola_parser.h"
66 
70 const std::string sick_scan::ColaParser::s_command_type_string[MAX_COLA_COMMAND_NUMBER] =
71  {
72  "sINVALID", "sRN", "sRA", "sMN", "sAN", "sMA", "sWN", "sWA", "sEN", "sEA", "sSN", "sFA"
73  };
74 
78 const std::map<std::string, sick_scan::ColaParser::COLA_SOPAS_COMMAND> sick_scan::ColaParser::s_command_type_map =
79  {
80  {"", sINVALID}, {"sINVALID", sINVALID}, {"sRN", sRN}, {"sRA", sRA}, {"sMN", sMN}, {"sAN", sAN}, {"sMA", sMA}, {"sWN", sWN}, {"sWA", sWA}, {"sEN", sEN}, {"sEA", sEA}, {"sSN", sSN}, {"sFA", sFA}
81  };
82 
86 const std::string sick_scan::ColaParser::s_cola_ascii_start_tag = "<STX>";
87 
91 const std::string sick_scan::ColaParser::s_cola_ascii_end_tag = "<ETX>";
92 
100 sick_scan::SickLocColaTelegramMsg sick_scan::ColaParser::createColaTelegram(const COLA_SOPAS_COMMAND & command_type,
101  const std::string & command_name, const std::vector<std::string> & parameter)
102 {
103  sick_scan::SickLocColaTelegramMsg cola_telegram;
104  cola_telegram.header.stamp = ROS::now();
105  cola_telegram.command_type = command_type;
106  cola_telegram.command_name = command_name;
107  cola_telegram.parameter = parameter;
108  return cola_telegram;
109 }
110 
119 sick_scan::SickLocColaTelegramMsg sick_scan::ColaParser::decodeColaTelegram(const std::vector<uint8_t> & cola_binary)
120 {
122  return decodeColaTelegram(cola_ascii);
123 }
124 
130 sick_scan::SickLocColaTelegramMsg sick_scan::ColaParser::decodeColaTelegram(const std::string & cola_ascii)
131 {
132  // Check and remove start and end tags ("<STX>" and "<ETX>")
133  std::string cola_ascii_cmd;
134  if (cola_ascii.size() > s_cola_ascii_start_tag.size() + s_cola_ascii_end_tag.size()
135  && cola_ascii.substr(0, s_cola_ascii_start_tag.size()) == s_cola_ascii_start_tag
136  && cola_ascii.substr(cola_ascii.size() - s_cola_ascii_end_tag.size()) == s_cola_ascii_end_tag)
137  {
138  cola_ascii_cmd = cola_ascii.substr(s_cola_ascii_start_tag.size(), cola_ascii.size() - s_cola_ascii_start_tag.size() - s_cola_ascii_end_tag.size());
139  }
140  else
141  {
142  cola_ascii_cmd = cola_ascii;
143  }
144  // Split in command_type, command_name and optional parameter by spaces
145  std::vector<std::string> cola_parts;
146  boost::split(cola_parts, cola_ascii_cmd, boost::algorithm::is_space());
147  if(cola_parts.size() < 2) // at least command_type and command_name required
148  {
149  ROS_WARN_STREAM("## ERROR Parse error in ColaParser::decodeColaTelegram(\"" << cola_ascii_cmd << "\"): to few arguments, at least command_type and command_name required");
150  return createColaTelegram(sINVALID, "");
151  }
152  // Convert command_type from string to COLA_SOPAS_COMMAND
153  sick_scan::ColaParser::COLA_SOPAS_COMMAND command_type = convertSopasCommand(cola_parts[0]);
154  if(command_type == sINVALID)
155  {
156  ROS_WARN_STREAM("## ERROR Parse error in ColaParser::decodeColaTelegram(\"" << cola_ascii_cmd << "\"): invalid command_type \"" << cola_parts[0] << "\"");
157  return createColaTelegram(sINVALID, "");
158  }
159  // Check command_name
160  if(cola_parts[1].empty())
161  {
162  ROS_WARN_STREAM("## ERROR Parse error in ColaParser::decodeColaTelegram(\"" << cola_ascii_cmd << "\"): invalid command_name \"" << cola_parts[1] << "\"");
163  return createColaTelegram(sINVALID, "");
164  }
165  // Append command parameter
166  sick_scan::SickLocColaTelegramMsg cola_telegram = createColaTelegram(command_type, cola_parts[1]);
167  if(cola_parts.size() > 2)
168  {
169  cola_telegram.parameter.reserve(cola_parts.size() - 2);
170  for(size_t n = 2; n < cola_parts.size(); n++)
171  cola_telegram.parameter.push_back(cola_parts[n]);
172  }
173  return cola_telegram;
174 }
175 
182 std::vector<uint8_t> sick_scan::ColaParser::encodeColaTelegram(const sick_scan::SickLocColaTelegramMsg & cola_telegram, bool parameter_is_ascii)
183 {
184  assert(cola_telegram.command_type > sINVALID && cola_telegram.command_type < MAX_COLA_COMMAND_NUMBER);
185  std::string cola_ascii;
186  cola_ascii.reserve(64*1024);
187  cola_ascii += s_cola_ascii_start_tag;
188  cola_ascii += convertSopasCommand((COLA_SOPAS_COMMAND)cola_telegram.command_type);
189  cola_ascii += " ";
190  cola_ascii += cola_telegram.command_name;
191  for(size_t n = 0; n < cola_telegram.parameter.size(); n++)
192  {
193  if( n == 0 || parameter_is_ascii)
194  cola_ascii += " ";
195  cola_ascii += cola_telegram.parameter[n];
196  }
197  cola_ascii += s_cola_ascii_end_tag;
199 }
200 
208 std::vector<uint8_t> sick_scan::ColaParser::encodeColaTelegram(const COLA_SOPAS_COMMAND & command_type, const std::string & command_name,
209  const std::vector<std::string> & parameter, bool parameter_is_ascii)
210 {
211  return encodeColaTelegram(createColaTelegram(command_type, command_name, parameter), parameter_is_ascii);
212 }
213 
221 {
222  return s_command_type_string[command_type];
223 }
224 
232 {
233  std::map<std::string, sick_scan::ColaParser::COLA_SOPAS_COMMAND>::const_iterator iter_command_type = s_command_type_map.find(sopas_command);
234  sick_scan::ColaParser::COLA_SOPAS_COMMAND command_type = (iter_command_type != s_command_type_map.cend()) ? (iter_command_type->second) : sINVALID;
235  return command_type;
236 }
237 
245 int32_t sick_scan::ColaParser::convertColaArg(const std::string & cola_arg, int base, int32_t default_value)
246 {
247  try
248  {
249  if(base < 0)
250  base = ((cola_arg.find_first_of("+-") != std::string::npos) ? 10 : 16); // base 10 if +/-sign in cola_arg, otherwise hex
251  return std::stoi(cola_arg, 0, base);
252  }
253  catch(const std::exception & exc)
254  {
255  ROS_WARN_STREAM("## ERROR ColaParser::convertColaArg(" << cola_arg << ") failed, exception " << exc.what());
256  }
257  return default_value;
258 }
259 
267 uint32_t sick_scan::ColaParser::convertColaArg(const std::string & cola_arg, int base, uint32_t default_value)
268 {
269  try
270  {
271  if(base < 0)
272  base = ((cola_arg.find_first_of("+-") != std::string::npos) ? 10 : 16); // base 10 if +/-sign in cola_arg, otherwise hex
273  return (uint32_t)std::stoul(cola_arg, 0, base);
274  }
275  catch(const std::exception & exc)
276  {
277  ROS_WARN_STREAM("## ERROR ColaParser::convertColaArg(" << cola_arg << ") failed, exception " << exc.what());
278  }
279  return default_value;
280 }
281 
288 bool sick_scan::ColaParser::convertColaResponseBool(const std::string & cola_response_arg, bool default_value)
289 {
290  return ((sick_scan::ColaParser::convertColaArg(cola_response_arg, 10, (default_value ? 1 : 0)) > 0) ? true : false);
291 }
sick_scan::ColaParser::convertColaResponseBool
static bool convertColaResponseBool(const std::string &cola_response_arg, bool default_value)
Definition: cola_parser.cpp:288
pcap_json_converter.cola_ascii
bool cola_ascii
Definition: pcap_json_converter.py:132
sick_scan::ColaParser::convertColaArg
static int32_t convertColaArg(const std::string &cola_arg, int base=10, int32_t default_value=0)
Definition: cola_parser.cpp:245
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::ColaParser::encodeColaTelegram
static std::vector< uint8_t > encodeColaTelegram(const sick_scan::SickLocColaTelegramMsg &cola_telegram, bool parameter_is_ascii=true)
Encodes and returns a Cola Binary telegram from ros message SickLocColaTelegramMsg.
Definition: cola_parser.cpp:182
sick_scan::ColaParser::s_command_type_map
static const std::map< std::string, COLA_SOPAS_COMMAND > s_command_type_map
static map to convert COLA_SOPAS_COMMANDs from string to enum, f.e. s_command_type_map["sRN"]:=sRN,...
Definition: cola_parser.h:223
sick_scan::ColaParser::s_cola_ascii_end_tag
static const std::string s_cola_ascii_end_tag
All Cola-ACSII telegrams start with s_cola_ascii_start_tag := "<ETX>" ("End of TeXt")
Definition: cola_parser.h:225
ROS::now
ROS::Time now(void)
Definition: ros_wrapper.cpp:116
ROS_WARN_STREAM
#define ROS_WARN_STREAM(args)
sick_scan::ColaParser::createColaTelegram
static sick_scan::SickLocColaTelegramMsg createColaTelegram(const COLA_SOPAS_COMMAND &command_type, const std::string &command_name, const std::vector< std::string > &parameter=std::vector< std::string >())
Creates and returns a Cola telegram (type SickLocColaTelegramMsg).
Definition: cola_parser.cpp:100
sick_scan::ColaParser::s_command_type_string
static const std::string s_command_type_string[MAX_COLA_COMMAND_NUMBER]
static table to convert COLA_SOPAS_COMMAND to string, f.e. s_command_type_string[sRN]:="sRN",...
Definition: cola_parser.h:222
default_value
def default_value(type_)
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::ColaParser::s_cola_ascii_start_tag
static const std::string s_cola_ascii_start_tag
All Cola-ACSII telegrams start with s_cola_ascii_start_tag := "<STX>" ("Start of TeXt")
Definition: cola_parser.h:224


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