test_server_thread.cpp
Go to the documentation of this file.
1 /*
2  * @brief sim_loc_test_server_thread implements a simple tcp server thread,
3  * simulating a localization controller for unittests. It runs a thread to listen
4  * and accept tcp connections from clients and generates telegrams to test
5  * the ros driver for sim localization.
6  *
7  * Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim
8  * Copyright (C) 2019 SICK AG, Waldkirch
9  *
10  * Licensed under the Apache License, Version 2.0 (the "License");
11  * you may not use this file except in compliance with the License.
12  * You may obtain a copy of the License at
13  *
14  * http://www.apache.org/licenses/LICENSE-2.0
15  *
16  * Unless required by applicable law or agreed to in writing, software
17  * distributed under the License is distributed on an "AS IS" BASIS,
18  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
19  * See the License for the specific language governing permissions and
20  * limitations under the License.
21  *
22  * All rights reserved.
23  *
24  * Redistribution and use in source and binary forms, with or without
25  * modification, are permitted provided that the following conditions are met:
26  *
27  * * Redistributions of source code must retain the above copyright
28  * notice, this list of conditions and the following disclaimer.
29  * * Redistributions in binary form must reproduce the above copyright
30  * notice, this list of conditions and the following disclaimer in the
31  * documentation and/or other materials provided with the distribution.
32  * * Neither the name of SICK AG nor the names of its
33  * contributors may be used to endorse or promote products derived from
34  * this software without specific prior written permission
35  * * Neither the name of Ing.-Buero Dr. Michael Lehning nor the names of its
36  * contributors may be used to endorse or promote products derived from
37  * this software without specific prior written permission
38  *
39  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
40  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
41  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
42  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
43  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
44  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
45  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
46  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
47  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
48  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
49  * POSSIBILITY OF SUCH DAMAGE.
50  *
51  * Authors:
52  * Michael Lehning <michael.lehning@lehning.de>
53  *
54  * Copyright 2019 SICK AG
55  * Copyright 2019 Ing.-Buero Dr. Michael Lehning
56  *
57  */
58 #include "sick_scan/ros_wrapper.h"
59 
60 #include "sick_scan/cola_parser.h"
66 #include "sick_scan/utils.h"
67 
74 sick_scan::TestServerThread::TestServerThread(ROS::NodePtr nh, int ip_port_results, int ip_port_cola)
75 : m_ip_port_results(ip_port_results), m_ip_port_cola(ip_port_cola), m_ioservice(),
76  m_tcp_connection_thread_results(0), m_tcp_connection_thread_cola(0), m_tcp_send_scandata_thread(0),
77  m_tcp_connection_thread_running(false), m_worker_thread_running(false), m_tcp_send_scandata_thread_running(false),
78  m_tcp_acceptor_results(m_ioservice, boost::asio::ip::tcp::endpoint(boost::asio::ip::tcp::v4(), m_ip_port_results)),
79  m_tcp_acceptor_cola(m_ioservice, boost::asio::ip::tcp::endpoint(boost::asio::ip::tcp::v4(), m_ip_port_cola)),
80  m_start_scandata_delay(1), m_result_telegram_rate(10), m_demo_move_in_circles(false), m_error_simulation_enabled(false), m_error_simulation_flag(NO_ERROR),
81  m_error_simulation_thread(0), m_error_simulation_thread_running(false)
82 {
83  m_tcp_acceptor_results.set_option(boost::asio::ip::tcp::acceptor::reuse_address(true));
84  m_tcp_acceptor_cola.set_option(boost::asio::ip::tcp::acceptor::reuse_address(true));
85  if(nh)
86  {
87 
88  ROS::param<std::string>(nh, "/sick_scan_emulator/scandatafiles", m_scandatafiles, m_scandatafiles); // comma separated list of jsonfiles to emulate scandata messages, f.e. "tim781s_scandata.pcapng.json,tim781s_sopas.pcapng.json"
89  ROS::param<std::string>(nh, "/sick_scan_emulator/scandatatypes", m_scandatatypes, m_scandatatypes); // comma separated list of scandata message types, f.e. "sSN LMDscandata,sSN LMDscandatamon"
90  ROS::param<std::string>(nh, "/sick_scan_emulator/scanner_type", m_scanner_type, m_scanner_type); // currently supported: "sick_lms_5xx", "sick_tim_7xx"
91  ROS::param<double>(nh, "/sick_scan/test_server/start_scandata_delay", m_start_scandata_delay, m_start_scandata_delay); // delay between scandata activation ("LMCstartmeas" request) and first scandata message, default: 1 second
92  std::string result_testcases_topic = "/sick_scan/test_server/result_testcases"; // default topic to publish testcases with result port telegrams (type SickLocResultPortTestcaseMsg)
93  ROS::param<double>(nh, "/sick_scan/test_server/result_telegrams_rate", m_result_telegram_rate, m_result_telegram_rate);
94  ROS::param<std::string>(nh, "/sick_scan/test_server/result_testcases_topic", result_testcases_topic, result_testcases_topic);
95  ROS::param<std::string>(nh, "/sick_scan/test_server/result_testcases_frame_id", m_result_testcases_frame_id, "result_testcases");
96  ROS::param<bool>(nh, "/sim_loc_test_server/demo_circles", m_demo_move_in_circles, m_demo_move_in_circles);
97  ROS::param<bool>(nh, "/sim_loc_test_server/error_simulation", m_error_simulation_enabled, m_error_simulation_enabled);
98  std::string sim_loc_test_server_demo_circles, sim_loc_test_server_error_simulation;
99  ROS::param<std::string>(nh, "/system/test_server/demo_circles", sim_loc_test_server_demo_circles, sim_loc_test_server_demo_circles);
100  ROS::param<std::string>(nh, "/system/test_server/error_simulation", sim_loc_test_server_error_simulation, sim_loc_test_server_error_simulation);
101  if(!sim_loc_test_server_demo_circles.empty())
102  m_demo_move_in_circles = std::atoi(sim_loc_test_server_demo_circles.c_str()) > 0;
103  if(!sim_loc_test_server_error_simulation.empty())
104  m_error_simulation_enabled = std::atoi(sim_loc_test_server_error_simulation.c_str()) > 0;
105  // ros publisher for testcases with result port telegrams (type SickLocResultPortTestcaseMsg)
106  m_result_testcases_publisher = ROS_CREATE_PUBLISHER(nh, sick_scan::SickLocResultPortTestcaseMsg, result_testcases_topic);
107  }
108 }
109 
114 {
115  stop();
116 }
117 
123 {
124  if(m_error_simulation_enabled)
125  {
126  ROS_WARN_STREAM("TestServerThread: running in error simulation mode.");
127  ROS_WARN_STREAM("Test server will intentionally react incorrect and will send false data and produce communication errors.");
128  ROS_WARN_STREAM("Running test server in error simulation mode is for test purposes only. Do not expect typical results.");
129  m_error_simulation_thread_running = true;
130  m_error_simulation_thread = new boost::thread(&sick_scan::TestServerThread::runErrorSimulationThreadCb, this);
131  }
132  else if(m_demo_move_in_circles)
133  {
134  ROS_INFO_STREAM("TestServerThread: running in demo mode (moving in circles, no error simulation).");
135  }
136  else
137  {
138  ROS_INFO_STREAM("TestServerThread: running in normal mode (no error simulation).");
139  }
140  // Start and run 3 threads to create sockets for new tcp clients on ip ports 2201 (results), 2111 (requests) and 2112 (responses)
141  m_tcp_connection_thread_running = true;
142  m_tcp_connection_thread_results = new boost::thread(&sick_scan::TestServerThread::runConnectionThreadResultCb, this);
143  m_tcp_connection_thread_cola = new boost::thread(&sick_scan::TestServerThread::runConnectionThreadColaCb, this);
144  return true;
145 }
146 
152 {
153  m_tcp_connection_thread_running = false;
154  m_error_simulation_thread_running = false;
155  m_worker_thread_running = false;
156  if(m_error_simulation_thread)
157  {
158  m_error_simulation_thread->join();
159  delete(m_error_simulation_thread);
160  m_error_simulation_thread = 0;
161  }
162  m_ioservice.stop();
163  // close both tcp acceptors for ip ports 2201 (results), 2111 (cola command requests)
164  std::vector<boost::asio::ip::tcp::acceptor*> tcp_acceptors = { &m_tcp_acceptor_results, & m_tcp_acceptor_cola };
165  for(std::vector<boost::asio::ip::tcp::acceptor*>::iterator iter_acceptor = tcp_acceptors.begin(); iter_acceptor != tcp_acceptors.end(); iter_acceptor++)
166  {
167  if((*iter_acceptor)->is_open())
168  {
169  (*iter_acceptor)->cancel(); // cancel possibly blocking calls of m_tcp_acceptor_results.listen() in the connection thread
170  (*iter_acceptor)->close(); // close tcp acceptor
171  }
172  }
173  // close 2 threads creating sockets for new tcp clients on ip ports 2201 (results), 2111 (cola command requests)
174  std::vector<thread_ptr*> tcp_connection_threads = { &m_tcp_connection_thread_results, &m_tcp_connection_thread_cola };
175  for(std::vector<thread_ptr*>::iterator iter_connection_thread = tcp_connection_threads.begin(); iter_connection_thread != tcp_connection_threads.end(); iter_connection_thread++)
176  {
177  thread_ptr & tcp_connection_thread = **iter_connection_thread;
178  if(tcp_connection_thread)
179  {
180  tcp_connection_thread->join();
181  delete(tcp_connection_thread);
182  tcp_connection_thread = 0;
183  }
184  }
185  // close send scandata thread
186  closeScandataThread();
187  // close sockets
188  closeTcpConnections();
189  // close worker/client threads
190  closeWorkerThreads();
191  return true;
192 }
193 
198 {
199  if(m_tcp_send_scandata_thread)
200  {
201  m_tcp_send_scandata_thread_running = false;
202  m_tcp_send_scandata_thread->join();
203  delete(m_tcp_send_scandata_thread);
204  m_tcp_send_scandata_thread = 0;
205  }
206 }
207 
213 {
214  for(std::list<boost::asio::ip::tcp::socket*>::iterator socket_iter = m_tcp_sockets.begin(); socket_iter != m_tcp_sockets.end(); socket_iter++)
215  {
216  boost::asio::ip::tcp::socket* p_socket = *socket_iter;
217  try
218  {
219  if(p_socket && (force_shutdown || p_socket->is_open()))
220  {
221  p_socket->shutdown(boost::asio::ip::tcp::socket::shutdown_both);
222  p_socket->close();
223  }
224  }
225  catch(std::exception & exc)
226  {
227  ROS_WARN_STREAM("TestServerThread::closeTcpConnections(): exception " << exc.what() << " on closing socket.");
228  }
229  *socket_iter = 0;
230  }
231  m_tcp_sockets.clear();
232 }
233 
239 {
240  boost::lock_guard<boost::mutex> worker_thread_lockguard(m_tcp_worker_threads_mutex);
241  try
242  {
243  if(p_socket && p_socket->is_open())
244  {
245  p_socket->shutdown(boost::asio::ip::tcp::socket::shutdown_both);
246  p_socket->close();
247  }
248  if(p_socket)
249  {
250  for(std::list<boost::asio::ip::tcp::socket*>::iterator socket_iter = m_tcp_sockets.begin(); socket_iter != m_tcp_sockets.end(); )
251  {
252  if(p_socket == *socket_iter)
253  socket_iter = m_tcp_sockets.erase(socket_iter);
254  else
255  socket_iter++;
256  }
257  p_socket = 0;
258  }
259  }
260  catch(std::exception & exc)
261  {
262  ROS_WARN_STREAM("TestServerThread::closeSocket(): exception " << exc.what() << " on closing socket.");
263  }
264 }
265 
270 {
271  m_worker_thread_running = false;
272  boost::lock_guard<boost::mutex> worker_thread_lockguard(m_tcp_worker_threads_mutex);
273  for(std::list<thread_ptr>::iterator thread_iter = m_tcp_worker_threads.begin(); thread_iter != m_tcp_worker_threads.end(); thread_iter++)
274  {
275  boost::thread *p_thread = *thread_iter;
276  p_thread->join();
277  delete(p_thread);
278  }
279  m_tcp_worker_threads.clear();
280 }
281 
287 void sick_scan::TestServerThread::messageCbResultPortTelegrams(const sick_scan::SickLocResultPortTelegramMsg & msg)
288 {
289  m_last_telegram_received.set(msg);
290 }
291 
297 {
298  runConnectionThreadGenericCb(m_tcp_acceptor_results, m_ip_port_results, &sick_scan::TestServerThread::runWorkerThreadResultCb);
299 }
300 
306 {
307  runConnectionThreadGenericCb(m_tcp_acceptor_cola, m_ip_port_cola, &sick_scan::TestServerThread::runWorkerThreadColaCb);
308 }
309 
314 template<typename Callable>void sick_scan::TestServerThread::runConnectionThreadGenericCb(boost::asio::ip::tcp::acceptor & tcp_acceptor_results, int ip_port_results, Callable thread_function_cb)
315 {
316  ROS_INFO_STREAM("TestServerThread: connection thread started");
317  while(ROS::ok() && m_tcp_connection_thread_running && tcp_acceptor_results.is_open())
318  {
319  if(m_error_simulation_flag.get() == DONT_LISTEN) // error simulation: testserver does not open listening port
320  {
321  ROS::sleep(0.1);
322  continue;
323  }
324  // normal mode: listen to tcp port, accept and connect to new tcp clients
325  boost::asio::ip::tcp::socket* tcp_client_socket = new boost::asio::ip::tcp::socket(m_ioservice);
326  boost::system::error_code errorcode;
327  if(m_error_simulation_flag.get() != DONT_LISTEN && m_error_simulation_flag.get() != DONT_ACCECPT)
328  ROS_INFO_STREAM("TestServerThread: listening to tcp connections on port " << ip_port_results);
329  tcp_acceptor_results.listen();
330  if(m_error_simulation_flag.get() == DONT_LISTEN || m_error_simulation_flag.get() == DONT_ACCECPT) // error simulation: testserver does not does not accecpt tcp clients
331  {
332  ROS::sleep(0.1);
333  continue;
334  }
335  tcp_acceptor_results.accept(*tcp_client_socket, errorcode); // normal mode: accept new tcp client
336  if(m_error_simulation_flag.get() == DONT_LISTEN || m_error_simulation_flag.get() == DONT_ACCECPT) // error simulation: testserver does not does not accecpt tcp clients
337  {
338  if(tcp_client_socket->is_open())
339  {
340  tcp_client_socket->shutdown(boost::asio::ip::tcp::socket::shutdown_both);
341  tcp_client_socket->close();
342 
343  }
344  continue;
345  }
346  if (!errorcode && tcp_client_socket->is_open())
347  {
348  // tcp client connected, start worker thread
349  ROS_INFO_STREAM("TestServerThread: established new tcp client connection");
350  boost::lock_guard<boost::mutex> worker_thread_lockguard(m_tcp_worker_threads_mutex);
351  m_tcp_sockets.push_back(tcp_client_socket);
352  m_worker_thread_running = true;
353  m_tcp_worker_threads.push_back(new boost::thread(thread_function_cb, this, tcp_client_socket));
354  }
355  }
356  closeTcpConnections();
357  m_tcp_connection_thread_running = false;
358  ROS_INFO_STREAM("TestServerThread: connection thread finished");
359 }
360 
366 void sick_scan::TestServerThread::runWorkerThreadResultCb(boost::asio::ip::tcp::socket* p_socket)
367 {
368  ROS_INFO_STREAM("TestServerThread: worker thread for result telegrams started");
369  sick_scan::UniformRandomInteger random_generator(0,255);
370  sick_scan::UniformRandomInteger random_length(1, 512);
371  sick_scan::UniformRandomInteger random_integer(0, INT_MAX);
372  double circle_yaw = 0;
373  while(ROS::ok() && m_worker_thread_running && p_socket && p_socket->is_open())
374  {
375  ROS::sleep((double)sick_scan::TestcaseGenerator::ResultPoseInterval() / m_result_telegram_rate);
376  boost::system::error_code error_code;
377  if (m_error_simulation_flag.get() == DONT_SEND) // error simulation: testserver does not send any telegrams
378  {
379  ROS_DEBUG_STREAM("TestServerThread for cresult telegrams: error simulation, server not sending any telegrams");
380  continue;
381  }
382  if (m_error_simulation_flag.get() == SEND_RANDOM_TCP) // error simulation: testserver sends invalid random tcp packets
383  {
384  std::vector<uint8_t> random_data = random_generator.generate(random_length.generate()); // binary random data of random size
385  boost::asio::write(*p_socket, boost::asio::buffer(random_data.data(), random_data.size()), boost::asio::transfer_exactly(random_data.size()), error_code);
386  ROS_DEBUG_STREAM("TestServerThread for result telegrams: send random data " << sick_scan::Utils::toHexString(random_data));
387  continue;
388  }
389  // create testcase is a result port telegram with random based sythetical data
390  sick_scan::SickLocResultPortTestcaseMsg testcase = sick_scan::TestcaseGenerator::createRandomResultPortTestcase();
391  if(m_demo_move_in_circles) // simulate a sensor moving in circles
392  {
393  testcase = sick_scan::TestcaseGenerator::createResultPortCircles(2.0, circle_yaw);
394  circle_yaw = sick_scan::Utils::normalizeAngle(circle_yaw + 1.0 * M_PI / 180);
395  }
396  if (m_error_simulation_flag.get() == SEND_INVALID_TELEGRAMS) // error simulation: testserver sends invalid telegrams (invalid data, false checksums, etc.)
397  {
398  int number_random_bytes = ((random_integer.generate()) % (testcase.binary_data.size()));
399  for(int cnt_random_bytes = 0; cnt_random_bytes < number_random_bytes; cnt_random_bytes++)
400  {
401  int byte_cnt = ((random_integer.generate()) % (testcase.binary_data.size()));
402  testcase.binary_data[byte_cnt] = (uint8_t)(random_generator.generate() & 0xFF);
403  }
404  ROS_DEBUG_STREAM("TestServerThread for result telegrams: send random binary telegram " << sick_scan::Utils::toHexString(testcase.binary_data));
405  }
406  // send binary result port telegram to tcp client (if localization is "on")
408  {
409  size_t bytes_written = boost::asio::write(*p_socket, boost::asio::buffer(testcase.binary_data.data(), testcase.binary_data.size()), boost::asio::transfer_exactly(testcase.binary_data.size()), error_code);
410  if (error_code || bytes_written != testcase.binary_data.size())
411  {
412  std::stringstream error_info;
413  error_info << "## ERROR TestServerThread for result telegrams: failed to send binary result port telegram, " << bytes_written << " of " << testcase.binary_data.size() << " bytes send, error code: " << error_code.message();
414  if (m_error_simulation_flag.get() == NO_ERROR)
415  {
416  ROS_WARN_STREAM(error_info.str() << ", close socket and leave worker thread for result telegrams");
417  break;
418  }
419  ROS_DEBUG_STREAM(error_info.str());
420  }
421  else
422  {
423  ROS_DEBUG_STREAM("TestServerThread for result telegrams: send binary result port telegram " << sick_scan::Utils::toHexString(testcase.binary_data));
424  }
425  // publish testcases (SickLocResultPortTestcaseMsg, i.e. binary telegram and SickLocResultPortTelegramMsg messages) for test and verification of sim_loc_driver
426  testcase.header.stamp = ROS::now();
427  testcase.header.frame_id = m_result_testcases_frame_id;
428  ROS_PUBLISH(m_result_testcases_publisher, testcase);
429  }
430  }
431  closeSocket(p_socket);
432  ROS_INFO_STREAM("TestServerThread: worker thread for result telegrams finished");
433 }
434 
441 void sick_scan::TestServerThread::runWorkerThreadColaCb(boost::asio::ip::tcp::socket* p_socket)
442 {
443  ROS_INFO_STREAM("TestServerThread: worker thread for command requests started");
444  int iTransmitErrorCnt = 0;
445  sick_scan::UniformRandomInteger random_generator(0,255);
446  sick_scan::UniformRandomInteger random_length(1, 128);
448  while(ROS::ok() && m_worker_thread_running && p_socket && p_socket->is_open())
449  {
450  // Read command request from tcp client
451  ServerColaRequest request;
452  ROS::Time receive_timestamp;
453  if(sick_scan::ColaTransmitter::receive(*p_socket, request.telegram_data, 1, receive_timestamp))
454  {
455  if (m_error_simulation_flag.get() == DONT_SEND) // error simulation: testserver does not send any telegrams
456  {
457  ROS_DEBUG_STREAM("TestServerThread for command requests: error simulation, server not sending any telegrams");
458  continue;
459  }
460  if (m_error_simulation_flag.get() == SEND_RANDOM_TCP) // error simulation: testserver sends invalid random tcp packets
461  {
462  boost::system::error_code error_code;
463  std::vector<uint8_t> random_data = random_generator.generate(random_length.generate()); // binary random data of random size
464  boost::asio::write(*p_socket, boost::asio::buffer(random_data.data(), random_data.size()), boost::asio::transfer_exactly(random_data.size()), error_code);
465  ROS_DEBUG_STREAM("TestServerThread for command requests: send random data " << sick_scan::Utils::toHexString(random_data));
466  continue;
467  }
468  // command requests received, generate and send a synthetical response
470  if(cola_binary)
472  std::string ascii_telegram = sick_scan::ColaAsciiBinaryConverter::ConvertColaAscii(request.telegram_data);
473  ROS_INFO_STREAM("TestServerThread: received cola request " << ascii_telegram);
474  sick_scan::SickLocColaTelegramMsg telegram_msg = sick_scan::ColaParser::decodeColaTelegram(ascii_telegram);
475  // Generate a synthetical response depending on the request
476  sick_scan::SickLocColaTelegramMsg telegram_answer = sick_scan::TestcaseGenerator::createColaResponse(telegram_msg, m_scanner_type);
477  if (m_error_simulation_flag.get() == SEND_INVALID_TELEGRAMS) // error simulation: testserver sends invalid telegrams (invalid data, false checksums, etc.)
478  {
479  telegram_answer.command_name = random_ascii.generate(random_length.generate()); // random ascii string
480  telegram_answer.parameter.clear();
481  for(int n = random_length.generate(); n > 0; n--)
482  telegram_answer.parameter.push_back(random_ascii.generate(random_length.generate())); // random ascii string
483  ROS_DEBUG_STREAM("TestServerThread for result telegrams: send random cola response " << sick_scan::Utils::flattenToString(telegram_answer));
484  }
485  std::vector<uint8_t> binary_response = sick_scan::ColaParser::encodeColaTelegram(telegram_answer);
486  std::string ascii_response = sick_scan::ColaAsciiBinaryConverter::ConvertColaAscii(binary_response);
487  if(cola_binary) // binary_response = sick_scan::ColaAsciiBinaryConverter::ColaAsciiToColaBinary(binary_response);
488  binary_response = sick_scan::ColaAsciiBinaryConverter::ColaTelegramToColaBinary(telegram_answer);
489  ROS_INFO_STREAM("TestServerThread: sending cola response " << ascii_response << (cola_binary ? " (Cola-Binary)" : " (Cola-ASCII)"));
490  // Send command response to tcp client
491  if(cola_binary)
492  ROS_DEBUG_STREAM("TestServerThread: sending cola hex response " << sick_scan::Utils::toHexString(binary_response));
493  ROS::Time send_timestamp;
494  if (!sick_scan::ColaTransmitter::send(*p_socket, binary_response, send_timestamp))
495  {
496  ROS_WARN_STREAM("## ERROR TestServerThread: failed to send cola response, ColaTransmitter::send() returned false, data hexdump: " << sick_scan::Utils::toHexString(binary_response));
497  iTransmitErrorCnt++;
498  if(iTransmitErrorCnt >= 10)
499  {
500  ROS_WARN_STREAM("## ERROR TestServerThread: " << iTransmitErrorCnt << " transmission errors, giving up.");
501  ROS_WARN_STREAM("## ERROR TestServerThread: shutdown after error, aborting");
502  ROS::shutdown();
503  }
504  }
505  else
506  {
507  iTransmitErrorCnt = 0;
508  }
509  // Start or stop scandata thread
510  if(sick_scan::TestcaseGenerator::SendScandataEnabled() == true && m_tcp_send_scandata_thread == 0)
511  {
512  // Start scandata thread
513  m_tcp_send_scandata_thread_running = true;
514  m_tcp_send_scandata_thread = new boost::thread(&sick_scan::TestServerThread::runWorkerThreadScandataCb, this, p_socket);
515  }
516  else if(sick_scan::TestcaseGenerator::SendScandataEnabled() == false && m_tcp_send_scandata_thread != 0)
517  {
518  // Stop scandata thread
519  closeScandataThread();
520  }
521  }
522  ROS::sleep(0.0001);
523  }
524  closeScandataThread();
525  closeSocket(p_socket);
526  ROS_INFO_STREAM("TestServerThread: worker thread for command requests finished");
527 }
528 
535 void sick_scan::TestServerThread::runWorkerThreadScandataCb(boost::asio::ip::tcp::socket* p_socket)
536 {
537  std::vector<sick_scan::JsonScanData> binary_messages;
538  ROS_INFO_STREAM("TestServerThread: worker thread sending scandata and scandatamon messages started.");
539  if(m_scandatafiles.empty())
540  {
541  ROS_WARN_STREAM("## WARNING TestServerThread::runWorkerThreadScandataCb(): no scandata files configured, aborting worker thread to send scandata.");
542  m_tcp_send_scandata_thread_running = false;
543  return;
544  }
545  if(ROS::ok() && m_tcp_send_scandata_thread_running)
546  {
547  // Read scandata and scandatamon messages from jsonfile
548  std::vector<std::string> scandatafiles = sick_scan::PcapngJsonParser::split(m_scandatafiles, ',');
549  std::vector<std::string> scandatatypes = sick_scan::PcapngJsonParser::split(m_scandatatypes, ',');
550  double start_timestamp = 0;
551  for(int n = 0; n < scandatafiles.size(); n++)
552  {
553  if(!sick_scan::PcapngJsonParser::parseJsonfile(scandatafiles[n], scandatatypes, start_timestamp, binary_messages) || binary_messages.empty())
554  {
555  ROS_WARN_STREAM("## WARNING TestServerThread: error reading file \"" << scandatafiles[n] << "\".");
556  }
557  else
558  {
559  ROS_INFO_STREAM("TestServerThread: file \"" << scandatafiles[n] << "\" successfully parsed, " << binary_messages.size() << " messages of types \"" << m_scandatatypes << "\".");
560  start_timestamp = binary_messages.back().timestamp + 0.01;
561  }
562  }
563  }
564  if(binary_messages.empty())
565  {
566  ROS_WARN_STREAM("## WARNING TestServerThread::runWorkerThreadScandataCb(): no scandata found, aborting worker thread to send scandata.");
567  m_tcp_send_scandata_thread_running = false;
568  return;
569  }
570  ROS::sleep(m_start_scandata_delay); // delay between scandata activation ("LMCstartmeas" request) and first scandata message, default: 1 second
571  double last_msg_timestamp = 0;
572  int iTransmitErrorCnt = 0;
573  for(int msg_cnt = 0, msg_cnt_delta = 1; ROS::ok() && m_tcp_send_scandata_thread_running && p_socket; msg_cnt+=msg_cnt_delta)
574  {
575  if(msg_cnt >= (int)binary_messages.size()) // revert messages
576  {
577  msg_cnt = (int)binary_messages.size() - 2;
578  msg_cnt_delta *= -1;
579  }
580  if(msg_cnt < 0) // revert messages
581  {
582  msg_cnt = std::min(1, (int)binary_messages.size() - 1);
583  msg_cnt_delta *= -1;
584  }
585  sick_scan::JsonScanData& binary_message = binary_messages[msg_cnt];
586  ROS::sleep(std::max(0.001, std::abs(binary_message.timestamp - last_msg_timestamp)));
587  // Send messages
588  // ROS_DEBUG_STREAM("TestServerThread: sending scan data " << sick_scan::Utils::toHexString(binary_message.data));
589  ROS_INFO_STREAM("TestServerThread: sending " << binary_message.data.size() << " byte scan data "
590  << sick_scan::Utils::toAsciiString(&binary_message.data[0], std::min(32, (int)binary_message.data.size())) << " ... ");
591  ROS::Time send_timestamp = ROS::now();
592  if (!sick_scan::ColaTransmitter::send(*p_socket, binary_message.data, send_timestamp))
593  {
594  ROS_WARN_STREAM("## ERROR TestServerThread: failed to send cola response, ColaTransmitter::send() returned false, data hexdump: " << sick_scan::Utils::toHexString(binary_message.data));
595  iTransmitErrorCnt++;
596  if(iTransmitErrorCnt >= 10)
597  {
598  ROS_WARN_STREAM("## ERROR TestServerThread: " << iTransmitErrorCnt << " transmission errors, giving up.");
599  ROS_WARN_STREAM("## ERROR TestServerThread: shutdown after error, aborting");
600  ROS::shutdown();
601  }
602  }
603  else
604  {
605  iTransmitErrorCnt = 0;
606  }
607  last_msg_timestamp = binary_message.timestamp;
608  ROS::sleep(0.001);
609  }
610  m_tcp_send_scandata_thread_running = false;
611  ROS_INFO_STREAM("TestServerThread: worker thread sending scandata and scandatamon messages finished.");
612 }
613 
619 {
620  ROS::Time starttime = ROS::now();
621  while(ROS::ok() && m_error_simulation_thread_running && ROS::seconds(ROS::now() - starttime) < seconds)
622  {
623  ROS::sleep(0.001);
624  }
625 }
626 
633 bool sick_scan::TestServerThread::errorSimulationWaitForTelegramReceived(double timeout_seconds, sick_scan::SickLocResultPortTelegramMsg & telegram_msg)
634 {
635  ROS::Time starttime = ROS::now();
636  while(ROS::ok() && m_error_simulation_thread_running)
637  {
638  telegram_msg = m_last_telegram_received.get();
639  if(ROS::timeFromHeader(&telegram_msg.header) > starttime)
640  return true; // new telegram received
641  if(ROS::seconds(ROS::now() - starttime) >= timeout_seconds)
642  return false; // timeout
643  ROS::sleep(0.001);
644  }
645  return false;
646 }
647 
652 {
653  sick_scan::SickLocResultPortTelegramMsg telegram_msg;
654  size_t number_testcases = 0, number_testcases_failed = 0;
655  ROS_INFO_STREAM("TestServerThread: error simulation thread started");
656 
657  // Error simulation: start normal execution
658  number_testcases++;
659  m_error_simulation_flag.set(NO_ERROR);
660  ROS_INFO_STREAM("TestServerThread: 1. error simulation testcase: normal execution, expecting telegram messages from driver");
661  errorSimulationWait(10);
662  if(!errorSimulationWaitForTelegramReceived(10, telegram_msg))
663  {
664  number_testcases_failed++;
665  ROS_WARN_STREAM("## ERROR TestServerThread: 1. error simulation testcase: no ros telegram message received, expected SickLocResultPortTelegramMsg from driver");
666  }
667  else
668  ROS_INFO_STREAM("TestServerThread: 1. error simulation testcase: received telegram message \"" << sick_scan::Utils::flattenToString(telegram_msg) << "\", okay");
669 
670  // Error simulation testcase: testserver does not open listening port for 10 seconds, normal execution afterwards.
671  number_testcases++;
672  m_error_simulation_flag.set(DONT_LISTEN);
673  ROS_INFO_STREAM("TestServerThread: 2. error simulation testcase: server not responding, not listening, no tcp connections accepted.");
674  m_worker_thread_running = false;
675  ROS::sleep(1.0 / m_result_telegram_rate);
676  closeTcpConnections(true);
677  errorSimulationWait(10);
678  m_error_simulation_flag.set(NO_ERROR);
679  ROS_INFO_STREAM("TestServerThread: 2. error simulation testcase: switched to normal execution, expecting telegram messages from driver");
680  errorSimulationWait(10);
681  if(!errorSimulationWaitForTelegramReceived(10, telegram_msg))
682  {
683  number_testcases_failed++;
684  ROS_WARN_STREAM("## ERROR TestServerThread: 2. error simulation testcase: no ros telegram message received, expected SickLocResultPortTelegramMsg from driver");
685  }
686  else
687  ROS_INFO_STREAM("TestServerThread: 2. error simulation testcase: received telegram message \"" << sick_scan::Utils::flattenToString(telegram_msg) << "\", okay");
688 
689  // Error simulation testcase: testserver does not accecpt tcp clients for 10 seconds, normal execution afterwards.
690  number_testcases++;
691  m_error_simulation_flag.set(DONT_ACCECPT);
692  ROS_INFO_STREAM("TestServerThread: 3. error simulation testcase: server not responding, listening on port " << m_ip_port_results << ", but accepting no tcp clients");
693  m_worker_thread_running = false;
694  ROS::sleep(1.0 / m_result_telegram_rate);
695  closeTcpConnections(true);
696  errorSimulationWait(10);
697  m_error_simulation_flag.set(NO_ERROR);
698  errorSimulationWait(10);
699  if(!errorSimulationWaitForTelegramReceived(10, telegram_msg))
700  {
701  number_testcases_failed++;
702  ROS_WARN_STREAM("## ERROR TestServerThread: 3. error simulation testcase: no ros telegram message received, expected SickLocResultPortTelegramMsg from driver");
703  }
704  else
705  ROS_INFO_STREAM("TestServerThread: 3. error simulation testcase: received telegram message \"" << sick_scan::Utils::flattenToString(telegram_msg) << "\", okay");
706 
707  // Error simulation testcase: testserver does not send telegrams for 10 seconds, normal execution afterwards.
708  number_testcases++;
709  m_error_simulation_flag.set(DONT_SEND);
710  ROS_INFO_STREAM("TestServerThread: 4. error simulation testcase: server not sending telegrams");
711  errorSimulationWait(10);
712  m_error_simulation_flag.set(NO_ERROR);
713  errorSimulationWait(10);
714  if(!errorSimulationWaitForTelegramReceived(10, telegram_msg))
715  {
716  number_testcases_failed++;
717  ROS_WARN_STREAM("## ERROR TestServerThread: 4. error simulation testcase: no ros telegram message received, expected SickLocResultPortTelegramMsg from driver");
718  }
719  else
720  ROS_INFO_STREAM("TestServerThread: 4. error simulation testcase: received telegram message \"" << sick_scan::Utils::flattenToString(telegram_msg) << "\", okay");
721 
722  // Error simulation testcase: testserver sends random tcp data for 10 seconds, normal execution afterwards.
723  number_testcases++;
724  m_error_simulation_flag.set(SEND_RANDOM_TCP);
725  ROS_INFO_STREAM("TestServerThread: 5. error simulation testcase: server sending random tcp data");
726  errorSimulationWait(10);
727  m_error_simulation_flag.set(NO_ERROR);
728  errorSimulationWait(10);
729  if(!errorSimulationWaitForTelegramReceived(10, telegram_msg))
730  {
731  number_testcases_failed++;
732  ROS_WARN_STREAM("## ERROR TestServerThread: 5. error simulation testcase: no ros telegram message received, expected SickLocResultPortTelegramMsg from driver");
733  }
734  else
735  ROS_INFO_STREAM("TestServerThread: 5. error simulation testcase: received telegram message \"" << sick_scan::Utils::flattenToString(telegram_msg) << "\", okay");
736 
737  // Error simulation testcase: testserver sends invalid telegrams (invalid data, false checksums, etc.) for 10 seconds, normal execution afterwards.
738  number_testcases++;
739  m_error_simulation_flag.set(SEND_INVALID_TELEGRAMS);
740  ROS_INFO_STREAM("TestServerThread: 6. error simulation testcase: server sending invalid telegrams");
741  errorSimulationWait(10);
742  m_error_simulation_flag.set(NO_ERROR);
743  errorSimulationWait(10);
744  if(!errorSimulationWaitForTelegramReceived(10, telegram_msg))
745  {
746  number_testcases_failed++;
747  ROS_WARN_STREAM("## ERROR TestServerThread: 6. error simulation testcase: no ros telegram message received, expected SickLocResultPortTelegramMsg from driver");
748  }
749  else
750  ROS_INFO_STREAM("TestServerThread: 6. error simulation testcase: received telegram message \"" << sick_scan::Utils::flattenToString(telegram_msg) << "\", okay");
751 
752  // End of error simulation, print summary and exit
753  m_error_simulation_thread_running = false;
754  ROS_INFO_STREAM("TestServerThread: error simulation thread finished");
755  ROS_INFO_STREAM("TestServerThread: error simulation summary: " << (number_testcases - number_testcases_failed) << " of " << number_testcases<< " testcases passed, " << number_testcases_failed << " failures.");
756  ROS_INFO_STREAM("TestServerThread: exit with ros::shutdown()");
757  ROS::shutdown();
758 }
sick_scan::TestServerThread::runWorkerThreadScandataCb
virtual void runWorkerThreadScandataCb(boost::asio::ip::tcp::socket *p_socket)
Definition: test_server_thread.cpp:535
sick_scan::PcapngJsonParser::split
static std::vector< std::string > split(const std::string &s, char delimiter)
Splits a comma separated string into its parts.
Definition: pcapng_json_parser.cpp:69
sick_scan::TestServerThread::m_tcp_acceptor_cola
boost::asio::ip::tcp::acceptor m_tcp_acceptor_cola
boost acceptor for tcp clients for command requests and responses
Definition: test_server_thread.h:231
sick_scan::TestServerThread::m_error_simulation_enabled
bool m_error_simulation_enabled
default: false (no error simulation), if true, test server simulates errors and communication failure...
Definition: test_server_thread.h:258
msg
msg
ROS::sleep
void sleep(double seconds)
Definition: ros_wrapper.cpp:110
sick_scan::TestServerThread::closeWorkerThreads
void closeWorkerThreads(void)
Definition: test_server_thread.cpp:269
sick_scan::TestServerThread::m_start_scandata_delay
double m_start_scandata_delay
delay between scandata activation ("LMCstartmeas" request) and first scandata message,...
Definition: test_server_thread.h:227
sick_scan::TestServerThread::start
virtual bool start(void)
Definition: test_server_thread.cpp:122
sick_scan::TestcaseGenerator::SendScandataEnabled
static bool SendScandataEnabled(void)
Definition: testcase_generator.cpp:94
sick_scan::TestcaseGenerator::ResultTelegramsEnabled
static bool ResultTelegramsEnabled(void)
Definition: testcase_generator.cpp:112
sick_scan::TestServerThread::m_result_telegram_rate
double m_result_telegram_rate
frequency to generate and send result port telegrams, default: 10 Hz
Definition: test_server_thread.h:222
sick_scan::TestServerThread::ServerColaRequest
Definition: test_server_thread.h:123
sick_scan::Utils::flattenToString
static std::string flattenToString(const T &x)
Definition: utils.h:144
sick_scan::TestServerThread::runConnectionThreadResultCb
virtual void runConnectionThreadResultCb(void)
Definition: test_server_thread.cpp:296
sick_scan::ColaAsciiBinaryConverter::IsColaBinary
static bool IsColaBinary(const std::vector< uint8_t > &cola_telegram)
Returns true for Cola-Binary, if a given telegram is Cola-Binary encoded and starts with 4 Bytes { 0x...
Definition: cola_converter.cpp:342
sick_scan::TestServerThread::runWorkerThreadResultCb
virtual void runWorkerThreadResultCb(boost::asio::ip::tcp::socket *p_socket)
Definition: test_server_thread.cpp:366
sick_scan::ColaParser::encodeColaTelegram
static std::vector< uint8_t > encodeColaTelegram(const sick_scan::SickLocColaTelegramMsg &cola_telegram, bool parameter_is_ascii=true)
Encodes and returns a Cola Binary telegram from ros message SickLocColaTelegramMsg.
Definition: cola_parser.cpp:182
boost
sick_scan::TestcaseGenerator::createColaResponse
static sick_scan::SickLocColaTelegramMsg createColaResponse(const sick_scan::SickLocColaTelegramMsg &cola_request, const std::string &scanner_type)
Definition: testcase_generator.cpp:261
sick_scan::TestServerThread::errorSimulationWait
void errorSimulationWait(double seconds)
Definition: test_server_thread.cpp:618
sick_scan::TestServerThread::runWorkerThreadColaCb
virtual void runWorkerThreadColaCb(boost::asio::ip::tcp::socket *p_socket)
Definition: test_server_thread.cpp:441
sick_scan::PcapngJsonParser::parseJsonfile
static bool parseJsonfile(const std::string &json_filename, const std::vector< std::string > &scandatatypes, double start_time, std::vector< sick_scan::JsonScanData > &scandata)
Parses a jsonfile and returns a list of binary scandata messages of given type.
Definition: pcapng_json_parser.cpp:87
sick_scan::UniformRandomAsciiString::generate
std::string generate(int length)
Definition: random_generator.cpp:107
sick_scan::TestcaseGenerator::LocalizationEnabled
static bool LocalizationEnabled(void)
Definition: testcase_generator.cpp:103
ROS_DEBUG_STREAM
#define ROS_DEBUG_STREAM(args)
sick_scan::UniformRandomAsciiString
Definition: random_generator.h:104
ROS::now
ROS::Time now(void)
Definition: ros_wrapper.cpp:116
ROS_WARN_STREAM
#define ROS_WARN_STREAM(args)
utils.h
sick_scan::ColaAsciiBinaryConverter::ColaBinaryToColaAscii
static std::vector< uint8_t > ColaBinaryToColaAscii(const std::vector< uint8_t > &cola_telegram, bool parameter_to_ascii=true)
Converts and returns a Cola telegram from Cola-ASCII to Cola-Binary.
Definition: cola_converter.cpp:267
ROS::timeFromHeader
ROS::Time timeFromHeader(const std_msgs::Header *msg_header)
Definition: ros_wrapper.cpp:152
sick_scan::UniformRandomInteger
Definition: random_generator.h:65
sick_scan::TestServerThread::~TestServerThread
virtual ~TestServerThread()
Definition: test_server_thread.cpp:113
sick_scan::Utils::toAsciiString
static std::string toAsciiString(const uint8_t *binary_data, int length)
Definition: utils.cpp:82
sick_scan::ColaTransmitter::send
virtual bool send(const std::vector< uint8_t > &data, ROS::Time &send_timestamp)
Definition: cola_transmitter.cpp:125
sick_scan::TestServerThread::m_tcp_acceptor_results
boost::asio::ip::tcp::acceptor m_tcp_acceptor_results
boost acceptor for tcp clients for result telegrams
Definition: test_server_thread.h:230
testcase_generator.h
sick_scan::TestServerThread::closeSocket
void closeSocket(socket_ptr &p_socket)
Definition: test_server_thread.cpp:238
sick_scan::TestServerThread::errorSimulationWaitForTelegramReceived
bool errorSimulationWaitForTelegramReceived(double timeout_seconds, sick_scan::SickLocResultPortTelegramMsg &telegram_msg)
Definition: test_server_thread.cpp:633
sick_scan::TestServerThread::runConnectionThreadGenericCb
void runConnectionThreadGenericCb(boost::asio::ip::tcp::acceptor &tcp_acceptor_results, int ip_port_results, Callable thread_function_cb)
Definition: test_server_thread.cpp:314
sick_scan::TestServerThread::stop
virtual bool stop(void)
Definition: test_server_thread.cpp:151
ROS_INFO_STREAM
#define ROS_INFO_STREAM(args)
sick_scan::JsonScanData::timestamp
double timestamp
Definition: pcapng_json_parser.h:75
sick_scan::TestcaseGenerator::ResultPoseInterval
static uint32_t ResultPoseInterval(void)
Definition: testcase_generator.h:111
sick_scan::UniformRandomInteger::generate
int generate(void)
Definition: random_generator.cpp:75
sick_scan::TestServerThread::m_scandatatypes
std::string m_scandatatypes
comma separated list of scandata message types, f.e. "sSN LMDscandata,sSN LMDscandatamon"
Definition: test_server_thread.h:240
sick_scan::TestServerThread::thread_ptr
boost::thread * thread_ptr
shortcut for pointer to boost::thread
Definition: test_server_thread.h:131
pcap_json_converter.start_timestamp
float start_timestamp
Definition: pcap_json_converter.py:129
sick_scan::JsonScanData::data
std::vector< uint8_t > data
Definition: pcapng_json_parser.h:76
sick_scan::Utils::toHexString
static std::string toHexString(const std::vector< uint8_t > &binary_data)
Definition: utils.cpp:69
sick_scan::TestServerThread::closeScandataThread
void closeScandataThread(void)
Definition: test_server_thread.cpp:197
sick_scan::TestServerThread::m_scandatafiles
std::string m_scandatafiles
comma separated list of jsonfiles to emulate scandata messages, f.e. "tim781s_scandata....
Definition: test_server_thread.h:239
sick_scan::JsonScanData
class JsonScanData: utility container for binary scandata incl. timestamp
Definition: pcapng_json_parser.h:70
sick_scan::TestcaseGenerator::createResultPortCircles
static sick_scan::SickLocResultPortTestcaseMsg createResultPortCircles(double circle_radius, double circle_yaw)
Definition: testcase_generator.cpp:220
NO_ERROR
NO_ERROR
test_server_thread.h
sick_scan::TestServerThread::m_scanner_type
std::string m_scanner_type
currently supported: "sick_lms_5xx", "sick_tim_7xx"
Definition: test_server_thread.h:241
sick_scan::TestServerThread::m_demo_move_in_circles
bool m_demo_move_in_circles
true: simulate a sensor moving in circles, false (default): create random based result port telegrams
Definition: test_server_thread.h:238
sick_scan::TestServerThread::closeTcpConnections
virtual void closeTcpConnections(bool force_shutdown=false)
Definition: test_server_thread.cpp:212
sick_scan::TestServerThread::messageCbResultPortTelegrams
void messageCbResultPortTelegrams(const sick_scan::SickLocResultPortTelegramMsg &msg)
Definition: test_server_thread.cpp:287
ROS::seconds
double seconds(ROS::Duration duration)
Definition: ros_wrapper.cpp:180
sick_scan::TestServerThread::m_result_testcases_frame_id
std::string m_result_testcases_frame_id
ros frame id of testcase messages (type SickLocResultPortTestcaseMsg), default: "result_testcases"
Definition: test_server_thread.h:237
sick_scan::TestcaseGenerator::createRandomResultPortTestcase
static sick_scan::SickLocResultPortTestcaseMsg createRandomResultPortTestcase(void)
Definition: testcase_generator.cpp:158
sick_scan::TestServerThread::runConnectionThreadColaCb
virtual void runConnectionThreadColaCb(void)
Definition: test_server_thread.cpp:305
sick_scan::TestServerThread::m_result_testcases_publisher
sick_scan::SickLocResultPortTestcaseMsgPublisher m_result_testcases_publisher
ros publisher for testcases with result port telegrams (type SickLocResultPortTestcaseMsg)
Definition: test_server_thread.h:236
sick_scan::TestServerThread::socket_ptr
boost::asio::ip::tcp::socket * socket_ptr
shortcut for pointer to boost::asio::ip::tcp::socket
Definition: test_server_thread.h:132
cola_transmitter.h
sick_scan::TestServerThread::runErrorSimulationThreadCb
virtual void runErrorSimulationThreadCb(void)
Definition: test_server_thread.cpp:651
sick_scan::ColaTransmitter::receive
virtual bool receive(std::vector< uint8_t > &telegram, double timeout, ROS::Time &receive_timestamp)
Definition: cola_transmitter.cpp:166
sick_scan::ColaParser::decodeColaTelegram
static sick_scan::SickLocColaTelegramMsg decodeColaTelegram(const std::vector< uint8_t > &cola_binary)
Decodes and returns a Cola message of type SickLocColaTelegramMsg from a Cola-Binary telegram....
Definition: cola_parser.cpp:119
ros_wrapper.h
pcapng_json_parser.h
sick_scan::TestServerThread::ServerColaRequest::telegram_data
std::vector< uint8_t > telegram_data
received telegram_data (Cola-Ascii or Cola-Binary)
Definition: test_server_thread.h:128
sick_scan::TestServerThread::TestServerThread
TestServerThread(ROS::NodePtr nh=0, int ip_port_results=2201, int ip_port_cola=2111)
Definition: test_server_thread.cpp:74
sick_scan::ColaAsciiBinaryConverter::ConvertColaAscii
static std::string ConvertColaAscii(const std::vector< uint8_t > &cola_telegram)
Converts and returns a Cola-ASCII telegram to string.
Definition: cola_converter.cpp:105
cola_parser.h
sick_scan::ColaAsciiBinaryConverter::ColaTelegramToColaBinary
static std::vector< uint8_t > ColaTelegramToColaBinary(const sick_scan::SickLocColaTelegramMsg &cola_telegram, int parameter_is_ascii=-1)
Converts and returns a Cola telegram to Cola-Binary.
Definition: cola_converter.cpp:185
sick_scan::Utils::normalizeAngle
static double normalizeAngle(double angle)
Definition: utils.h:260
random_generator.h


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