testcase_generator.cpp
Go to the documentation of this file.
1 /*
2  * @brief sim_loc_testcase_generator generates testcases for SIM Localization driver.
3  * The generator creates deterministic and random based result port telegrams.
4  *
5  * Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim
6  * Copyright (C) 2019 SICK AG, Waldkirch
7  *
8  * Licensed under the Apache License, Version 2.0 (the "License");
9  * you may not use this file except in compliance with the License.
10  * You may obtain a copy of the License at
11  *
12  * http://www.apache.org/licenses/LICENSE-2.0
13  *
14  * Unless required by applicable law or agreed to in writing, software
15  * distributed under the License is distributed on an "AS IS" BASIS,
16  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
17  * See the License for the specific language governing permissions and
18  * limitations under the License.
19  *
20  * All rights reserved.
21  *
22  * Redistribution and use in source and binary forms, with or without
23  * modification, are permitted provided that the following conditions are met:
24  *
25  * * Redistributions of source code must retain the above copyright
26  * notice, this list of conditions and the following disclaimer.
27  * * Redistributions in binary form must reproduce the above copyright
28  * notice, this list of conditions and the following disclaimer in the
29  * documentation and/or other materials provided with the distribution.
30  * * Neither the name of SICK AG nor the names of its
31  * contributors may be used to endorse or promote products derived from
32  * this software without specific prior written permission
33  * * Neither the name of Ing.-Buero Dr. Michael Lehning 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  *
37  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
38  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
39  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
40  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
41  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
42  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
43  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
44  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
45  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
46  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
47  * POSSIBILITY OF SUCH DAMAGE.
48  *
49  * Authors:
50  * Michael Lehning <michael.lehning@lehning.de>
51  *
52  * Copyright 2019 SICK AG
53  * Copyright 2019 Ing.-Buero Dr. Michael Lehning
54  *
55  */
56 #include "sick_scan/ros_wrapper.h"
57 
58 #include "sick_scan/cola_parser.h"
59 #include "sick_scan/random_generator.h"
60 #include "sick_scan/testcase_generator.h"
61 #include "sick_scan/utils.h"
62 
63 
67 uint32_t sick_scan::TestcaseGenerator::s_u32ResultPoseInterval = 1;
68 
72 std::map<std::string, int32_t> sick_scan::TestcaseGenerator::s_controller_settings = {
73  {"IsSystemReady", 1}, // 0:false, 1:true (default)
74  {"LMCstartmeas", 0}, // 0:false (do not send LMDscandata or LMDscandatamon), 1:true (send LMDscandata and LMDscandatamon)
75  {"LocState", 2}, // controller state: 0:BOOTING, 1:IDLE, 2:LOCALIZING, 3:DEMO_MAPPING
76  {"LocResultPort", 2201}, // tcp port for result telegrams (default: 2201)
77  {"LocResultMode", 0}, // 0:stream (default), 1:poll
78  {"LocResultState", 1}, // result output: 0: disabled, 1: enabled
79  {"LocResultEndianness", 0}, // 0: big endian (default), 1: little endian
80  {"LocMapState", 1}, // map state: 0:not active, 1:active
81  {"LocRequestResultData", 1} // in poll mode, trigger sending the localization result of the next processed scan via TCP interface.
82 };
83 
87 std::map<std::string, std::string> sick_scan::TestcaseGenerator::s_controller_settings_str;
88 
94 bool sick_scan::TestcaseGenerator::SendScandataEnabled(void)
95 {
96  return s_controller_settings["LMCstartmeas"] > 0; // 0:false (do not send LMDscandata or LMDscandatamon), 1:true (send LMDscandata and LMDscandatamon)
97 }
98 
103 bool sick_scan::TestcaseGenerator::LocalizationEnabled(void)
104 {
105  return s_controller_settings["LocState"] == 2; // localization on
106 }
107 
112 bool sick_scan::TestcaseGenerator::ResultTelegramsEnabled(void)
113 {
114  return LocalizationEnabled() && s_controller_settings["LocResultState"] > 0; // localization on and result telegrams activated, otherwise result telegrams deactivated
115 }
116 
121 sick_scan::SickLocResultPortTestcaseMsg sick_scan::TestcaseGenerator::createDefaultResultPortTestcase(void)
122 {
123  sick_scan::SickLocResultPortTestcaseMsg testcase;
124 
125  // ROS Header with sequence id, timestamp and frame id
126  testcase.header.stamp = ROS::now();
127  testcase.header.frame_id = "sick_localization_testcase";
128 
129  // binary encoded result port telegram (default example)
130  testcase.binary_data = {
131  0x53, 0x49, 0x43, 0x4B, 0x00, 0x00, 0x00, 0x6A, 0x06, 0x42, 0x00, 0x01, 0x00, 0x10, 0xC0, 0x58, 0x01, 0x22, 0xA2, 0x72,
132  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x4C, 0x4C, 0x53, 0x20, 0x56, 0x30, 0x2E, 0x31, 0x2E, 0x39, 0x2E, 0x78, 0x42,
133  0x00, 0x00, 0x02, 0x6D, 0x83, 0xAA, 0x8C, 0x0C, 0x8E, 0x14, 0x78, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x6F, 0x00, 0x34,
134  0xEC, 0xF3, 0x00, 0x00, 0x00, 0x5D, 0x00, 0x00, 0x00, 0x21, 0x00, 0x00, 0x45, 0xE7, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
135  0x00, 0x00, 0x37, 0x00, 0x00, 0x00, 0x80, 0x89, 0x00, 0x00, 0x99, 0x93, 0x00, 0x12, 0x78, 0x9F, 0x00, 0x00, 0x00, 0x00,
136  0x00, 0x00, 0x00, 0x00, 0x62, 0x11 };
137 
138  // decoded result port telegram
139  sick_scan::ResultPortParser result_port_parser(testcase.header.frame_id);
140  std::vector<uint8_t> recoded_telegram;
141  if(!result_port_parser.decode(testcase.binary_data) || (recoded_telegram = result_port_parser.encode()) != testcase.binary_data)
142  {
143  ROS_ERROR_STREAM("## ERROR TestcaseGenerator::createDefaultResultPortTestcase: sick_scan::ResultPortParser::decode() failed. " << testcase.binary_data.size() << " byte input (hex):");
144  ROS_ERROR_STREAM(sick_scan::Utils::toHexString(testcase.binary_data));
145  ROS_ERROR_STREAM("## output (decoded): " << sick_scan::Utils::flattenToString(result_port_parser.getTelegramMsg()));
146  ROS_ERROR_STREAM("## recoded:");
147  ROS_ERROR_STREAM(sick_scan::Utils::toHexString(recoded_telegram));
148  }
149  testcase.telegram_msg = result_port_parser.getTelegramMsg();
150 
151  return testcase;
152 }
153 
158 sick_scan::SickLocResultPortTestcaseMsg sick_scan::TestcaseGenerator::createRandomResultPortTestcase(void)
159 {
160  // Random number generators
161  static sick_scan::UniformRandomInteger random1_generator(0, 1);
162  static sick_scan::UniformRandomInteger random8_generator(0, 255);
163  static sick_scan::UniformRandomInteger random32_generator(-INT32_MAX, INT32_MAX);
164  static sick_scan::UniformRandomInteger random_yaw_generator(-180000, 180000);
165  static sick_scan::UniformRandomInteger random_quality_generator(0, 100);
166  static sick_scan::UniformRandomInteger random_covariance_generator(0, INT32_MAX);
167 
168  // Create default SickLocResultPortTelegramMsg
169  static ROS::Time start_time = ROS::now();
170  static sick_scan::SickLocResultPortTestcaseMsg default_testcase = createDefaultResultPortTestcase();
171  sick_scan::SickLocResultPortTestcaseMsg testcase = default_testcase;
172  sick_scan::SickLocResultPortTelegramMsg & telegram_msg = testcase.telegram_msg;
173 
174  // Modify SickLocResultPortTelegramMsg with random values
175  telegram_msg.telegram_header.payloadtype = ((random1_generator.generate() > 0) ? 0x06c2 : 0x0642); // Payload type: 0x06c2 = Little Endian, 0x0642 = Big Endian. Size: UInt16 = 2 byte
176  telegram_msg.telegram_header.ordernumber = (uint32_t)random32_generator.generate(); // Order number of the localization controller. Size: UInt32 = 4 byte
177  telegram_msg.telegram_header.serialnumber = (uint32_t)random32_generator.generate(); // Serial number of the localization controller. Size: UInt32 = 4 byte
178  for(size_t n = 0; n < telegram_msg.telegram_header.fw_version.size(); n++)
179  telegram_msg.telegram_header.fw_version[n] = (uint8_t)random8_generator.generate(); // Software version of the localization controller. Size: 20 × UInt8 = 20 byte
180  telegram_msg.telegram_payload.posex = random32_generator.generate(); // Position X of the vehicle on the map in cartesian global coordinates [mm]. Size: Int32 = 4 byte
181  telegram_msg.telegram_payload.posey = random32_generator.generate(); // Position Y of the vehicle on the map in cartesian global coordinates [mm]. Size: Int32 = 4 byte
182  telegram_msg.telegram_payload.poseyaw = random_yaw_generator.generate(); // Orientation (yaw) of the vehicle on the map [mdeg], range -180 to +180 deg assumed. Size: Int32 = 4 byte
183  telegram_msg.telegram_payload.reserved1 = (uint32_t)random32_generator.generate(); // Reserved. Size: UInt32 = 4 byte
184  telegram_msg.telegram_payload.reserved2 = random32_generator.generate(); // Reserved. Size: Int32 = 4 byte
185  telegram_msg.telegram_payload.quality = (uint8_t)random_quality_generator.generate(); // Quality of pose [0 … 100], 1 = bad pose quality, 100 = good pose quality. Size: UInt8 = 1 byte
186  telegram_msg.telegram_payload.outliersratio = (uint8_t)random_quality_generator.generate(); // Ratio of beams that cannot be assigned to the current reference map [%]. Size: UInt8 = 1 byte
187  telegram_msg.telegram_payload.covariancex = random_covariance_generator.generate(); // Covariance c1 of the pose X [mm^2]. Size: Int32 = 4 byte
188  telegram_msg.telegram_payload.covariancey = random_covariance_generator.generate(); // Covariance c5 of the pose Y [mm^2]. Size: Int32 = 4 byte
189  telegram_msg.telegram_payload.covarianceyaw = random_covariance_generator.generate(); // Covariance c9 of the pose Yaw [mdeg^2]. Size: Int32 = 4 byte
190  telegram_msg.telegram_payload.reserved3 = (((uint64_t)random32_generator.generate() << 32) | (uint64_t)random32_generator.generate()); // Reserved. Size: UInt64 = 8 byte
191 
192  // Update telegram timestamps
193  double delta_time_seconds = ROS::seconds(ROS::now() - start_time);
194  telegram_msg.telegram_payload.timestamp = createTimestampTicksMilliSec(); // Time stamp of the pose [ms]. The time stamp indicates the time at which the pose is calculated. Size: UInt32 = 4 byte
195  telegram_msg.telegram_header.systemtime += (uint64_t)(delta_time_seconds); // SystemTime not used. Size: NTP = 8 byte
196 
197  // Re-encode the modified result port telegram (SickLocResultPortTelegramMsg)
198  sick_scan::ResultPortParser result_port_parser(testcase.header.frame_id);
199  result_port_parser.getTelegramMsg() = telegram_msg;
200  testcase.binary_data = result_port_parser.encode();
201  testcase.telegram_msg = result_port_parser.getTelegramMsg();
202 
203  // Increment telegram counter for next testcase
204  default_testcase.telegram_msg.telegram_header.telegramcounter += 1; // Telegram counter since last start-up. Size: UInt32 = 4 byte
205  default_testcase.telegram_msg.telegram_payload.scancounter += 1; // Counter of related scan data. Size: UInt32 = 4 byte
206 
207  // Update testcase timestamp
208  testcase.header.stamp = ROS::now();
209  return testcase;
210 }
211 
220 sick_scan::SickLocResultPortTestcaseMsg sick_scan::TestcaseGenerator::createResultPortCircles(double circle_radius, double circle_yaw)
221 {
222  // Create default SickLocResultPortTelegramMsg
223  static ROS::Time start_time = ROS::now();
224  static sick_scan::SickLocResultPortTestcaseMsg default_testcase = createDefaultResultPortTestcase();
225  sick_scan::SickLocResultPortTestcaseMsg testcase = default_testcase;
226  sick_scan::SickLocResultPortTelegramMsg & telegram_msg = testcase.telegram_msg;
227 
228  // Set current position and orientation
229  telegram_msg.telegram_payload.posex = (int32_t)(1000.0 * circle_radius * std::cos(circle_yaw)); // Position X of the vehicle on the map in cartesian global coordinates [mm]. Size: Int32 = 4 byte
230  telegram_msg.telegram_payload.posey = (int32_t)(1000.0 * circle_radius * std::sin(circle_yaw)); // Position Y of the vehicle on the map in cartesian global coordinates [mm]. Size: Int32 = 4 byte
231  double orientation = sick_scan::Utils::normalizeAngle(circle_yaw + M_PI_2); // Orienation := circle_yaw + 90 degree
232  telegram_msg.telegram_payload.poseyaw = (int32_t)(1000.0 * orientation * 180.0 / M_PI); // Orientation (yaw) of the vehicle on the map [mdeg], range -180 to +180 deg assumed. Size: Int32 = 4 byte
233 
234  // Update telegram timestamps
235  double delta_time_seconds = ROS::seconds(ROS::now() - start_time);
236  telegram_msg.telegram_payload.timestamp = createTimestampTicksMilliSec(); // Time stamp of the pose [ms]. The time stamp indicates the time at which the pose is calculated. Size: UInt32 = 4 byte
237  telegram_msg.telegram_header.systemtime += (uint64_t)(delta_time_seconds); // SystemTime not used. Size: NTP = 8 byte
238 
239  // Re-encode the modified result port telegram (SickLocResultPortTelegramMsg)
240  sick_scan::ResultPortParser result_port_parser(testcase.header.frame_id);
241  result_port_parser.getTelegramMsg() = telegram_msg;
242  testcase.binary_data = result_port_parser.encode();
243  testcase.telegram_msg = result_port_parser.getTelegramMsg();
244 
245  // Increment telegram counter for next testcase
246  default_testcase.telegram_msg.telegram_header.telegramcounter += 1; // Telegram counter since last start-up. Size: UInt32 = 4 byte
247  default_testcase.telegram_msg.telegram_payload.scancounter += 1; // Counter of related scan data. Size: UInt32 = 4 byte
248 
249  // Update testcase timestamp
250  testcase.header.stamp = ROS::now();
251  return testcase;
252 }
253 
261 sick_scan::SickLocColaTelegramMsg sick_scan::TestcaseGenerator::createColaResponse(const sick_scan::SickLocColaTelegramMsg & cola_request, const std::string& scanner_type)
262 {
263  static std::map<sick_scan::ColaParser::COLA_SOPAS_COMMAND, std::map<std::string, sick_scan::SickLocColaTelegramMsg>> s_mapped_responses; // static responses for requests
264  static bool s_mapped_responses_initialized = false;
265  if(!s_mapped_responses_initialized)
266  {
267  s_mapped_responses[sick_scan::ColaParser::sEN] = { // static responses for sEN requests
268  {"ECRChangeArr", sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sEA"), "ECRChangeArr", {"01"})},
269  {"LFErec", sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sEA"), "LFErec", {"01"})},
270  {"LIDinputstate", sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sEA"), "LIDinputstate", {"01"})},
271  {"LIDoutputstate", sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sEA"), "LIDoutputstate", {"01"})},
272  {"LMDscandata", sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sEA"), "LMDscandata", {"01"})}
273  };
274  s_mapped_responses[sick_scan::ColaParser::sMN] = { // static responses for sMN requests
275  {"Run", sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sAN"), "Run", {"01"})},
276  {"SetAccessMode", sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sAN"), "SetAccessMode", {"01"})}
277  };
278  s_mapped_responses[sick_scan::ColaParser::sRN] = { // static responses for sRN requests
279  // todo: read from configured json-file! The following "sRA fieldxxx" responses are hardcoded from file 20210113_tim871s_elephant.pcapng
280  {"field000", sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sRA"), "field000", {"3f8000000000000000000d05fff92230020100010000000000000000000100084669656c643030310000"})},
281  {"field001", sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sRA"), "field001", {"3f8000000000000000000d05fff9223002020001000200f6ffff00fa0195ffff012c000000000000000100084669656c643030320000"})},
282  {"field002", sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sRA"), "field002", {"3f8000000000000000000d05fff922300203000100030195ffff012c026bffff009d02a3ffff00a0000000000000000100084669656c643030330000"})},
283  {"field003", sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sRA"), "field003", {"3f8000000000000000000d05fff92230020400010006010affff04be0115ffff047e0120ffff04410135ffff03e90154ffff03ad0178ffff0377000000000000000100084669656c643030340000"})},
284  {"field004", sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sRA"), "field004", {"3f8000000000000000000d05fff92230020500010003017cffff037b018effff037901b0ffff0383000000000000000100084669656c643030350000"})},
285  {"field005", sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sRA"), "field005", {"3f8000000000000000000d05fff9223002060001000401baffff039201c1ffff03a101dfffff03fd0211ffff04fd000000000000000100084669656c643030360000"})},
286  {"field006", sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sRA"), "field006", {"3f8000000000000000000d05fff92230020700010000000000000000000100084669656c643030370000"})},
287  {"field007", sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sRA"), "field007", {"3f8000000000000000000d05fff92230020800010000000000000000000100084669656c643030380000"})},
288  {"field008", sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sRA"), "field008", {"3f8000000000000000000d05fff92230020900010000000000000000000100084669656c643030390000"})},
289  {"field009", sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sRA"), "field009", {"3f8000000000000000000d05fff92230020a00010000000000000000000100084669656c643031300000"})},
290  {"field010", sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sRA"), "field010", {"3f8000000000000000000d05fff92230020b00010000000000000000000100084669656c643031310000"})},
291  {"field011", sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sRA"), "field011", {"3f8000000000000000000d05fff92230020c00010000000000000000000100084669656c643031320000"})},
292  {"field012", sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sRA"), "field012", {"3f8000000000000000000d05fff92230020d00010000000000000000000100084669656c643031330000"})},
293  {"field013", sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sRA"), "field013", {"3f8000000000000000000d05fff92230020e00010000000000000000000100084669656c643031340000"})},
294  {"field014", sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sRA"), "field014", {"3f8000000000000000000d05fff92230020f00010000000000000000000100084669656c643031350000"})},
295  {"field015", sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sRA"), "field015", {"3f8000000000000000000d05fff92230021000010000000000000000000100084669656c643031360000"})},
296  {"field016", sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sRA"), "field016", {"3f8000000000000000000d05fff92230021100010000000000000000000100084669656c643031370000"})},
297  {"field017", sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sRA"), "field017", {"3f8000000000000000000d05fff92230021200010000000000000000000100084669656c643031380000"})},
298  {"field018", sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sRA"), "field018", {"3f8000000000000000000d05fff92230021300010000000000000000000100084669656c643031390000"})},
299  {"field019", sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sRA"), "field019", {"3f8000000000000000000d05fff92230021400010000000000000000000100084669656c643032300000"})},
300  {"field020", sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sRA"), "field020", {"3f8000000000000000000d05fff92230021500010000000000000000000100084669656c643032310000"})},
301  {"field021", sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sRA"), "field021", {"3f8000000000000000000d05fff92230021600010000000000000000000100084669656c643032320000"})},
302  {"field022", sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sRA"), "field022", {"3f8000000000000000000d05fff92230021700010000000000000000000100084669656c643032330000"})},
303  {"field023", sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sRA"), "field023", {"3f8000000000000000000d05fff92230021800010000000000000000000100084669656c643032340000"})},
304  {"field024", sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sRA"), "field024", {"3f8000000000000000000d05fff92230021900010000000000000000000100084669656c643032350000"})},
305  {"field025", sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sRA"), "field025", {"3f8000000000000000000d05fff92230021a00010000000000000000000100084669656c643032360000"})},
306  {"field026", sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sRA"), "field026", {"3f8000000000000000000d05fff92230021b00010000000000000000000100084669656c643032370000"})},
307  {"field027", sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sRA"), "field027", {"3f8000000000000000000d05fff92230021c00010000000000000000000100084669656c643032380000"})},
308  {"field028", sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sRA"), "field028", {"3f8000000000000000000d05fff92230021d00010000000000000000000100084669656c643032390000"})},
309  {"field029", sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sRA"), "field029", {"3f8000000000000000000d05fff92230021e00010000000000000000000100084669656c643033300000"})},
310  {"field030", sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sRA"), "field030", {"3f8000000000000000000d05fff92230021f00010000000000000000000100084669656c643033310000"})},
311  {"field031", sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sRA"), "field031", {"3f8000000000000000000d05fff92230022000010000000000000000000100084669656c643033320000"})},
312  {"field032", sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sRA"), "field032", {"3f8000000000000000000d05fff92230022100010000000000000000000100084669656c643033330000"})},
313  {"field033", sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sRA"), "field033", {"3f8000000000000000000d05fff92230022200010000000000000000000100084669656c643033340000"})},
314  {"field034", sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sRA"), "field034", {"3f8000000000000000000d05fff92230022300010000000000000000000100084669656c643033350000"})},
315  {"field035", sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sRA"), "field035", {"3f8000000000000000000d05fff92230022400010000000000000000000100084669656c643033360000"})},
316  {"field036", sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sRA"), "field036", {"3f8000000000000000000d05fff92230022500010000000000000000000100084669656c643033370000"})},
317  {"field037", sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sRA"), "field037", {"3f8000000000000000000d05fff92230022600010000000000000000000100084669656c643033380000"})},
318  {"field038", sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sRA"), "field038", {"3f8000000000000000000d05fff92230022700010000000000000000000100084669656c643033390000"})},
319  {"field039", sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sRA"), "field039", {"3f8000000000000000000d05fff92230022800010000000000000000000100084669656c643034300000"})},
320  {"field040", sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sRA"), "field040", {"3f8000000000000000000d05fff92230022900010000000000000000000100084669656c643034310000"})},
321  {"field041", sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sRA"), "field041", {"3f8000000000000000000d05fff92230022a00010000000000000000000100084669656c643034320000"})},
322  {"field042", sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sRA"), "field042", {"3f8000000000000000000d05fff92230022b00010000000000000000000100084669656c643034330000"})},
323  {"field043", sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sRA"), "field043", {"3f8000000000000000000d05fff92230022c00010000000000000000000100084669656c643034340000"})},
324  {"field044", sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sRA"), "field044", {"3f8000000000000000000d05fff92230022d00010000000000000000000100084669656c643034350000"})},
325  {"field045", sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sRA"), "field045", {"3f8000000000000000000d05fff92230022e00010000000000000000000100084669656c643034360000"})},
326  {"field046", sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sRA"), "field046", {"3f8000000000000000000d05fff92230022f00010000000000000000000100084669656c643034370000"})},
327  {"field047", sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sRA"), "field047", {"3f8000000000000000000d05fff92230023000010000000000000000000100084669656c643034380000"})},
328  {"FirmwareVersion", sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sRA"), "FirmwareVersion", {"000556332e3133"})},
329  // todo: fieldset from config ...
330  {"LIDinputstate", sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sRA"), "LIDinputstate", {"000000000000000000000000"})}, // activate fieldset 0
331  //{"LIDinputstate", sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sRA"), "LIDinputstate", {"000000000000010000000000"})}, // activate fieldset 1
332  {"LMPoutputRange", sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sRA"), "LMPoutputRange", {"000100000d05fff9223000225510"})},
333  {"LocationName", sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sRA"), "LocationName", {"000b6e6f7420646566696e6564"})},
334  {"ODoprh", sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sRA"), "ODoprh", {"00000161"})},
335  {"ODpwrc", sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sRA"), "ODpwrc", {"00000017"})},
336  {"LMDscandatacfg", sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sRA"), "LMDscandatacfg", {"01000101000000000000010001"})},
337  {"SCdevicestate", sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sRA"), "SCdevicestate", {"00"})}
338  };
339  s_mapped_responses[sick_scan::ColaParser::sWN] = { // static responses for sWN requests
340  {"EIHstCola", sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sWA"), "EIHstCola", {""})},
341  {"LMDscandatacfg", sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sWA"), "LMDscandatacfg", {""})},
342  {"LMPoutputRange", sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sWA"), "LMPoutputRange", {""})}
343  };
344  if(scanner_type == "sick_lms_5xx") // overwrite for LMS5xx
345  {
346  s_mapped_responses[sick_scan::ColaParser::sRN]["field000"] = sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sRA"), "field000", {"400000000000000000000683ffff3cb0020100010003012cffff016201d2ffff01a301e6ffff00ce0000000000000001000b7365676d656e7465645f310000"});
347  s_mapped_responses[sick_scan::ColaParser::sRN]["field001"] = sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sRA"), "field001", {"400000000000000000000683ffff3cb0010200000001000dbba0007d00000000000000c8000000c8000000000001001572656374616e676c655f6669656c645f305f6465670000"});
348  s_mapped_responses[sick_scan::ColaParser::sRN]["field002"] = sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sRA"), "field002", {"400000000000000000000683ffff3cb0010300000001000f756b007f0006ddd0000000c8000000c80000000000010010726563746669656c645f34355f6465670000"});
349  s_mapped_responses[sick_scan::ColaParser::sRN]["field003"] = sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sRA"), "field003", {"400000000000000000000683ffff3cb003040000000000000001001b774000fa00000000000003e80000012c0960000005dc0001000d64796e616d69635f6669656c640000"});
350  s_mapped_responses[sick_scan::ColaParser::sRN]["field004"] = sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sRA"), "field004", {"400000000000000000001388ffff3cb002000000000000000000000100000000"});
351  s_mapped_responses[sick_scan::ColaParser::sRN]["field005"] = sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sRA"), "field005", {"400000000000000000001388ffff3cb002000000000000000000000100000000"});
352  s_mapped_responses[sick_scan::ColaParser::sRN]["field006"] = sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sRA"), "field006", {"400000000000000000001388ffff3cb002000000000000000000000100000000"});
353  s_mapped_responses[sick_scan::ColaParser::sRN]["field007"] = sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sRA"), "field007", {"400000000000000000001388ffff3cb002000000000000000000000100000000"});
354  s_mapped_responses[sick_scan::ColaParser::sRN]["field008"] = sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sRA"), "field008", {"400000000000000000001388ffff3cb002000000000000000000000100000000"});
355  s_mapped_responses[sick_scan::ColaParser::sRN]["field009"] = sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sRA"), "field009", {"400000000000000000001388ffff3cb002000000000000000000000100000000"});
356  s_mapped_responses[sick_scan::ColaParser::sRN]["field010"] = sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sRA"), "field010", {"00"});
357  s_mapped_responses[sick_scan::ColaParser::sRN]["field011"] = sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sRA"), "field011", {"00"});
358  s_mapped_responses[sick_scan::ColaParser::sRN]["field012"] = sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sRA"), "field012", {"00"});
359  s_mapped_responses[sick_scan::ColaParser::sRN]["field013"] = sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sRA"), "field013", {"00"});
360  s_mapped_responses[sick_scan::ColaParser::sRN]["field014"] = sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sRA"), "field014", {"00"});
361  s_mapped_responses[sick_scan::ColaParser::sRN]["field015"] = sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sRA"), "field015", {"00"});
362  s_mapped_responses[sick_scan::ColaParser::sRN]["field016"] = sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sRA"), "field016", {"00"});
363  s_mapped_responses[sick_scan::ColaParser::sRN]["field017"] = sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sRA"), "field017", {"00"});
364  s_mapped_responses[sick_scan::ColaParser::sRN]["field018"] = sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sRA"), "field018", {"00"});
365  s_mapped_responses[sick_scan::ColaParser::sRN]["field019"] = sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sRA"), "field019", {"00"});
366  s_mapped_responses[sick_scan::ColaParser::sRN]["field020"] = sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sRA"), "field020", {"00"});
367  s_mapped_responses[sick_scan::ColaParser::sRN]["field021"] = sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sRA"), "field021", {"00"});
368  s_mapped_responses[sick_scan::ColaParser::sRN]["field022"] = sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sRA"), "field022", {"00"});
369  s_mapped_responses[sick_scan::ColaParser::sRN]["field023"] = sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sRA"), "field023", {"00"});
370  s_mapped_responses[sick_scan::ColaParser::sRN]["field024"] = sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sRA"), "field024", {"00"});
371  s_mapped_responses[sick_scan::ColaParser::sRN]["field025"] = sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sRA"), "field025", {"00"});
372  s_mapped_responses[sick_scan::ColaParser::sRN]["field026"] = sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sRA"), "field026", {"00"});
373  s_mapped_responses[sick_scan::ColaParser::sRN]["field027"] = sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sRA"), "field027", {"00"});
374  s_mapped_responses[sick_scan::ColaParser::sRN]["field028"] = sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sRA"), "field028", {"00"});
375  s_mapped_responses[sick_scan::ColaParser::sRN]["field029"] = sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sRA"), "field029", {"00"});
376  s_mapped_responses[sick_scan::ColaParser::sRN]["field030"] = sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sFA"), "", {"03"});
377  s_mapped_responses[sick_scan::ColaParser::sRN]["field031"] = sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sFA"), "", {"03"});
378  s_mapped_responses[sick_scan::ColaParser::sRN]["field032"] = sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sFA"), "", {"03"});
379  s_mapped_responses[sick_scan::ColaParser::sRN]["field033"] = sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sFA"), "", {"03"});
380  s_mapped_responses[sick_scan::ColaParser::sRN]["field034"] = sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sFA"), "", {"03"});
381  s_mapped_responses[sick_scan::ColaParser::sRN]["field035"] = sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sFA"), "", {"03"});
382  s_mapped_responses[sick_scan::ColaParser::sRN]["field036"] = sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sFA"), "", {"03"});
383  s_mapped_responses[sick_scan::ColaParser::sRN]["field037"] = sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sFA"), "", {"03"});
384  s_mapped_responses[sick_scan::ColaParser::sRN]["field038"] = sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sFA"), "", {"03"});
385  s_mapped_responses[sick_scan::ColaParser::sRN]["field039"] = sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sFA"), "", {"03"});
386  s_mapped_responses[sick_scan::ColaParser::sRN]["field040"] = sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sFA"), "", {"03"});
387  s_mapped_responses[sick_scan::ColaParser::sRN]["field041"] = sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sFA"), "", {"03"});
388  s_mapped_responses[sick_scan::ColaParser::sRN]["field042"] = sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sFA"), "", {"03"});
389  s_mapped_responses[sick_scan::ColaParser::sRN]["field043"] = sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sFA"), "", {"03"});
390  s_mapped_responses[sick_scan::ColaParser::sRN]["field044"] = sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sFA"), "", {"03"});
391  s_mapped_responses[sick_scan::ColaParser::sRN]["field045"] = sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sFA"), "", {"03"});
392  s_mapped_responses[sick_scan::ColaParser::sRN]["field046"] = sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sFA"), "", {"03"});
393  s_mapped_responses[sick_scan::ColaParser::sRN]["field047"] = sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sFA"), "", {"03"});
394  s_mapped_responses[sick_scan::ColaParser::sRN]["LMPoutputRange"] = sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sRA"), "LMPoutputRange", {"000100000683ffff3cb0001c3a90"});
395  s_mapped_responses[sick_scan::ColaParser::sRN]["LMDscandatacfg"] = sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sRA"), "LMDscandatacfg", {"01000100000000000000010001"});
396  }
397  if(scanner_type == "sick_lms_1xx") // overwrite for LMS1xx
398  {
399  s_mapped_responses[sick_scan::ColaParser::sRN]["field000"] = sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sRA"), "field000", {"3f8000000000000000001388fff92230020100010029019affff0176019bffff0175019cffff0171019dffff015f019effff0154019fffff015101a0ffff015101a1ffff015101a2ffff015101a3ffff015101a4ffff015101a5ffff015101a6ffff015201a7ffff015301a8ffff014601a9ffff014601aaffff014601abffff014901acffff014901adffff014b01aeffff014901afffff014901b0ffff014901b1ffff014a01b2ffff014301b3ffff014301b4ffff014301b5ffff014801b6ffff014801b7ffff014801b8ffff014d01b9ffff014e01baffff014c01bbffff014a01bcffff014701bdffff014701beffff014701bfffff014a01c0ffff014601c1ffff014601c2ffff0146000000000000000100064649454c44310000"});
400  s_mapped_responses[sick_scan::ColaParser::sRN]["field001"] = sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sRA"), "field001", {"3f8000000000000000001388fff922300102000000010019e4b7045efffeee90000001900000006400000000000100064649454c44320000"});
401  s_mapped_responses[sick_scan::ColaParser::sRN]["field002"] = sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sRA"), "field002", {"3f8000000000000000001388fff9223003030000000000000001001b774003e800000000000007d0000003e81388000007d0000100064649454c44330000"});
402  s_mapped_responses[sick_scan::ColaParser::sRN]["field003"] = sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sRA"), "field003", {"3f8000000000000000001388fff9223002000000000000000000000100000000"});
403  s_mapped_responses[sick_scan::ColaParser::sRN]["field004"] = sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sRA"), "field004", {"3f8000000000000000001388fff9223002000000000000000000000100000000"});
404  s_mapped_responses[sick_scan::ColaParser::sRN]["field005"] = sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sRA"), "field005", {"3f8000000000000000001388fff9223002000000000000000000000100000000"});
405  s_mapped_responses[sick_scan::ColaParser::sRN]["field006"] = sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sRA"), "field006", {"3f8000000000000000001388fff9223002000000000000000000000100000000"});
406  s_mapped_responses[sick_scan::ColaParser::sRN]["field007"] = sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sRA"), "field007", {"3f8000000000000000001388fff9223002000000000000000000000100000000"});
407  s_mapped_responses[sick_scan::ColaParser::sRN]["field008"] = sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sRA"), "field008", {"3f8000000000000000001388fff9223002000000000000000000000100000000"});
408  s_mapped_responses[sick_scan::ColaParser::sRN]["field009"] = sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sRA"), "field009", {"3f8000000000000000001388fff9223002000000000000000000000100000000"});
409  s_mapped_responses[sick_scan::ColaParser::sRN]["field010"] = sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sRA"), "field010", {"00"});
410  s_mapped_responses[sick_scan::ColaParser::sRN]["field011"] = sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sRA"), "field011", {"00"});
411  s_mapped_responses[sick_scan::ColaParser::sRN]["field012"] = sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sRA"), "field012", {"00"});
412  s_mapped_responses[sick_scan::ColaParser::sRN]["field013"] = sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sRA"), "field013", {"00"});
413  s_mapped_responses[sick_scan::ColaParser::sRN]["field014"] = sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sRA"), "field014", {"00"});
414  s_mapped_responses[sick_scan::ColaParser::sRN]["field015"] = sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sRA"), "field015", {"00"});
415  s_mapped_responses[sick_scan::ColaParser::sRN]["field016"] = sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sRA"), "field016", {"00"});
416  s_mapped_responses[sick_scan::ColaParser::sRN]["field017"] = sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sRA"), "field017", {"00"});
417  s_mapped_responses[sick_scan::ColaParser::sRN]["field018"] = sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sRA"), "field018", {"00"});
418  s_mapped_responses[sick_scan::ColaParser::sRN]["field019"] = sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sRA"), "field019", {"00"});
419  s_mapped_responses[sick_scan::ColaParser::sRN]["field020"] = sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sRA"), "field020", {"00"});
420  s_mapped_responses[sick_scan::ColaParser::sRN]["field021"] = sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sRA"), "field021", {"00"});
421  s_mapped_responses[sick_scan::ColaParser::sRN]["field022"] = sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sRA"), "field022", {"00"});
422  s_mapped_responses[sick_scan::ColaParser::sRN]["field023"] = sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sRA"), "field023", {"00"});
423  s_mapped_responses[sick_scan::ColaParser::sRN]["field023"] = sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sRA"), "field023", {"00"});
424  s_mapped_responses[sick_scan::ColaParser::sRN]["field024"] = sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sRA"), "field024", {"00"});
425  s_mapped_responses[sick_scan::ColaParser::sRN]["field025"] = sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sRA"), "field025", {"00"});
426  s_mapped_responses[sick_scan::ColaParser::sRN]["field026"] = sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sRA"), "field026", {"00"});
427  s_mapped_responses[sick_scan::ColaParser::sRN]["field027"] = sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sRA"), "field027", {"00"});
428  s_mapped_responses[sick_scan::ColaParser::sRN]["field028"] = sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sRA"), "field028", {"00"});
429  s_mapped_responses[sick_scan::ColaParser::sRN]["field029"] = sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sRA"), "field029", {"00"});
430  s_mapped_responses[sick_scan::ColaParser::sRN]["field030"] = sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sFA"), "", {"03"});
431  s_mapped_responses[sick_scan::ColaParser::sRN]["field031"] = sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sFA"), "", {"03"});
432  s_mapped_responses[sick_scan::ColaParser::sRN]["field032"] = sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sFA"), "", {"03"});
433  s_mapped_responses[sick_scan::ColaParser::sRN]["field033"] = sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sFA"), "", {"03"});
434  s_mapped_responses[sick_scan::ColaParser::sRN]["field034"] = sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sFA"), "", {"03"});
435  s_mapped_responses[sick_scan::ColaParser::sRN]["field035"] = sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sFA"), "", {"03"});
436  s_mapped_responses[sick_scan::ColaParser::sRN]["field036"] = sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sFA"), "", {"03"});
437  s_mapped_responses[sick_scan::ColaParser::sRN]["field037"] = sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sFA"), "", {"03"});
438  s_mapped_responses[sick_scan::ColaParser::sRN]["field038"] = sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sFA"), "", {"03"});
439  s_mapped_responses[sick_scan::ColaParser::sRN]["field039"] = sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sFA"), "", {"03"});
440  s_mapped_responses[sick_scan::ColaParser::sRN]["field040"] = sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sFA"), "", {"03"});
441  s_mapped_responses[sick_scan::ColaParser::sRN]["field041"] = sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sFA"), "", {"03"});
442  s_mapped_responses[sick_scan::ColaParser::sRN]["field042"] = sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sFA"), "", {"03"});
443  s_mapped_responses[sick_scan::ColaParser::sRN]["field043"] = sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sFA"), "", {"03"});
444  s_mapped_responses[sick_scan::ColaParser::sRN]["field044"] = sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sFA"), "", {"03"});
445  s_mapped_responses[sick_scan::ColaParser::sRN]["field045"] = sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sFA"), "", {"03"});
446  s_mapped_responses[sick_scan::ColaParser::sRN]["field046"] = sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sFA"), "", {"03"});
447  s_mapped_responses[sick_scan::ColaParser::sRN]["field047"] = sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sFA"), "", {"03"});
448  s_mapped_responses[sick_scan::ColaParser::sRN]["LMPoutputRange"] = sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sRA"), "LMPoutputRange", {"000100001388fff922330022550d"});
449  s_mapped_responses[sick_scan::ColaParser::sRN]["LMDscandatacfg"] = sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sRA"), "LMDscandatacfg", {"01000000000000000000010001"});
450  }
451  s_mapped_responses_initialized = true;
452  }
453 
454  // Return response from static table, if request can be found in s_mapped_responses
455  for(std::map<sick_scan::ColaParser::COLA_SOPAS_COMMAND, std::map<std::string, sick_scan::SickLocColaTelegramMsg>>::iterator iter_cmd = s_mapped_responses.begin(); iter_cmd != s_mapped_responses.end(); iter_cmd++)
456  {
457  if(cola_request.command_type == iter_cmd->first)
458  {
459  std::map<std::string, sick_scan::SickLocColaTelegramMsg>& mapped_response = iter_cmd->second;
460  if(mapped_response.find(cola_request.command_name) != mapped_response.end())
461  {
462  return mapped_response[cola_request.command_name];
463  }
464  }
465  }
466 
467  // Generate a synthetical response to LocRequestTimestamp requests: "sAN LocRequestTimestamp <timestamp>" with uint32_t timestamp in hex and ms
468  if(cola_request.command_type == sick_scan::ColaParser::sMN && cola_request.command_name == "LocRequestTimestamp")
469  {
470  static sick_scan::UniformRandomInteger time_jitter_network_ms(0, 2);
471  // Simulate some network latency
472  ROS::sleep(0.001 * time_jitter_network_ms.generate());
473  // Create current timestamp in ticks
474  uint32_t ticks_ms = createTimestampTicksMilliSec();
475  // Simulate some network latency
476  ROS::sleep(0.001 * time_jitter_network_ms.generate());
477  return sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::sAN, cola_request.command_name, {hexstr(ticks_ms)});
478  }
479 
480  // Set settings from Configuration Telegrams
481  if(cola_request.command_type == sick_scan::ColaParser::sMN && cola_request.command_name == "LMCstartmeas")
482  {
483  s_controller_settings["LMCstartmeas"] = 1;
484  return sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sAN"), "LMCstartmeas", {"00"});
485  }
486  if(cola_request.command_type == sick_scan::ColaParser::sMN && cola_request.command_name == "LMCstopmeas")
487  {
488  s_controller_settings["LMCstartmeas"] = 0;
489  return sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::convertSopasCommand("sAN"), "LMCstopmeas", {"00"});
490  }
491  if(cola_request.command_type == sick_scan::ColaParser::sMN && cola_request.command_name == "LocStartLocalizing")
492  {
493  s_controller_settings["LocState"] = 2;
494  return sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::sAN, cola_request.command_name, {decstr(1)});
495  }
496  if(cola_request.command_type == sick_scan::ColaParser::sMN && cola_request.command_name == "LocStop")
497  {
498  s_controller_settings["LocState"] = 1;
499  return sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::sAN, cola_request.command_name, {decstr(1)});
500  }
501  if(cola_request.command_type == sick_scan::ColaParser::sMN && cola_request.command_name == "LocStopAndSave")
502  {
503  s_controller_settings["LocState"] = 1;
504  return sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::sAN, cola_request.command_name, {decstr(1)});
505  }
506  if(cola_request.command_type == sick_scan::ColaParser::sMN && cola_request.command_name == "LocSetResultPort" && cola_request.parameter.size() == 1)
507  {
508  s_controller_settings["LocResultPort"] = std::strtol(cola_request.parameter[0].c_str(), 0, 0);
509  return sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::sAN, cola_request.command_name, {decstr(1), decstr(1)});
510  }
511  if(cola_request.command_type == sick_scan::ColaParser::sMN && cola_request.command_name == "LocSetResultMode" && cola_request.parameter.size() == 1)
512  {
513  s_controller_settings["LocResultMode"] = std::strtol(cola_request.parameter[0].c_str(), 0, 0);
514  return sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::sAN, cola_request.command_name, {decstr(1)});
515  }
516  if(cola_request.command_type == sick_scan::ColaParser::sMN && cola_request.command_name == "LocSetResultPoseEnabled" && cola_request.parameter.size() == 1)
517  {
518  s_controller_settings["LocResultState"] = std::strtol(cola_request.parameter[0].c_str(), 0, 0);
519  return sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::sAN, cola_request.command_name, {decstr(1)});
520  }
521  if(cola_request.command_type == sick_scan::ColaParser::sMN && cola_request.command_name == "LocSetResultEndianness" && cola_request.parameter.size() == 1)
522  {
523  s_controller_settings["LocResultEndianness"] = std::strtol(cola_request.parameter[0].c_str(), 0, 0);;
524  return sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::sAN, cola_request.command_name, {decstr(1)});
525  }
526 
527  if(cola_request.command_type == sick_scan::ColaParser::sMN && cola_request.command_name == "LocSetPose" && cola_request.parameter.size() == 4)
528  {
529  int32_t posex_mm = std::strtol(cola_request.parameter[0].c_str(), 0, 0);
530  int32_t posey_mm = std::strtol(cola_request.parameter[1].c_str(), 0, 0);
531  int32_t yaw_mdeg = std::strtol(cola_request.parameter[2].c_str(), 0, 0);
532  int32_t uncertainty = std::strtol(cola_request.parameter[3].c_str(), 0, 0);
533  bool success = (posex_mm >= -300000 && posex_mm <= +300000 && posey_mm >= -300000 && posey_mm <= +300000
534  && yaw_mdeg >= -180000 && yaw_mdeg <= +180000 && uncertainty >= 0 && uncertainty < 300000);
535  return sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::sAN, cola_request.command_name, {decstr(success?1:0)});
536  }
537 
538  if(cola_request.command_type == sick_scan::ColaParser::sMN && cola_request.command_name == "LocSetResultPoseInterval" && cola_request.parameter.size() == 1)
539  {
540  s_u32ResultPoseInterval = std::strtoul(cola_request.parameter[0].c_str(), 0, 0);
541  return sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::sAN, cola_request.command_name, {decstr(1)});
542  }
543 
544  /* Start of test server responses for new service requests (release 4 or later) */
545 
546  if(cola_request.command_type == sick_scan::ColaParser::sMN && cola_request.command_name == "DevSetLidarConfig" && cola_request.parameter.size() == 15)
547  {
548  for(size_t n = 0; n < cola_request.parameter.size(); n++)
549  s_controller_settings_str["DevSetLidarConfig_"+std::to_string(n)] = cola_request.parameter[n];
550  return sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::sAN, cola_request.command_name, {decstr(1), decstr(1)});
551  }
552 
553  if(cola_request.command_type == sick_scan::ColaParser::sMN && cola_request.command_name == "DevGetLidarConfig" && cola_request.parameter.size() == 1)
554  {
555  std::vector<std::string> config_parameter;
556  for(size_t n = 1; n < 15; n++)
557  // if(n != 9)
558  config_parameter.push_back(s_controller_settings_str["DevSetLidarConfig_"+std::to_string(n)]);
559  return sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::sAN, cola_request.command_name, config_parameter);
560  }
561 
562  if(cola_request.command_type == sick_scan::ColaParser::sMN && cola_request.command_name == "LocSetMap" && cola_request.parameter.size() == 2)
563  {
564  s_controller_settings_str["LocSetMap"] = cola_request.parameter[1];
565  return sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::sAN, cola_request.command_name, {decstr(1), decstr(1)});
566  }
567 
568  if(cola_request.command_name == "LocMap")//if(cola_request.command_type == sick_scan::ColaParser::sRN && cola_request.command_name == "LocMap" && cola_request.parameter.size() == 0)
569  {
570  return sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::sRA, cola_request.command_name, {s_controller_settings_str["LocSetMap"]});
571  }
572 
573  if(cola_request.command_type == sick_scan::ColaParser::sRN && cola_request.command_name == "LocMapState" && cola_request.parameter.size() == 0)
574  {
575  return sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::sAN, cola_request.command_name, {decstr(1)});
576  }
577 
578  if(cola_request.command_type == sick_scan::ColaParser::sMN && cola_request.command_name == "LocInitializePose" && cola_request.parameter.size() == 4)
579  {
580  for(size_t n = 0; n < cola_request.parameter.size(); n++)
581  s_controller_settings_str["LocInitializePose_"+std::to_string(n)] = cola_request.parameter[n];
582  return sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::sAN, cola_request.command_name, {decstr(1)});
583  }
584 
585  if(cola_request.command_type == sick_scan::ColaParser::sRN && cola_request.command_name == "LocInitialPose" && cola_request.parameter.size() == 0)
586  {
587  std::vector<std::string> parameter;
588  for(size_t n = 0; n < 4; n++)
589  parameter.push_back(s_controller_settings_str["LocInitializePose_"+std::to_string(n)]);
590  return sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::sAN, cola_request.command_name, parameter);
591  }
592 
593  if(cola_request.command_type == sick_scan::ColaParser::sMN && cola_request.command_name == "LocSetPoseQualityCovWeight" && cola_request.parameter.size() == 1)
594  {
595  s_controller_settings_str["LocSetPoseQualityCovWeight"] = cola_request.parameter[0];
596  return sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::sAN, cola_request.command_name, {decstr(1)});
597  }
598 
599  if(cola_request.command_type == sick_scan::ColaParser::sRN && cola_request.command_name == "LocPoseQualityCovWeight" && cola_request.parameter.size() == 0)
600  {
601  return sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::sAN, cola_request.command_name, {s_controller_settings_str["LocSetPoseQualityCovWeight"]});
602  }
603 
604  if(cola_request.command_type == sick_scan::ColaParser::sMN && cola_request.command_name == "LocSetPoseQualityMeanDistWeight" && cola_request.parameter.size() == 1)
605  {
606  s_controller_settings_str["LocSetPoseQualityMeanDistWeight"] = cola_request.parameter[0];
607  return sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::sAN, cola_request.command_name, {decstr(1)});
608  }
609 
610  if(cola_request.command_type == sick_scan::ColaParser::sRN && cola_request.command_name == "LocPoseQualityMeanDistWeight" && cola_request.parameter.size() == 0)
611  {
612  return sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::sAN, cola_request.command_name, {s_controller_settings_str["LocSetPoseQualityMeanDistWeight"]});
613  }
614 
615  if(cola_request.command_type == sick_scan::ColaParser::sMN && cola_request.command_name == "LocSetReflectorsForSupportActive" && cola_request.parameter.size() == 1)
616  {
617  s_controller_settings_str["LocSetReflectorsForSupportActive"] = cola_request.parameter[0];
618  return sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::sAN, cola_request.command_name, {decstr(1)});
619  }
620 
621  if(cola_request.command_type == sick_scan::ColaParser::sRN && cola_request.command_name == "LocReflectorsForSupportActive" && cola_request.parameter.size() == 0)
622  {
623  return sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::sAN, cola_request.command_name, {s_controller_settings_str["LocSetReflectorsForSupportActive"]});
624  }
625 
626  if(cola_request.command_type == sick_scan::ColaParser::sMN && cola_request.command_name == "LocSetOdometryActive" && cola_request.parameter.size() == 1)
627  {
628  s_controller_settings_str["LocSetOdometryActive"] = cola_request.parameter[0];
629  return sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::sAN, cola_request.command_name, {decstr(1), decstr(1)});
630  }
631 
632  if(cola_request.command_type == sick_scan::ColaParser::sRN && cola_request.command_name == "LocOdometryActive" && cola_request.parameter.size() == 0)
633  {
634  return sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::sAN, cola_request.command_name, {s_controller_settings_str["LocSetOdometryActive"]});
635  }
636 
637  if(cola_request.command_type == sick_scan::ColaParser::sMN && cola_request.command_name == "LocSetOdometryPort" && cola_request.parameter.size() == 1)
638  {
639  s_controller_settings_str["LocSetOdometryPort"] = cola_request.parameter[0];
640  return sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::sAN, cola_request.command_name, {decstr(1), decstr(1)});
641  }
642 
643  if(cola_request.command_type == sick_scan::ColaParser::sRN && cola_request.command_name == "LocOdometryPort" && cola_request.parameter.size() == 0)
644  {
645  return sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::sAN, cola_request.command_name, {s_controller_settings_str["LocSetOdometryPort"]});
646  }
647 
648  if(cola_request.command_type == sick_scan::ColaParser::sMN && cola_request.command_name == "LocSetOdometryRestrictYMotion" && cola_request.parameter.size() == 1)
649  {
650  s_controller_settings_str["LocSetOdometryRestrictYMotion"] = cola_request.parameter[0];
651  return sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::sAN, cola_request.command_name, {decstr(1)});
652  }
653 
654  if(cola_request.command_type == sick_scan::ColaParser::sRN && cola_request.command_name == "LocOdometryRestrictYMotion" && cola_request.parameter.size() == 0)
655  {
656  return sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::sAN, cola_request.command_name, {s_controller_settings_str["LocSetOdometryRestrictYMotion"]});
657  }
658 
659  if(cola_request.command_type == sick_scan::ColaParser::sMN && cola_request.command_name == "LocSetAutoStartActive" && cola_request.parameter.size() == 1)
660  {
661  s_controller_settings_str["LocSetAutoStartActive"] = cola_request.parameter[0];
662  return sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::sAN, cola_request.command_name, {decstr(1)});
663  }
664 
665  if(cola_request.command_type == sick_scan::ColaParser::sRN && cola_request.command_name == "LocAutoStartActive" && cola_request.parameter.size() == 0)
666  {
667  return sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::sAN, cola_request.command_name, {s_controller_settings_str["LocSetAutoStartActive"]});
668  }
669 
670  if(cola_request.command_type == sick_scan::ColaParser::sMN && cola_request.command_name == "LocSetAutoStartSavePoseInterval" && cola_request.parameter.size() == 1)
671  {
672  s_controller_settings_str["LocSetAutoStartSavePoseInterval"] = cola_request.parameter[0];
673  return sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::sAN, cola_request.command_name, {decstr(1)});
674  }
675 
676  if(cola_request.command_type == sick_scan::ColaParser::sRN && cola_request.command_name == "LocAutoStartSavePoseInterval" && cola_request.parameter.size() == 0)
677  {
678  return sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::sAN, cola_request.command_name, {s_controller_settings_str["LocSetAutoStartSavePoseInterval"]});
679  }
680 
681  if(cola_request.command_type == sick_scan::ColaParser::sMN && cola_request.command_name == "LocSetRingBufferRecordingActive" && cola_request.parameter.size() == 1)
682  {
683  s_controller_settings_str["LocSetRingBufferRecordingActive"] = cola_request.parameter[0];
684  return sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::sAN, cola_request.command_name, {decstr(1)});
685  }
686 
687  if(cola_request.command_type == sick_scan::ColaParser::sRN && cola_request.command_name == "LocRingBufferRecordingActive" && cola_request.parameter.size() == 0)
688  {
689  return sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::sAN, cola_request.command_name, {s_controller_settings_str["LocSetRingBufferRecordingActive"]});
690  }
691 
692  if(cola_request.command_type == sick_scan::ColaParser::sMN && cola_request.command_name == "DevGetLidarIdent" && cola_request.parameter.size() == 1)
693  {
694  return sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::sAN, cola_request.command_name, {"TestcaseGenerator" + cola_request.parameter[0]});
695  }
696 
697  if(cola_request.command_type == sick_scan::ColaParser::sMN && cola_request.command_name == "DevGetLidarState" && cola_request.parameter.size() == 1)
698  {
699  return sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::sAN, cola_request.command_name, {decstr(2), decstr(2), decstr(2)});
700  }
701 
702  if(cola_request.command_type == sick_scan::ColaParser::sMN && cola_request.command_name == "GetSoftwareVersion" && cola_request.parameter.size() == 0)
703  {
704  return sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::sAN, cola_request.command_name, {"1.0"});
705  }
706 
707  if(cola_request.command_type == sick_scan::ColaParser::sMN && cola_request.command_name == "LocAutoStartSavePose" && cola_request.parameter.size() == 0)
708  {
709  return sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::sAN, cola_request.command_name, {decstr(1)});
710  }
711 
712  if(cola_request.command_type == sick_scan::ColaParser::sMN && cola_request.command_name == "LocForceUpdate" && cola_request.parameter.size() == 0)
713  {
714  return sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::sAN, cola_request.command_name, {decstr(1)});
715  }
716 
717  if(cola_request.command_type == sick_scan::ColaParser::sMN && cola_request.command_name == "LocSaveRingBufferRecording" && cola_request.parameter.size() == 2)
718  {
719  return sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::sAN, cola_request.command_name, {decstr(2)});
720  }
721 
722  if(cola_request.command_type == sick_scan::ColaParser::sMN && cola_request.command_name == "LocStartDemoMapping" && cola_request.parameter.size() == 0)
723  {
724  return sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::sAN, cola_request.command_name, {decstr(1)});
725  }
726 
727  if(cola_request.command_type == sick_scan::ColaParser::sMN && cola_request.command_name == "ReportUserMessage" && cola_request.parameter.size() == 2)
728  {
729  return sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::sAN, cola_request.command_name, {decstr(1)});
730  }
731 
732  if(cola_request.command_type == sick_scan::ColaParser::sMN && cola_request.command_name == "SavePermanent" && cola_request.parameter.size() == 0)
733  {
734  return sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::sAN, cola_request.command_name, {decstr(1)});
735  }
736 
737  if(cola_request.command_type == sick_scan::ColaParser::sRN && cola_request.command_name == "LocResultPort" && cola_request.parameter.size() == 0)
738  {
739  int32_t port = ((s_controller_settings["LocResultPort"]) > 0 ? s_controller_settings["LocResultPort"] : 2201);
740  return sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::sRA, cola_request.command_name, {hexstr(port)});
741  }
742 
743  if(cola_request.command_type == sick_scan::ColaParser::sRN && cola_request.command_name == "LocResultMode" && cola_request.parameter.size() == 0)
744  {
745  return sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::sRA, cola_request.command_name, {decstr(s_controller_settings["LocResultMode"])});
746  }
747 
748  if(cola_request.command_type == sick_scan::ColaParser::sRN && cola_request.command_name == "LocResultEndianness" && cola_request.parameter.size() == 0)
749  {
750  return sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::sRA, cola_request.command_name, {decstr(s_controller_settings["LocResultEndianness"])});
751  }
752 
753  if(cola_request.command_type == sick_scan::ColaParser::sRN && cola_request.command_name == "LocResultState" && cola_request.parameter.size() == 0)
754  {
755  return sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::sRA, cola_request.command_name, {decstr(s_controller_settings["LocResultState"])});
756  }
757 
758  if(cola_request.command_type == sick_scan::ColaParser::sRN && cola_request.command_name == "LocResultPoseInterval" && cola_request.parameter.size() == 0)
759  {
760  return sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::sRA, cola_request.command_name, {decstr(s_u32ResultPoseInterval)});
761  }
762 
763  if(cola_request.command_type == sick_scan::ColaParser::sMN && cola_request.command_name == "DevSetIMUActive" && cola_request.parameter.size() == 1)
764  {
765  s_controller_settings_str["DevSetIMUActive"] = cola_request.parameter[0];
766  return sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::sAN, cola_request.command_name, {decstr(1)});
767  }
768 
769  if(cola_request.command_type == sick_scan::ColaParser::sRN && cola_request.command_name == "DevIMUActive" && cola_request.parameter.size() == 0)
770  {
771  return sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::sRA, cola_request.command_name, {s_controller_settings_str["DevSetIMUActive"]});
772  }
773 
774  /* End of test server responses for new service requests (release 4 or later) */
775 
776  // Create sAN responses to sMN requests resp. sRA responses to sRN requests
777  if(cola_request.command_type == sick_scan::ColaParser::sMN || cola_request.command_type == sick_scan::ColaParser::sRN)
778  {
779  sick_scan::ColaParser::COLA_SOPAS_COMMAND response_type = sick_scan::ColaParser::sINVALID;
780  if(cola_request.command_type == sick_scan::ColaParser::sMN)
781  response_type = sick_scan::ColaParser::sAN; // sAN responses to sMN requests
782  else if(cola_request.command_type == sick_scan::ColaParser::sRN)
783  response_type = sick_scan::ColaParser::sRA; // sRA responses to sRN requests
784  for(std::map<std::string, int32_t>::iterator iter_settings = s_controller_settings.begin(); iter_settings != s_controller_settings.end(); iter_settings++)
785  {
786  if(cola_request.command_name == iter_settings->first)
787  {
788  return sick_scan::ColaParser::createColaTelegram(response_type, cola_request.command_name, {hexstr(iter_settings->second)});
789  }
790  }
791  }
792 
793  // Default response: "sAN <command_name>" without parameter (sAN: Response to sMN)
794  return sick_scan::ColaParser::createColaTelegram(sick_scan::ColaParser::sAN, cola_request.command_name);
795 }
796 
803 uint32_t sick_scan::TestcaseGenerator::createTimestampTicksMilliSec(void)
804 {
805  static ROS::Time start = ROS::now();
806  static sick_scan::UniformRandomInteger time_jitter_ticks_ms(-2, +2);
807  // Create current timestamp in ticks
808  ROS::Duration timestamp = (ROS::now() - start);
809  uint32_t seconds = 0, nanoseconds = 0;
810  ROS::splitTime(timestamp, seconds, nanoseconds);
811  uint32_t ticks_ms = (((uint64_t)seconds * 1000 + (uint64_t)nanoseconds/1000000 + 1000) & 0xFFFFFFFF);
812  // Create some jitter, simulation network latency and time drift
813  ticks_ms += time_jitter_ticks_ms.generate();
814  return ticks_ms;
815 }
sick_scan::Utils::normalizeAngle
static double normalizeAngle(double angle)
Definition: utils.h:260
sick_scan::ResultPortParser::getTelegramMsg
virtual sick_scan::SickLocResultPortTelegramMsg & getTelegramMsg(void)
Definition: result_port_parser.h:93
sick_scan::Utils::flattenToString
static std::string flattenToString(const T &x)
Definition: utils.h:144
sick_scan::ColaParser::sMN
Method by name (request)
Definition: cola_parser.h:97
cola_parser.h
sick_scan::TestcaseGenerator::ResultTelegramsEnabled
static bool ResultTelegramsEnabled(void)
Definition: testcase_generator.cpp:112
sick_scan::ColaParser::sRA
Read by name (response)
Definition: cola_parser.h:96
ROS::now
ROS::Time now(void)
Definition: ros_wrapper.cpp:116
sick_scan::TestcaseGenerator::s_controller_settings
static std::map< std::string, int32_t > s_controller_settings
test server int32 settings, set by sMN or sRN requests
Definition: testcase_generator.h:167
sick_scan::TestcaseGenerator::createColaResponse
static sick_scan::SickLocColaTelegramMsg createColaResponse(const sick_scan::SickLocColaTelegramMsg &cola_request, const std::string &scanner_type)
Definition: testcase_generator.cpp:261
sick_scan::ColaParser::sAN
Response to sMN.
Definition: cola_parser.h:98
random_generator.h
testcase_generator.h
sick_scan::TestcaseGenerator::s_u32ResultPoseInterval
static uint32_t s_u32ResultPoseInterval
result pose interval, i.e. the interval in number of scans (default: 1, i.e. result telegram with eac...
Definition: testcase_generator.h:166
sick_scan::Utils::toHexString
static std::string toHexString(const std::vector< uint8_t > &binary_data)
Definition: utils.cpp:69
sick_scan::TestcaseGenerator::createTimestampTicksMilliSec
static uint32_t createTimestampTicksMilliSec(void)
Definition: testcase_generator.cpp:803
sick_scan::TestcaseGenerator::SendScandataEnabled
static bool SendScandataEnabled(void)
Definition: testcase_generator.cpp:94
sick_scan::ResultPortParser
Definition: result_port_parser.h:67
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::sINVALID
uninitialized, command_type should never have this value)
Definition: cola_parser.h:94
ROS::splitTime
void splitTime(ROS::Duration time, uint32_t &seconds, uint32_t &nanoseconds)
Definition: ros_wrapper.cpp:128
sick_scan::TestcaseGenerator::createDefaultResultPortTestcase
static sick_scan::SickLocResultPortTestcaseMsg createDefaultResultPortTestcase(void)
Definition: testcase_generator.cpp:121
ros_wrapper.h
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::sWN
Write by name (request)
Definition: cola_parser.h:100
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::TestcaseGenerator::s_controller_settings_str
static std::map< std::string, std::string > s_controller_settings_str
test server string settings, set by sMN or sRN requests
Definition: testcase_generator.h:168
sick_scan::TestcaseGenerator::LocalizationEnabled
static bool LocalizationEnabled(void)
Definition: testcase_generator.cpp:103
sick_scan::UniformRandomInteger
Definition: random_generator.h:65
sick_scan::ResultPortParser::encode
virtual std::vector< uint8_t > encode(void)
Definition: result_port_parser.cpp:429
sick_scan::ResultPortParser::decode
virtual bool decode(const std::vector< uint8_t > &binary_data)
Definition: result_port_parser.cpp:314
ROS_ERROR_STREAM
#define ROS_ERROR_STREAM(args)
sick_scan::TestcaseGenerator::createRandomResultPortTestcase
static sick_scan::SickLocResultPortTestcaseMsg createRandomResultPortTestcase(void)
Definition: testcase_generator.cpp:158
sick_scan::UniformRandomInteger::generate
int generate(void)
Definition: random_generator.cpp:75
ROS::sleep
void sleep(double seconds)
Definition: ros_wrapper.cpp:110
sick_scan::ColaParser::sRN
Read by name (request)
Definition: cola_parser.h:95
ROS::seconds
double seconds(ROS::Duration duration)
Definition: ros_wrapper.cpp:180
sick_scan::TestcaseGenerator::createResultPortCircles
static sick_scan::SickLocResultPortTestcaseMsg createResultPortCircles(double circle_radius, double circle_yaw)
Definition: testcase_generator.cpp:220
sick_scan::ColaParser::sEN
Event by name (request)
Definition: cola_parser.h:102

sick_scan
Author(s): Michael Lehning , Jochen Sprickerhof , Martin Günther
autogenerated on Wed May 5 2021 03:05:48