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_xd::TestcaseGenerator::s_u32ResultPoseInterval = 1;
68 
72 std::map<std::string, int32_t> sick_scan_xd::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_xd::TestcaseGenerator::s_controller_settings_str;
88 
94 bool sick_scan_xd::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_xd::TestcaseGenerator::LocalizationEnabled(void)
104 {
105  return s_controller_settings["LocState"] == 2; // localization on
106 }
107 
112 bool sick_scan_xd::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_xd::SickLocResultPortTestcaseMsg sick_scan_xd::TestcaseGenerator::createDefaultResultPortTestcase(void)
122 {
123  sick_scan_xd::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_xd::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_xd::ResultPortParser::decode() failed. " << testcase.binary_data.size() << " byte input (hex):");
144  ROS_ERROR_STREAM(sick_scan_xd::Utils::toHexString(testcase.binary_data));
145  ROS_ERROR_STREAM("## output (decoded): " << sick_scan_xd::Utils::flattenToString(result_port_parser.getTelegramMsg()));
146  ROS_ERROR_STREAM("## recoded:");
147  ROS_ERROR_STREAM(sick_scan_xd::Utils::toHexString(recoded_telegram));
148  }
149  testcase.telegram_msg = result_port_parser.getTelegramMsg();
150 
151  return testcase;
152 }
153 
158 sick_scan_xd::SickLocResultPortTestcaseMsg sick_scan_xd::TestcaseGenerator::createRandomResultPortTestcase(void)
159 {
160  // Random number generators
161  static sick_scan_xd::UniformRandomInteger random1_generator(0, 1);
162  static sick_scan_xd::UniformRandomInteger random8_generator(0, 255);
163  static sick_scan_xd::UniformRandomInteger random32_generator(-INT32_MAX, INT32_MAX);
164  static sick_scan_xd::UniformRandomInteger random_yaw_generator(-180000, 180000);
165  static sick_scan_xd::UniformRandomInteger random_quality_generator(0, 100);
166  static sick_scan_xd::UniformRandomInteger random_covariance_generator(0, INT32_MAX);
167 
168  // Create default SickLocResultPortTelegramMsg
169  static ROS::Time start_time = ROS::now();
170  static sick_scan_xd::SickLocResultPortTestcaseMsg default_testcase = createDefaultResultPortTestcase();
171  sick_scan_xd::SickLocResultPortTestcaseMsg testcase = default_testcase;
172  sick_scan_xd::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_xd::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_xd::SickLocResultPortTestcaseMsg sick_scan_xd::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_xd::SickLocResultPortTestcaseMsg default_testcase = createDefaultResultPortTestcase();
225  sick_scan_xd::SickLocResultPortTestcaseMsg testcase = default_testcase;
226  sick_scan_xd::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_xd::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_xd::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_xd::SickLocColaTelegramMsg sick_scan_xd::TestcaseGenerator::createColaResponse(const sick_scan_xd::SickLocColaTelegramMsg & cola_request, const std::string& scanner_type)
262 {
263  static std::map<sick_scan_xd::ColaParser::COLA_SOPAS_COMMAND, std::map<std::string, sick_scan_xd::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_xd::ColaParser::sEN] = { // static responses for sEN requests
268  {"ECRChangeArr", sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sEA"), "ECRChangeArr", {"01"})},
269  {"LFErec", sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sEA"), "LFErec", {"01"})},
270  {"LIDinputstate", sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sEA"), "LIDinputstate", {"01"})},
271  {"LIDoutputstate", sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sEA"), "LIDoutputstate", {"01"})},
272  {"LMDscandata", sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sEA"), "LMDscandata", {"01"})}
273  };
274  s_mapped_responses[sick_scan_xd::ColaParser::sMN] = { // static responses for sMN requests
275  {"Run", sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sAN"), "Run", {"01"})},
276  {"SetAccessMode", sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sAN"), "SetAccessMode", {"01"})}
277  };
278  s_mapped_responses[sick_scan_xd::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_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field000", {"3f8000000000000000000d05fff92230020100010000000000000000000100084669656c643030310000"})},
281  {"field001", sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field001", {"3f8000000000000000000d05fff9223002020001000200f6ffff00fa0195ffff012c000000000000000100084669656c643030320000"})},
282  {"field002", sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field002", {"3f8000000000000000000d05fff922300203000100030195ffff012c026bffff009d02a3ffff00a0000000000000000100084669656c643030330000"})},
283  {"field003", sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field003", {"3f8000000000000000000d05fff92230020400010006010affff04be0115ffff047e0120ffff04410135ffff03e90154ffff03ad0178ffff0377000000000000000100084669656c643030340000"})},
284  {"field004", sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field004", {"3f8000000000000000000d05fff92230020500010003017cffff037b018effff037901b0ffff0383000000000000000100084669656c643030350000"})},
285  {"field005", sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field005", {"3f8000000000000000000d05fff9223002060001000401baffff039201c1ffff03a101dfffff03fd0211ffff04fd000000000000000100084669656c643030360000"})},
286  {"field006", sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field006", {"3f8000000000000000000d05fff92230020700010000000000000000000100084669656c643030370000"})},
287  {"field007", sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field007", {"3f8000000000000000000d05fff92230020800010000000000000000000100084669656c643030380000"})},
288  {"field008", sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field008", {"3f8000000000000000000d05fff92230020900010000000000000000000100084669656c643030390000"})},
289  {"field009", sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field009", {"3f8000000000000000000d05fff92230020a00010000000000000000000100084669656c643031300000"})},
290  {"field010", sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field010", {"3f8000000000000000000d05fff92230020b00010000000000000000000100084669656c643031310000"})},
291  {"field011", sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field011", {"3f8000000000000000000d05fff92230020c00010000000000000000000100084669656c643031320000"})},
292  {"field012", sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field012", {"3f8000000000000000000d05fff92230020d00010000000000000000000100084669656c643031330000"})},
293  {"field013", sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field013", {"3f8000000000000000000d05fff92230020e00010000000000000000000100084669656c643031340000"})},
294  {"field014", sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field014", {"3f8000000000000000000d05fff92230020f00010000000000000000000100084669656c643031350000"})},
295  {"field015", sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field015", {"3f8000000000000000000d05fff92230021000010000000000000000000100084669656c643031360000"})},
296  {"field016", sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field016", {"3f8000000000000000000d05fff92230021100010000000000000000000100084669656c643031370000"})},
297  {"field017", sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field017", {"3f8000000000000000000d05fff92230021200010000000000000000000100084669656c643031380000"})},
298  {"field018", sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field018", {"3f8000000000000000000d05fff92230021300010000000000000000000100084669656c643031390000"})},
299  {"field019", sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field019", {"3f8000000000000000000d05fff92230021400010000000000000000000100084669656c643032300000"})},
300  {"field020", sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field020", {"3f8000000000000000000d05fff92230021500010000000000000000000100084669656c643032310000"})},
301  {"field021", sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field021", {"3f8000000000000000000d05fff92230021600010000000000000000000100084669656c643032320000"})},
302  {"field022", sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field022", {"3f8000000000000000000d05fff92230021700010000000000000000000100084669656c643032330000"})},
303  {"field023", sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field023", {"3f8000000000000000000d05fff92230021800010000000000000000000100084669656c643032340000"})},
304  {"field024", sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field024", {"3f8000000000000000000d05fff92230021900010000000000000000000100084669656c643032350000"})},
305  {"field025", sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field025", {"3f8000000000000000000d05fff92230021a00010000000000000000000100084669656c643032360000"})},
306  {"field026", sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field026", {"3f8000000000000000000d05fff92230021b00010000000000000000000100084669656c643032370000"})},
307  {"field027", sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field027", {"3f8000000000000000000d05fff92230021c00010000000000000000000100084669656c643032380000"})},
308  {"field028", sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field028", {"3f8000000000000000000d05fff92230021d00010000000000000000000100084669656c643032390000"})},
309  {"field029", sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field029", {"3f8000000000000000000d05fff92230021e00010000000000000000000100084669656c643033300000"})},
310  {"field030", sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field030", {"3f8000000000000000000d05fff92230021f00010000000000000000000100084669656c643033310000"})},
311  {"field031", sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field031", {"3f8000000000000000000d05fff92230022000010000000000000000000100084669656c643033320000"})},
312  {"field032", sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field032", {"3f8000000000000000000d05fff92230022100010000000000000000000100084669656c643033330000"})},
313  {"field033", sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field033", {"3f8000000000000000000d05fff92230022200010000000000000000000100084669656c643033340000"})},
314  {"field034", sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field034", {"3f8000000000000000000d05fff92230022300010000000000000000000100084669656c643033350000"})},
315  {"field035", sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field035", {"3f8000000000000000000d05fff92230022400010000000000000000000100084669656c643033360000"})},
316  {"field036", sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field036", {"3f8000000000000000000d05fff92230022500010000000000000000000100084669656c643033370000"})},
317  {"field037", sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field037", {"3f8000000000000000000d05fff92230022600010000000000000000000100084669656c643033380000"})},
318  {"field038", sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field038", {"3f8000000000000000000d05fff92230022700010000000000000000000100084669656c643033390000"})},
319  {"field039", sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field039", {"3f8000000000000000000d05fff92230022800010000000000000000000100084669656c643034300000"})},
320  {"field040", sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field040", {"3f8000000000000000000d05fff92230022900010000000000000000000100084669656c643034310000"})},
321  {"field041", sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field041", {"3f8000000000000000000d05fff92230022a00010000000000000000000100084669656c643034320000"})},
322  {"field042", sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field042", {"3f8000000000000000000d05fff92230022b00010000000000000000000100084669656c643034330000"})},
323  {"field043", sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field043", {"3f8000000000000000000d05fff92230022c00010000000000000000000100084669656c643034340000"})},
324  {"field044", sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field044", {"3f8000000000000000000d05fff92230022d00010000000000000000000100084669656c643034350000"})},
325  {"field045", sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field045", {"3f8000000000000000000d05fff92230022e00010000000000000000000100084669656c643034360000"})},
326  {"field046", sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field046", {"3f8000000000000000000d05fff92230022f00010000000000000000000100084669656c643034370000"})},
327  {"field047", sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field047", {"3f8000000000000000000d05fff92230023000010000000000000000000100084669656c643034380000"})},
328  {"FirmwareVersion", sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "FirmwareVersion", {"000556332e3133"})},
329  // todo: fieldset from config ...
330  {"LIDinputstate", sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "LIDinputstate", {"000000000000000000000000"})}, // activate fieldset 0
331  //{"LIDinputstate", sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "LIDinputstate", {"000000000000010000000000"})}, // activate fieldset 1
332  {"LMPoutputRange", sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "LMPoutputRange", {"000100000d05fff9223000225510"})},
333  {"LocationName", sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "LocationName", {"000b6e6f7420646566696e6564"})},
334  {"ODoprh", sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "ODoprh", {"00000161"})},
335  {"ODpwrc", sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "ODpwrc", {"00000017"})},
336  {"LMDscandatacfg", sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "LMDscandatacfg", {"01000101000000000000010001"})},
337  // Sending : <STX><STX><STX><STX><Len=0017>sRN SCdevicestate CRC:<0x30> Receiving: <STX>sRA SCdevicestate \x01<ETX> // devicestate = 1: device ready
338  {"SCdevicestate", sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "SCdevicestate", {"01"})}
339  };
340  s_mapped_responses[sick_scan_xd::ColaParser::sWN] = { // static responses for sWN requests
341  {"EIHstCola", sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sWA"), "EIHstCola", {""})},
342  {"LMDscandatacfg", sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sWA"), "LMDscandatacfg", {""})},
343  {"LMPoutputRange", sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sWA"), "LMPoutputRange", {""})},
344  {"SetActiveApplications", sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sWA"), "SetActiveApplications", {""})}
345  };
346  // if(scanner_type == "sick_mrs_1xxx") // overwrite for MRS1104
347  {
348  // Sending: <STX><STX><STX><STX><Len=0015>sRN DeviceIdent CRC:<0x25> Receiving: <STX>sRA DeviceIdent \x00\x08\x4d\x52\x53\x31\x78\x78\x78\x78\x00\x08\x31\x2e\x30\x2e\x30\x2e\x30\x52<ETX>
349  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["DeviceIdent"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "DeviceIdent", {"00084d525331787878780008312e302e302e3052"});
350  // Sending: <STX><STX><STX><STX><Len=0016>sRN SerialNumber CRC:<0x4c> Receiving: <STX>sRA SerialNumber \x00\x08\x31\x37\x31\x30\x30\x30\x30\x31<ETX>
351  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["SerialNumber"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "SerialNumber", {"00083137313030303031"});
352  // Sending: <STX><STX><STX><STX><Len=0018>sWN FREchoFilter 0x00 CRC:<0x7f> Receiving: <STX>sWA FREchoFilter<ETX>
353  s_mapped_responses[sick_scan_xd::ColaParser::sWN]["FREchoFilter"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sWA"), "FREchoFilter", {""});
354  }
355  if(scanner_type == "sick_lms_5xx") // overwrite for LMS5xx
356  {
357  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field000"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field000", {"400000000000000000000683ffff3cb0020100010003012cffff016201d2ffff01a301e6ffff00ce0000000000000001000b7365676d656e7465645f310000"});
358  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field001"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field001", {"400000000000000000000683ffff3cb0010200000001000dbba0007d00000000000000c8000000c8000000000001001572656374616e676c655f6669656c645f305f6465670000"});
359  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field002"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field002", {"400000000000000000000683ffff3cb0010300000001000f756b007f0006ddd0000000c8000000c80000000000010010726563746669656c645f34355f6465670000"});
360  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field003"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field003", {"400000000000000000000683ffff3cb003040000000000000001001b774000fa00000000000003e80000012c0960000005dc0001000d64796e616d69635f6669656c640000"});
361  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field004"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field004", {"400000000000000000001388ffff3cb002000000000000000000000100000000"});
362  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field005"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field005", {"400000000000000000001388ffff3cb002000000000000000000000100000000"});
363  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field006"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field006", {"400000000000000000001388ffff3cb002000000000000000000000100000000"});
364  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field007"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field007", {"400000000000000000001388ffff3cb002000000000000000000000100000000"});
365  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field008"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field008", {"400000000000000000001388ffff3cb002000000000000000000000100000000"});
366  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field009"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field009", {"400000000000000000001388ffff3cb002000000000000000000000100000000"});
367  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field010"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field010", {"00"});
368  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field011"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field011", {"00"});
369  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field012"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field012", {"00"});
370  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field013"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field013", {"00"});
371  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field014"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field014", {"00"});
372  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field015"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field015", {"00"});
373  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field016"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field016", {"00"});
374  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field017"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field017", {"00"});
375  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field018"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field018", {"00"});
376  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field019"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field019", {"00"});
377  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field020"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field020", {"00"});
378  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field021"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field021", {"00"});
379  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field022"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field022", {"00"});
380  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field023"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field023", {"00"});
381  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field024"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field024", {"00"});
382  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field025"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field025", {"00"});
383  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field026"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field026", {"00"});
384  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field027"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field027", {"00"});
385  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field028"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field028", {"00"});
386  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field029"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field029", {"00"});
387  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field030"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sFA"), "", {"03"});
388  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field031"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sFA"), "", {"03"});
389  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field032"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sFA"), "", {"03"});
390  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field033"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sFA"), "", {"03"});
391  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field034"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sFA"), "", {"03"});
392  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field035"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sFA"), "", {"03"});
393  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field036"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sFA"), "", {"03"});
394  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field037"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sFA"), "", {"03"});
395  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field038"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sFA"), "", {"03"});
396  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field039"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sFA"), "", {"03"});
397  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field040"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sFA"), "", {"03"});
398  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field041"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sFA"), "", {"03"});
399  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field042"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sFA"), "", {"03"});
400  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field043"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sFA"), "", {"03"});
401  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field044"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sFA"), "", {"03"});
402  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field045"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sFA"), "", {"03"});
403  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field046"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sFA"), "", {"03"});
404  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field047"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sFA"), "", {"03"});
405  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["LMPoutputRange"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "LMPoutputRange", {"000100000683ffff3cb0001c3a90"});
406  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["LMDscandatacfg"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "LMDscandatacfg", {"01000100000000000000010001"});
407  }
408  if(scanner_type == "sick_lms_511") // overwrite for LMS511
409  {
410  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field000"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field000", {"400000000000000000000683ffff3cb0020100010003012cffff016201d2ffff01a301e6ffff00ce0000000000000001000b7365676d656e7465645f310000"});
411  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field001"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field001", {"400000000000000000000683ffff3cb0010200000001000dbba0007d00000000000000c8000000c8000000000001001572656374616e676c655f6669656c645f305f6465670000"});
412  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field002"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field002", {"400000000000000000000683ffff3cb0010300000001000f756b007f0006ddd0000000c8000000c80000000000010010726563746669656c645f34355f6465670000"});
413  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field003"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field003", {"400000000000000000000683ffff3cb003040000000000000001001b774000fa00000000000003e80000012c0960000005dc0001000d64796e616d69635f6669656c640000"});
414  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field004"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field004", {"400000000000000000001388ffff3cb002000000000000000000000100000000"});
415  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field005"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field005", {"400000000000000000001388ffff3cb002000000000000000000000100000000"});
416  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field006"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field006", {"400000000000000000001388ffff3cb002000000000000000000000100000000"});
417  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field007"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field007", {"400000000000000000001388ffff3cb002000000000000000000000100000000"});
418  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field008"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field008", {"400000000000000000001388ffff3cb002000000000000000000000100000000"});
419  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field009"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field009", {"400000000000000000001388ffff3cb002000000000000000000000100000000"});
420  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field010"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field010", {"00"});
421  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field011"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field011", {"00"});
422  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field012"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field012", {"00"});
423  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field013"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field013", {"00"});
424  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field014"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field014", {"00"});
425  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field015"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field015", {"00"});
426  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field016"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field016", {"00"});
427  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field017"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field017", {"00"});
428  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field018"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field018", {"00"});
429  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field019"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field019", {"00"});
430  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field020"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field020", {"00"});
431  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field021"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field021", {"00"});
432  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field022"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field022", {"00"});
433  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field023"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field023", {"00"});
434  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field024"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field024", {"00"});
435  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field025"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field025", {"00"});
436  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field026"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field026", {"00"});
437  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field027"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field027", {"00"});
438  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field028"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field028", {"00"});
439  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field029"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field029", {"00"});
440  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["LMDscandatacfg"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "LMDscandatacfg", {"1f000100000000000000010001"});
441  }
442  if(scanner_type == "sick_lms_1xx") // overwrite for LMS1xx
443  {
444  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field000"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field000", {"3f8000000000000000001388fff92230020100010029019affff0176019bffff0175019cffff0171019dffff015f019effff0154019fffff015101a0ffff015101a1ffff015101a2ffff015101a3ffff015101a4ffff015101a5ffff015101a6ffff015201a7ffff015301a8ffff014601a9ffff014601aaffff014601abffff014901acffff014901adffff014b01aeffff014901afffff014901b0ffff014901b1ffff014a01b2ffff014301b3ffff014301b4ffff014301b5ffff014801b6ffff014801b7ffff014801b8ffff014d01b9ffff014e01baffff014c01bbffff014a01bcffff014701bdffff014701beffff014701bfffff014a01c0ffff014601c1ffff014601c2ffff0146000000000000000100064649454c44310000"});
445  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field001"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field001", {"3f8000000000000000001388fff922300102000000010019e4b7045efffeee90000001900000006400000000000100064649454c44320000"});
446  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field002"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field002", {"3f8000000000000000001388fff9223003030000000000000001001b774003e800000000000007d0000003e81388000007d0000100064649454c44330000"});
447  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field003"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field003", {"3f8000000000000000001388fff9223002000000000000000000000100000000"});
448  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field004"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field004", {"3f8000000000000000001388fff9223002000000000000000000000100000000"});
449  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field005"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field005", {"3f8000000000000000001388fff9223002000000000000000000000100000000"});
450  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field006"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field006", {"3f8000000000000000001388fff9223002000000000000000000000100000000"});
451  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field007"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field007", {"3f8000000000000000001388fff9223002000000000000000000000100000000"});
452  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field008"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field008", {"3f8000000000000000001388fff9223002000000000000000000000100000000"});
453  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field009"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field009", {"3f8000000000000000001388fff9223002000000000000000000000100000000"});
454  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field010"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field010", {"00"});
455  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field011"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field011", {"00"});
456  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field012"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field012", {"00"});
457  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field013"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field013", {"00"});
458  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field014"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field014", {"00"});
459  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field015"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field015", {"00"});
460  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field016"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field016", {"00"});
461  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field017"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field017", {"00"});
462  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field018"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field018", {"00"});
463  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field019"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field019", {"00"});
464  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field020"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field020", {"00"});
465  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field021"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field021", {"00"});
466  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field022"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field022", {"00"});
467  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field023"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field023", {"00"});
468  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field023"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field023", {"00"});
469  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field024"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field024", {"00"});
470  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field025"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field025", {"00"});
471  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field026"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field026", {"00"});
472  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field027"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field027", {"00"});
473  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field028"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field028", {"00"});
474  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field029"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field029", {"00"});
475  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field030"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sFA"), "", {"03"});
476  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field031"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sFA"), "", {"03"});
477  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field032"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sFA"), "", {"03"});
478  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field033"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sFA"), "", {"03"});
479  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field034"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sFA"), "", {"03"});
480  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field035"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sFA"), "", {"03"});
481  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field036"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sFA"), "", {"03"});
482  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field037"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sFA"), "", {"03"});
483  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field038"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sFA"), "", {"03"});
484  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field039"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sFA"), "", {"03"});
485  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field040"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sFA"), "", {"03"});
486  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field041"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sFA"), "", {"03"});
487  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field042"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sFA"), "", {"03"});
488  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field043"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sFA"), "", {"03"});
489  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field044"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sFA"), "", {"03"});
490  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field045"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sFA"), "", {"03"});
491  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field046"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sFA"), "", {"03"});
492  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field047"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sFA"), "", {"03"});
493  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["LMPoutputRange"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "LMPoutputRange", {"000100001388fff922330022550d"});
494  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["LMDscandatacfg"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "LMDscandatacfg", {"01000000000000000000010001"});
495  }
496  if(scanner_type == "sick_lms_111") // overwrite for LMS111
497  {
498  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field000"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field000", {"3f8000000000000000001388fff922300101000000010000000005dc00000000000001f4000001f400000000000100064649454c44310000"});
499  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field001"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field001", {"3f8000000000000000001388fff9223002000000000000000000000100000000"});
500  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field002"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field002", {"3f8000000000000000001388fff9223002000000000000000000000100000000"});
501  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field003"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field003", {"3f8000000000000000001388fff9223002000000000000000000000100000000"});
502  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field004"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field004", {"3f8000000000000000001388fff9223002000000000000000000000100000000"});
503  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field005"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field005", {"3f8000000000000000001388fff9223002000000000000000000000100000000"});
504  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field006"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field006", {"3f8000000000000000001388fff9223002000000000000000000000100000000"});
505  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field007"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field007", {"3f8000000000000000001388fff9223002000000000000000000000100000000"});
506  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field008"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field008", {"3f8000000000000000001388fff9223002000000000000000000000100000000"});
507  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field009"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field009", {"3f8000000000000000001388fff9223002000000000000000000000100000000"});
508  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field010"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field010", {"00"});
509  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field011"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field011", {"00"});
510  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field012"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field012", {"00"});
511  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field013"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field013", {"00"});
512  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field014"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field014", {"00"});
513  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field015"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field015", {"00"});
514  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field016"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field016", {"00"});
515  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field017"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field017", {"00"});
516  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field018"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field018", {"00"});
517  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field019"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field019", {"00"});
518  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field020"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field020", {"00"});
519  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field021"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field021", {"00"});
520  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field022"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field022", {"00"});
521  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field023"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field023", {"00"});
522  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field024"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field024", {"00"});
523  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field025"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field025", {"00"});
524  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field026"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field026", {"00"});
525  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field027"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field027", {"00"});
526  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field028"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field028", {"00"});
527  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["field029"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "field029", {"00"});
528  s_mapped_responses[sick_scan_xd::ColaParser::sMN]["mLMPsetscancfg"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sAN"), "mLMPsetscancfg", {"0000001388000100001388fff9223000225510"});
529  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["LMPscancfg"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "LMPscancfg", {"00001388000100001388fff9223000225510"});
530  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["LMPoutputRange"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "LMPoutputRange", {"000100001388fff922330022550d"});
531  }
532  if(scanner_type == "sick_mrs_6xxx") // overwrite for MRS6124
533  {
534  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["DeviceIdent"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "DeviceIdent", {"00084d52533631323452000a312e312e302e36333452"});
535  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["FirmwareVersion"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "FirmwareVersion", {"000a312e312e302e36333452"});
536  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["ODoprh"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "ODoprh", {"00000874"});
537  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["ODpwrc"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "ODpwrc", {"0000001d"});
538  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["LocationName"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "LocationName", {"000b6e6f7420646566696e6564"});
539  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["LMPoutputRange"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "LMPoutputRange", {"000100000515000494510016e2ef"});
540  s_mapped_responses[sick_scan_xd::ColaParser::sRN]["LMDscandatacfg"] = sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sRA"), "LMDscandatacfg", {"1f000001000000000000010001"});
541  }
542  s_mapped_responses_initialized = true;
543  }
544 
545  // Return response from static table, if request can be found in s_mapped_responses
546  for(std::map<sick_scan_xd::ColaParser::COLA_SOPAS_COMMAND, std::map<std::string, sick_scan_xd::SickLocColaTelegramMsg>>::iterator iter_cmd = s_mapped_responses.begin(); iter_cmd != s_mapped_responses.end(); iter_cmd++)
547  {
548  if(cola_request.command_type == iter_cmd->first)
549  {
550  std::map<std::string, sick_scan_xd::SickLocColaTelegramMsg>& mapped_response = iter_cmd->second;
551  if(mapped_response.find(cola_request.command_name) != mapped_response.end())
552  {
553  return mapped_response[cola_request.command_name];
554  }
555  }
556  }
557 
558  // Generate a synthetical response to LocRequestTimestamp requests: "sAN LocRequestTimestamp <timestamp>" with uint32_t timestamp in hex and ms
559  if(cola_request.command_type == sick_scan_xd::ColaParser::sMN && cola_request.command_name == "LocRequestTimestamp")
560  {
561  static sick_scan_xd::UniformRandomInteger time_jitter_network_ms(0, 2);
562  // Simulate some network latency
563  ROS::sleep(0.001 * time_jitter_network_ms.generate());
564  // Create current timestamp in ticks
565  uint32_t ticks_ms = createTimestampTicksMilliSec();
566  // Simulate some network latency
567  ROS::sleep(0.001 * time_jitter_network_ms.generate());
568  return sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::sAN, cola_request.command_name, {hexstr(ticks_ms)});
569  }
570 
571  // Set settings from Configuration Telegrams
572  if(cola_request.command_type == sick_scan_xd::ColaParser::sMN && cola_request.command_name == "LMCstartmeas")
573  {
574  s_controller_settings["LMCstartmeas"] = 1;
575  return sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sAN"), "LMCstartmeas", {"00"});
576  }
577  if(cola_request.command_type == sick_scan_xd::ColaParser::sMN && cola_request.command_name == "LMCstopmeas")
578  {
579  s_controller_settings["LMCstartmeas"] = 0;
580  return sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sAN"), "LMCstopmeas", {"00"});
581  }
582  if(cola_request.command_type == sick_scan_xd::ColaParser::sEN && cola_request.command_name == "LMDradardata")
583  {
584  s_controller_settings["LMCstartmeas"] = 1;
585  return sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::convertSopasCommand("sEA"), "LMDradardata", {"01"});
586  }
587  if(cola_request.command_type == sick_scan_xd::ColaParser::sMN && cola_request.command_name == "LocStartLocalizing")
588  {
589  s_controller_settings["LocState"] = 2;
590  return sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::sAN, cola_request.command_name, {decstr(1)});
591  }
592  if(cola_request.command_type == sick_scan_xd::ColaParser::sMN && cola_request.command_name == "LocStop")
593  {
594  s_controller_settings["LocState"] = 1;
595  return sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::sAN, cola_request.command_name, {decstr(1)});
596  }
597  if(cola_request.command_type == sick_scan_xd::ColaParser::sMN && cola_request.command_name == "LocStopAndSave")
598  {
599  s_controller_settings["LocState"] = 1;
600  return sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::sAN, cola_request.command_name, {decstr(1)});
601  }
602  if(cola_request.command_type == sick_scan_xd::ColaParser::sMN && cola_request.command_name == "LocSetResultPort" && cola_request.parameter.size() == 1)
603  {
604  s_controller_settings["LocResultPort"] = std::strtol(cola_request.parameter[0].c_str(), 0, 0);
605  return sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::sAN, cola_request.command_name, {decstr(1), decstr(1)});
606  }
607  if(cola_request.command_type == sick_scan_xd::ColaParser::sMN && cola_request.command_name == "LocSetResultMode" && cola_request.parameter.size() == 1)
608  {
609  s_controller_settings["LocResultMode"] = std::strtol(cola_request.parameter[0].c_str(), 0, 0);
610  return sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::sAN, cola_request.command_name, {decstr(1)});
611  }
612  if(cola_request.command_type == sick_scan_xd::ColaParser::sMN && cola_request.command_name == "LocSetResultPoseEnabled" && cola_request.parameter.size() == 1)
613  {
614  s_controller_settings["LocResultState"] = std::strtol(cola_request.parameter[0].c_str(), 0, 0);
615  return sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::sAN, cola_request.command_name, {decstr(1)});
616  }
617  if(cola_request.command_type == sick_scan_xd::ColaParser::sMN && cola_request.command_name == "LocSetResultEndianness" && cola_request.parameter.size() == 1)
618  {
619  s_controller_settings["LocResultEndianness"] = std::strtol(cola_request.parameter[0].c_str(), 0, 0);;
620  return sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::sAN, cola_request.command_name, {decstr(1)});
621  }
622 
623  if(cola_request.command_type == sick_scan_xd::ColaParser::sMN && cola_request.command_name == "LocSetPose" && cola_request.parameter.size() == 4)
624  {
625  int32_t posex_mm = std::strtol(cola_request.parameter[0].c_str(), 0, 0);
626  int32_t posey_mm = std::strtol(cola_request.parameter[1].c_str(), 0, 0);
627  int32_t yaw_mdeg = std::strtol(cola_request.parameter[2].c_str(), 0, 0);
628  int32_t uncertainty = std::strtol(cola_request.parameter[3].c_str(), 0, 0);
629  bool success = (posex_mm >= -300000 && posex_mm <= +300000 && posey_mm >= -300000 && posey_mm <= +300000
630  && yaw_mdeg >= -180000 && yaw_mdeg <= +180000 && uncertainty >= 0 && uncertainty < 300000);
631  return sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::sAN, cola_request.command_name, {decstr(success?1:0)});
632  }
633 
634  if(cola_request.command_type == sick_scan_xd::ColaParser::sMN && cola_request.command_name == "LocSetResultPoseInterval" && cola_request.parameter.size() == 1)
635  {
636  s_u32ResultPoseInterval = std::strtoul(cola_request.parameter[0].c_str(), 0, 0);
637  return sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::sAN, cola_request.command_name, {decstr(1)});
638  }
639 
640  /* Start of test server responses for new service requests (release 4 or later) */
641 
642  if(cola_request.command_type == sick_scan_xd::ColaParser::sMN && cola_request.command_name == "DevSetLidarConfig" && cola_request.parameter.size() == 15)
643  {
644  for(size_t n = 0; n < cola_request.parameter.size(); n++)
645  s_controller_settings_str["DevSetLidarConfig_"+std::to_string(n)] = cola_request.parameter[n];
646  return sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::sAN, cola_request.command_name, {decstr(1), decstr(1)});
647  }
648 
649  if(cola_request.command_type == sick_scan_xd::ColaParser::sMN && cola_request.command_name == "DevGetLidarConfig" && cola_request.parameter.size() == 1)
650  {
651  std::vector<std::string> config_parameter;
652  for(size_t n = 1; n < 15; n++)
653  // if(n != 9)
654  config_parameter.push_back(s_controller_settings_str["DevSetLidarConfig_"+std::to_string(n)]);
655  return sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::sAN, cola_request.command_name, config_parameter);
656  }
657 
658  if(cola_request.command_type == sick_scan_xd::ColaParser::sMN && cola_request.command_name == "LocSetMap" && cola_request.parameter.size() == 2)
659  {
660  s_controller_settings_str["LocSetMap"] = cola_request.parameter[1];
661  return sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::sAN, cola_request.command_name, {decstr(1), decstr(1)});
662  }
663 
664  if(cola_request.command_name == "LocMap")//if(cola_request.command_type == sick_scan_xd::ColaParser::sRN && cola_request.command_name == "LocMap" && cola_request.parameter.size() == 0)
665  {
666  return sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::sRA, cola_request.command_name, {s_controller_settings_str["LocSetMap"]});
667  }
668 
669  if(cola_request.command_type == sick_scan_xd::ColaParser::sRN && cola_request.command_name == "LocMapState" && cola_request.parameter.size() == 0)
670  {
671  return sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::sAN, cola_request.command_name, {decstr(1)});
672  }
673 
674  if(cola_request.command_type == sick_scan_xd::ColaParser::sMN && cola_request.command_name == "LocInitializePose" && cola_request.parameter.size() == 4)
675  {
676  for(size_t n = 0; n < cola_request.parameter.size(); n++)
677  s_controller_settings_str["LocInitializePose_"+std::to_string(n)] = cola_request.parameter[n];
678  return sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::sAN, cola_request.command_name, {decstr(1)});
679  }
680 
681  if(cola_request.command_type == sick_scan_xd::ColaParser::sRN && cola_request.command_name == "LocInitialPose" && cola_request.parameter.size() == 0)
682  {
683  std::vector<std::string> parameter;
684  for(size_t n = 0; n < 4; n++)
685  parameter.push_back(s_controller_settings_str["LocInitializePose_"+std::to_string(n)]);
686  return sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::sAN, cola_request.command_name, parameter);
687  }
688 
689  if(cola_request.command_type == sick_scan_xd::ColaParser::sMN && cola_request.command_name == "LocSetPoseQualityCovWeight" && cola_request.parameter.size() == 1)
690  {
691  s_controller_settings_str["LocSetPoseQualityCovWeight"] = cola_request.parameter[0];
692  return sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::sAN, cola_request.command_name, {decstr(1)});
693  }
694 
695  if(cola_request.command_type == sick_scan_xd::ColaParser::sRN && cola_request.command_name == "LocPoseQualityCovWeight" && cola_request.parameter.size() == 0)
696  {
697  return sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::sAN, cola_request.command_name, {s_controller_settings_str["LocSetPoseQualityCovWeight"]});
698  }
699 
700  if(cola_request.command_type == sick_scan_xd::ColaParser::sMN && cola_request.command_name == "LocSetPoseQualityMeanDistWeight" && cola_request.parameter.size() == 1)
701  {
702  s_controller_settings_str["LocSetPoseQualityMeanDistWeight"] = cola_request.parameter[0];
703  return sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::sAN, cola_request.command_name, {decstr(1)});
704  }
705 
706  if(cola_request.command_type == sick_scan_xd::ColaParser::sRN && cola_request.command_name == "LocPoseQualityMeanDistWeight" && cola_request.parameter.size() == 0)
707  {
708  return sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::sAN, cola_request.command_name, {s_controller_settings_str["LocSetPoseQualityMeanDistWeight"]});
709  }
710 
711  if(cola_request.command_type == sick_scan_xd::ColaParser::sMN && cola_request.command_name == "LocSetReflectorsForSupportActive" && cola_request.parameter.size() == 1)
712  {
713  s_controller_settings_str["LocSetReflectorsForSupportActive"] = cola_request.parameter[0];
714  return sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::sAN, cola_request.command_name, {decstr(1)});
715  }
716 
717  if(cola_request.command_type == sick_scan_xd::ColaParser::sRN && cola_request.command_name == "LocReflectorsForSupportActive" && cola_request.parameter.size() == 0)
718  {
719  return sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::sAN, cola_request.command_name, {s_controller_settings_str["LocSetReflectorsForSupportActive"]});
720  }
721 
722  if(cola_request.command_type == sick_scan_xd::ColaParser::sMN && cola_request.command_name == "LocSetOdometryActive" && cola_request.parameter.size() == 1)
723  {
724  s_controller_settings_str["LocSetOdometryActive"] = cola_request.parameter[0];
725  return sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::sAN, cola_request.command_name, {decstr(1), decstr(1)});
726  }
727 
728  if(cola_request.command_type == sick_scan_xd::ColaParser::sRN && cola_request.command_name == "LocOdometryActive" && cola_request.parameter.size() == 0)
729  {
730  return sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::sAN, cola_request.command_name, {s_controller_settings_str["LocSetOdometryActive"]});
731  }
732 
733  if(cola_request.command_type == sick_scan_xd::ColaParser::sMN && cola_request.command_name == "LocSetOdometryPort" && cola_request.parameter.size() == 1)
734  {
735  s_controller_settings_str["LocSetOdometryPort"] = cola_request.parameter[0];
736  return sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::sAN, cola_request.command_name, {decstr(1), decstr(1)});
737  }
738 
739  if(cola_request.command_type == sick_scan_xd::ColaParser::sRN && cola_request.command_name == "LocOdometryPort" && cola_request.parameter.size() == 0)
740  {
741  return sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::sAN, cola_request.command_name, {s_controller_settings_str["LocSetOdometryPort"]});
742  }
743 
744  if(cola_request.command_type == sick_scan_xd::ColaParser::sMN && cola_request.command_name == "LocSetOdometryRestrictYMotion" && cola_request.parameter.size() == 1)
745  {
746  s_controller_settings_str["LocSetOdometryRestrictYMotion"] = cola_request.parameter[0];
747  return sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::sAN, cola_request.command_name, {decstr(1)});
748  }
749 
750  if(cola_request.command_type == sick_scan_xd::ColaParser::sRN && cola_request.command_name == "LocOdometryRestrictYMotion" && cola_request.parameter.size() == 0)
751  {
752  return sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::sAN, cola_request.command_name, {s_controller_settings_str["LocSetOdometryRestrictYMotion"]});
753  }
754 
755  if(cola_request.command_type == sick_scan_xd::ColaParser::sMN && cola_request.command_name == "LocSetAutoStartActive" && cola_request.parameter.size() == 1)
756  {
757  s_controller_settings_str["LocSetAutoStartActive"] = cola_request.parameter[0];
758  return sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::sAN, cola_request.command_name, {decstr(1)});
759  }
760 
761  if(cola_request.command_type == sick_scan_xd::ColaParser::sRN && cola_request.command_name == "LocAutoStartActive" && cola_request.parameter.size() == 0)
762  {
763  return sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::sAN, cola_request.command_name, {s_controller_settings_str["LocSetAutoStartActive"]});
764  }
765 
766  if(cola_request.command_type == sick_scan_xd::ColaParser::sMN && cola_request.command_name == "LocSetAutoStartSavePoseInterval" && cola_request.parameter.size() == 1)
767  {
768  s_controller_settings_str["LocSetAutoStartSavePoseInterval"] = cola_request.parameter[0];
769  return sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::sAN, cola_request.command_name, {decstr(1)});
770  }
771 
772  if(cola_request.command_type == sick_scan_xd::ColaParser::sRN && cola_request.command_name == "LocAutoStartSavePoseInterval" && cola_request.parameter.size() == 0)
773  {
774  return sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::sAN, cola_request.command_name, {s_controller_settings_str["LocSetAutoStartSavePoseInterval"]});
775  }
776 
777  if(cola_request.command_type == sick_scan_xd::ColaParser::sMN && cola_request.command_name == "LocSetRingBufferRecordingActive" && cola_request.parameter.size() == 1)
778  {
779  s_controller_settings_str["LocSetRingBufferRecordingActive"] = cola_request.parameter[0];
780  return sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::sAN, cola_request.command_name, {decstr(1)});
781  }
782 
783  if(cola_request.command_type == sick_scan_xd::ColaParser::sRN && cola_request.command_name == "LocRingBufferRecordingActive" && cola_request.parameter.size() == 0)
784  {
785  return sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::sAN, cola_request.command_name, {s_controller_settings_str["LocSetRingBufferRecordingActive"]});
786  }
787 
788  if(cola_request.command_type == sick_scan_xd::ColaParser::sMN && cola_request.command_name == "DevGetLidarIdent" && cola_request.parameter.size() == 1)
789  {
790  return sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::sAN, cola_request.command_name, {"TestcaseGenerator" + cola_request.parameter[0]});
791  }
792 
793  if(cola_request.command_type == sick_scan_xd::ColaParser::sMN && cola_request.command_name == "DevGetLidarState" && cola_request.parameter.size() == 1)
794  {
795  return sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::sAN, cola_request.command_name, {decstr(2), decstr(2), decstr(2)});
796  }
797 
798  if(cola_request.command_type == sick_scan_xd::ColaParser::sMN && cola_request.command_name == "GetSoftwareVersion" && cola_request.parameter.size() == 0)
799  {
800  return sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::sAN, cola_request.command_name, {"1.0"});
801  }
802 
803  if(cola_request.command_type == sick_scan_xd::ColaParser::sMN && cola_request.command_name == "LocAutoStartSavePose" && cola_request.parameter.size() == 0)
804  {
805  return sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::sAN, cola_request.command_name, {decstr(1)});
806  }
807 
808  if(cola_request.command_type == sick_scan_xd::ColaParser::sMN && cola_request.command_name == "LocForceUpdate" && cola_request.parameter.size() == 0)
809  {
810  return sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::sAN, cola_request.command_name, {decstr(1)});
811  }
812 
813  if(cola_request.command_type == sick_scan_xd::ColaParser::sMN && cola_request.command_name == "LocSaveRingBufferRecording" && cola_request.parameter.size() == 2)
814  {
815  return sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::sAN, cola_request.command_name, {decstr(2)});
816  }
817 
818  if(cola_request.command_type == sick_scan_xd::ColaParser::sMN && cola_request.command_name == "LocStartDemoMapping" && cola_request.parameter.size() == 0)
819  {
820  return sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::sAN, cola_request.command_name, {decstr(1)});
821  }
822 
823  if(cola_request.command_type == sick_scan_xd::ColaParser::sMN && cola_request.command_name == "ReportUserMessage" && cola_request.parameter.size() == 2)
824  {
825  return sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::sAN, cola_request.command_name, {decstr(1)});
826  }
827 
828  if(cola_request.command_type == sick_scan_xd::ColaParser::sMN && cola_request.command_name == "SavePermanent" && cola_request.parameter.size() == 0)
829  {
830  return sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::sAN, cola_request.command_name, {decstr(1)});
831  }
832 
833  if(cola_request.command_type == sick_scan_xd::ColaParser::sRN && cola_request.command_name == "LocResultPort" && cola_request.parameter.size() == 0)
834  {
835  int32_t port = ((s_controller_settings["LocResultPort"]) > 0 ? s_controller_settings["LocResultPort"] : 2201);
836  return sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::sRA, cola_request.command_name, {hexstr(port)});
837  }
838 
839  if(cola_request.command_type == sick_scan_xd::ColaParser::sRN && cola_request.command_name == "LocResultMode" && cola_request.parameter.size() == 0)
840  {
841  return sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::sRA, cola_request.command_name, {decstr(s_controller_settings["LocResultMode"])});
842  }
843 
844  if(cola_request.command_type == sick_scan_xd::ColaParser::sRN && cola_request.command_name == "LocResultEndianness" && cola_request.parameter.size() == 0)
845  {
846  return sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::sRA, cola_request.command_name, {decstr(s_controller_settings["LocResultEndianness"])});
847  }
848 
849  if(cola_request.command_type == sick_scan_xd::ColaParser::sRN && cola_request.command_name == "LocResultState" && cola_request.parameter.size() == 0)
850  {
851  return sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::sRA, cola_request.command_name, {decstr(s_controller_settings["LocResultState"])});
852  }
853 
854  if(cola_request.command_type == sick_scan_xd::ColaParser::sRN && cola_request.command_name == "LocResultPoseInterval" && cola_request.parameter.size() == 0)
855  {
856  return sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::sRA, cola_request.command_name, {decstr(s_u32ResultPoseInterval)});
857  }
858 
859  if(cola_request.command_type == sick_scan_xd::ColaParser::sMN && cola_request.command_name == "DevSetIMUActive" && cola_request.parameter.size() == 1)
860  {
861  s_controller_settings_str["DevSetIMUActive"] = cola_request.parameter[0];
862  return sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::sAN, cola_request.command_name, {decstr(1)});
863  }
864 
865  if(cola_request.command_type == sick_scan_xd::ColaParser::sRN && cola_request.command_name == "DevIMUActive" && cola_request.parameter.size() == 0)
866  {
867  return sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::sRA, cola_request.command_name, {s_controller_settings_str["DevSetIMUActive"]});
868  }
869 
870  /* End of test server responses for new service requests (release 4 or later) */
871 
872  // Create sAN responses to sMN requests resp. sRA responses to sRN requests
873  if(cola_request.command_type == sick_scan_xd::ColaParser::sMN || cola_request.command_type == sick_scan_xd::ColaParser::sRN)
874  {
875  sick_scan_xd::ColaParser::COLA_SOPAS_COMMAND response_type = sick_scan_xd::ColaParser::sINVALID;
876  if(cola_request.command_type == sick_scan_xd::ColaParser::sMN)
877  response_type = sick_scan_xd::ColaParser::sAN; // sAN responses to sMN requests
878  else if(cola_request.command_type == sick_scan_xd::ColaParser::sRN)
879  response_type = sick_scan_xd::ColaParser::sRA; // sRA responses to sRN requests
880  for(std::map<std::string, int32_t>::iterator iter_settings = s_controller_settings.begin(); iter_settings != s_controller_settings.end(); iter_settings++)
881  {
882  if(cola_request.command_name == iter_settings->first)
883  {
884  return sick_scan_xd::ColaParser::createColaTelegram(response_type, cola_request.command_name, {hexstr(iter_settings->second)});
885  }
886  }
887  }
888 
889  // Default response: "sAN <command_name>" without parameter (sAN: Response to sMN)
890  return sick_scan_xd::ColaParser::createColaTelegram(sick_scan_xd::ColaParser::sAN, cola_request.command_name);
891 }
892 
899 uint32_t sick_scan_xd::TestcaseGenerator::createTimestampTicksMilliSec(void)
900 {
901  static ROS::Time start = ROS::now();
902  static sick_scan_xd::UniformRandomInteger time_jitter_ticks_ms(-2, +2);
903  // Create current timestamp in ticks
904  ROS::Duration timestamp = (ROS::now() - start);
905  uint32_t seconds = 0, nanoseconds = 0;
906  ROS::splitTime(timestamp, seconds, nanoseconds);
907  uint32_t ticks_ms = (((uint64_t)seconds * 1000 + (uint64_t)nanoseconds/1000000 + 1000) & 0xFFFFFFFF);
908  // Create some jitter, simulation network latency and time drift
909  ticks_ms += time_jitter_ticks_ms.generate();
910  return ticks_ms;
911 }
sick_scan_xd::SickLocResultPortHeaderMsg_::serialnumber
_serialnumber_type serialnumber
Definition: SickLocResultPortHeaderMsg.h:69
std_msgs::Header_::stamp
_stamp_type stamp
Definition: Header.h:45
sick_scan_xd::Utils::toHexString
static std::string toHexString(const std::vector< uint8_t > &binary_data)
Definition: utils.cpp:69
sick_scan_xd::SickLocResultPortPayloadMsg_::reserved3
_reserved3_type reserved3
Definition: SickLocResultPortPayloadMsg.h:103
sick_scan_xd::TestcaseGenerator::createColaResponse
static sick_scan_xd::SickLocColaTelegramMsg createColaResponse(const sick_scan_xd::SickLocColaTelegramMsg &cola_request, const std::string &scanner_type)
Definition: testcase_generator.cpp:261
ROS::sleep
void sleep(double seconds)
Definition: ros_wrapper.cpp:110
sick_scan_xd::TestcaseGenerator::createDefaultResultPortTestcase
static sick_scan_xd::SickLocResultPortTestcaseMsg createDefaultResultPortTestcase(void)
Definition: testcase_generator.cpp:121
sick_scan_xd::SickLocResultPortTestcaseMsg_::header
_header_type header
Definition: SickLocResultPortTestcaseMsg.h:44
sick_scan_xd::SickLocResultPortPayloadMsg_::quality
_quality_type quality
Definition: SickLocResultPortPayloadMsg.h:88
sick_scan_xd::SickLocResultPortPayloadMsg_::poseyaw
_poseyaw_type poseyaw
Definition: SickLocResultPortPayloadMsg.h:79
sick_scan_xd::ColaParser::createColaTelegram
static sick_scan_xd::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:99
sick_scan_xd::SickLocResultPortPayloadMsg_::reserved1
_reserved1_type reserved1
Definition: SickLocResultPortPayloadMsg.h:82
utils.h
sick_scan_xd::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:167
sick_scan_xd::SickLocResultPortTelegramMsg_< std::allocator< void > >
sick_scan_xd::TestcaseGenerator::createRandomResultPortTestcase
static sick_scan_xd::SickLocResultPortTestcaseMsg createRandomResultPortTestcase(void)
Definition: testcase_generator.cpp:158
ROS::splitTime
void splitTime(ROS::Duration time, uint32_t &seconds, uint32_t &nanoseconds)
Definition: ros_wrapper.cpp:128
sick_scan_xd::SickLocResultPortPayloadMsg_::covariancex
_covariancex_type covariancex
Definition: SickLocResultPortPayloadMsg.h:94
sick_scan_xd::SickLocColaTelegramMsg_
Definition: SickLocColaTelegramMsg.h:24
sick_scan_xd::SickLocResultPortPayloadMsg_::posex
_posex_type posex
Definition: SickLocResultPortPayloadMsg.h:73
sick_scan_xd::SickLocResultPortHeaderMsg_::systemtime
_systemtime_type systemtime
Definition: SickLocResultPortHeaderMsg.h:78
sick_scan_xd::SickLocColaTelegramMsg_::command_name
_command_name_type command_name
Definition: SickLocColaTelegramMsg.h:51
sick_scan_xd::ColaParser::sRA
@ sRA
Read by name (response)
Definition: cola_parser.h:97
sick_scan_xd::ResultPortParser
Definition: result_port_parser.h:68
sick_scan_xd::Utils::normalizeAngle
static double normalizeAngle(double angle)
Definition: test/emulator/include/sick_scan/utils.h:273
sick_scan_xd::SickLocResultPortPayloadMsg_::scancounter
_scancounter_type scancounter
Definition: SickLocResultPortPayloadMsg.h:67
sick_scan_xd::SickLocResultPortTestcaseMsg_::binary_data
_binary_data_type binary_data
Definition: SickLocResultPortTestcaseMsg.h:47
sick_scan_xd::TestcaseGenerator::ResultTelegramsEnabled
static bool ResultTelegramsEnabled(void)
Definition: testcase_generator.cpp:112
sick_scan_xd::ColaParser::sWN
@ sWN
Write by name (request)
Definition: cola_parser.h:101
sick_scan_xd::TestcaseGenerator::createResultPortCircles
static sick_scan_xd::SickLocResultPortTestcaseMsg createResultPortCircles(double circle_radius, double circle_yaw)
Definition: testcase_generator.cpp:220
ROS::now
ROS::Time now(void)
Definition: ros_wrapper.cpp:116
sick_scan_xd::SickLocResultPortPayloadMsg_::reserved2
_reserved2_type reserved2
Definition: SickLocResultPortPayloadMsg.h:85
sick_scan_xd::SickLocResultPortTestcaseMsg_
Definition: SickLocResultPortTestcaseMsg.h:25
std_msgs::Duration
::std_msgs::Duration_< std::allocator< void > > Duration
Definition: Duration.h:48
sick_scan_xd::SickLocResultPortPayloadMsg_::timestamp
_timestamp_type timestamp
Definition: SickLocResultPortPayloadMsg.h:70
sick_scan_xd::ResultPortParser::getTelegramMsg
virtual sick_scan_xd::SickLocResultPortTelegramMsg & getTelegramMsg(void)
Definition: result_port_parser.h:94
imu_delay_tester.timestamp
timestamp
Definition: imu_delay_tester.py:142
sick_scan_xd::ColaParser::sINVALID
@ sINVALID
uninitialized, command_type should never have this value)
Definition: cola_parser.h:95
sick_scan_xd::ColaParser::sRN
@ sRN
Read by name (request)
Definition: cola_parser.h:96
sick_scan_xd::ColaParser::COLA_SOPAS_COMMAND
enum sick_scan_xd::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_xd::TestcaseGenerator::LocalizationEnabled
static bool LocalizationEnabled(void)
Definition: testcase_generator.cpp:103
testcase_generator.h
sick_scan_xd::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:168
sick_scan_xd::SickLocColaTelegramMsg_::command_type
_command_type_type command_type
Definition: SickLocColaTelegramMsg.h:48
std_msgs::Time
::std_msgs::Time_< std::allocator< void > > Time
Definition: Time.h:48
sick_scan_xd::Utils::flattenToString
static std::string flattenToString(const T &x)
Definition: test/emulator/include/sick_scan/utils.h:157
sick_scan_xd::SickLocResultPortTestcaseMsg_::telegram_msg
_telegram_msg_type telegram_msg
Definition: SickLocResultPortTestcaseMsg.h:50
sick_scan_xd::SickLocResultPortPayloadMsg_::covariancey
_covariancey_type covariancey
Definition: SickLocResultPortPayloadMsg.h:97
sick_scan_xd::SickLocResultPortPayloadMsg_::covarianceyaw
_covarianceyaw_type covarianceyaw
Definition: SickLocResultPortPayloadMsg.h:100
std_msgs::Header_::frame_id
_frame_id_type frame_id
Definition: Header.h:48
sick_scan_xd::SickLocResultPortTelegramMsg_::telegram_payload
_telegram_payload_type telegram_payload
Definition: SickLocResultPortTelegramMsg.h:60
sick_scan_xd::SickLocResultPortHeaderMsg_::payloadtype
_payloadtype_type payloadtype
Definition: SickLocResultPortHeaderMsg.h:60
sick_scan_xd::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:218
sick_scan_xd::ColaParser::sEN
@ sEN
Event by name (request)
Definition: cola_parser.h:103
sick_scan_xd::SickLocResultPortHeaderMsg_::fw_version
_fw_version_type fw_version
Definition: SickLocResultPortHeaderMsg.h:72
sick_scan_xd::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:169
ROS::seconds
double seconds(ROS::Duration duration)
Definition: ros_wrapper.cpp:180
sick_scan_xd::TestcaseGenerator::SendScandataEnabled
static bool SendScandataEnabled(void)
Definition: testcase_generator.cpp:94
sick_scan_xd::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(...)
Definition: sick_scan_ros2_example.cpp:72
sick_scan_xd::TestcaseGenerator::createTimestampTicksMilliSec
static uint32_t createTimestampTicksMilliSec(void)
Definition: testcase_generator.cpp:899
sick_scan_xd::SickLocResultPortPayloadMsg_::outliersratio
_outliersratio_type outliersratio
Definition: SickLocResultPortPayloadMsg.h:91
sick_scan_xd::SickLocColaTelegramMsg_::parameter
_parameter_type parameter
Definition: SickLocColaTelegramMsg.h:54
sick_scan_xd::SickLocResultPortHeaderMsg_::ordernumber
_ordernumber_type ordernumber
Definition: SickLocResultPortHeaderMsg.h:66
sick_scan_xd::ResultPortParser::encode
virtual std::vector< uint8_t > encode(void)
Definition: result_port_parser.cpp:429
ros_wrapper.h
sick_scan_xd::UniformRandomInteger
Definition: random_generator.h:67
cola_parser.h
roswrap::start
ROSCPP_DECL void start()
Actually starts the internals of the node (spins up threads, starts the network polling and xmlrpc lo...
sick_scan_xd::SickLocResultPortTelegramMsg_::telegram_header
_telegram_header_type telegram_header
Definition: SickLocResultPortTelegramMsg.h:57
sick_scan_xd::ColaParser::sAN
@ sAN
Response to sMN.
Definition: cola_parser.h:99
sick_scan_xd::SickLocResultPortPayloadMsg_::posey
_posey_type posey
Definition: SickLocResultPortPayloadMsg.h:76
sick_scan_xd::ColaParser::sMN
@ sMN
Method by name (request)
Definition: cola_parser.h:98
sick_scan_xd::SickLocResultPortHeaderMsg_::telegramcounter
_telegramcounter_type telegramcounter
Definition: SickLocResultPortHeaderMsg.h:75
sick_scan_xd::UniformRandomInteger::generate
int generate(void)
Definition: random_generator.cpp:75
random_generator.h

sick_scan_xd
Author(s): Michael Lehning , Jochen Sprickerhof , Martin Günther
autogenerated on Fri Oct 25 2024 02:47:12