cola_transmitter.cpp
Go to the documentation of this file.
1 /*
2  * @brief sim_loc_cola_transmitter connects to the localization controller, sends cola command requests,
3  * and waits for the response with timeout.
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 
57 #include <thread>
58 #include <mutex>
59 
60 #include "sick_scan/ros_wrapper.h"
61 
62 #include "sick_scan/cola_parser.h"
64 #include "sick_scan/utils.h"
65 
66 static std::mutex s_ColaTransmitterSendMutex; // mutex to lock ColaTransmitter::send
67 
74 sick_scan::ColaTransmitter::ColaTransmitter(const std::string & server_adress, int tcp_port, double default_receive_timeout)
75 : m_server_adress(server_adress), m_tcp_port(tcp_port), m_ioservice(), m_tcp_socket(m_ioservice),
76  m_receive_timeout(default_receive_timeout), m_receiver_thread_running(false), m_receiver_thread(0)
77 {
78 
79 }
80 
85 {
86  m_receiver_thread_running = false;
87  close();
88  stopReceiverThread();
89 }
90 
96 {
97  return m_tcp_socket.connect(m_ioservice, m_server_adress, m_tcp_port);
98 }
99 
105 {
106  try
107  {
108  m_tcp_socket.close();
109  m_ioservice.stop();
110  return true;
111  }
112  catch(std::exception & exc)
113  {
114  ROS_WARN_STREAM("ColaTransmitter::closeTcpConnections(): exception " << exc.what() << " on closing connection.");
115  }
116  return false;
117 }
118 
125 bool sick_scan::ColaTransmitter::send(const std::vector<uint8_t> & data, ROS::Time & send_timestamp)
126 {
127  return send(m_tcp_socket.socket(), data, send_timestamp);
128 }
129 
137 bool sick_scan::ColaTransmitter::send(boost::asio::ip::tcp::socket & socket, const std::vector<uint8_t> & data, ROS::Time & send_timestamp)
138 {
139  std::lock_guard<std::mutex> send_lock_guard(s_ColaTransmitterSendMutex);
140  try
141  {
142  if (ROS::ok() && socket.is_open())
143  {
144  boost::system::error_code errorcode;
145  send_timestamp = ROS::now();
146  size_t bytes_written = boost::asio::write(socket, boost::asio::buffer(data.data(), data.size()), boost::asio::transfer_exactly(data.size()), errorcode);
147  if (!errorcode && bytes_written == data.size())
148  return true;
149  ROS_WARN_STREAM("## ERROR ColaTransmitter::send: tcp socket write error, " << bytes_written << " of " << data.size() << " bytes written, errorcode " << errorcode.value() << " \"" << errorcode.message() << "\"");
150  }
151  }
152  catch(std::exception & exc)
153  {
154  ROS_WARN_STREAM("## ERROR ColaTransmitter::send(): exception " << exc.what());
155  }
156  return false;
157 }
158 
166 bool sick_scan::ColaTransmitter::receive(std::vector<uint8_t> & telegram, double timeout, ROS::Time & receive_timestamp)
167 {
168  return receive(m_tcp_socket.socket(), telegram, timeout, receive_timestamp);
169 }
170 
179 bool sick_scan::ColaTransmitter::receive(boost::asio::ip::tcp::socket & socket, std::vector<uint8_t> & telegram, double timeout, ROS::Time & receive_timestamp)
180 {
181  telegram.clear();
182  telegram.reserve(1024);
183  try
184  {
185  std::vector<uint8_t> binETX = sick_scan::ColaParser::binaryETX();
186  ROS::Time start_time = ROS::now();
187  while (ROS::ok() && socket.is_open())
188  {
189  // Read 1 byte
190  uint8_t byte_received = 0;
191  boost::system::error_code errorcode;
192  // Possibly better than receiving byte for byte: use boost::asio::read_until to read until <ETX> received
193  if (socket.available() > 0 && boost::asio::read(socket, boost::asio::buffer(&byte_received, 1), boost::asio::transfer_exactly(1), errorcode) > 0 && !errorcode)
194  {
195  if (telegram.empty())
196  receive_timestamp = ROS::now(); // timestamp after first byte received
197  telegram.push_back(byte_received);
198  }
199  else
200  ROS::sleep(0.0001);
201  // Check for "<ETX>" (message completed) and return if received data ends with "<ETX>"
202  bool is_binary_cola = sick_scan::ColaAsciiBinaryConverter::IsColaBinary(telegram);
203  if(is_binary_cola)
204  {
205  // Cola-Binary: Check telegram length and return if all bytes received
206  uint32_t telegram_length = sick_scan::ColaAsciiBinaryConverter::ColaBinaryTelegramLength(telegram);
207  if(telegram_length > 0 && telegram.size() >= telegram_length)
208  return true; // all bytes received, telegram completed
209  }
210  else
211  {
212  // Cola-ASCII: Check for "<ETX>" (message completed) and return if received data ends with "<ETX>"
213  if (dataEndWithETX(telegram, binETX))
214  return true; // <ETX> received, telegram completed
215  }
216  // Check for timeout
217  if (ROS::seconds(ROS::now() - start_time) >= timeout)
218  {
219  // ROS_DEBUG_STREAM("ColaTransmitter::receive(): timeout, " << telegram.size() << " byte received: " << sick_scan::Utils::toHexString(telegram));
220  break;
221  }
222  }
223  }
224  catch(std::exception & exc)
225  {
226  ROS_WARN_STREAM("## ERROR ColaTransmitter::receive(): exception " << exc.what());
227  }
228  return false; // no tcp connection or timeout
229 }
230 
237 bool sick_scan::ColaTransmitter::dataEndWithETX(const std::vector<uint8_t> & data, const std::vector<uint8_t> & etx)
238 {
239  if(data.size() < etx.size())
240  return false;
241  for(size_t n = data.size() - etx.size(), m = 0; m < etx.size(); n++, m++)
242  {
243  if(data[n] != etx[m])
244  return false;
245  }
246  return true;
247 }
248 
255 {
256  stopReceiverThread();
257  m_receiver_thread_running = true;
258  m_receiver_thread = new boost::thread(&sick_scan::ColaTransmitter::runReceiverThreadCb, this);
259  return true;
260 }
261 
267 {
268  m_receiver_thread_running = false;
269  if(m_receiver_thread)
270  {
271  m_receiver_thread->join();
272  delete(m_receiver_thread);
273  m_receiver_thread = 0;
274  }
275  return true;
276 }
277 
288 bool sick_scan::ColaTransmitter::waitPopResponse(std::vector<uint8_t> & telegram, double timeout, ROS::Time & receive_timestamp)
289 {
290  ROS::Time start_time = ROS::now();
291  while(ROS::ok() && m_receiver_thread_running && m_response_fifo.empty())
292  {
293  ROS::sleep(0.0001);
294  m_response_fifo.waitOnceForElement();
295  if(ROS::seconds(ROS::now() - start_time) >= timeout)
296  break;
297  }
298  if(!m_response_fifo.empty())
299  {
300  ColaResponseContainer response = m_response_fifo.pop();
301  telegram = response.telegram_data;
302  receive_timestamp = response.receive_timestamp;
303  return true;
304  }
305  return false; // timeout
306 }
307 
312 {
313  while(ROS::ok() && m_receiver_thread_running)
314  {
316  if(receive(response.telegram_data, m_receive_timeout, response.receive_timestamp))
317  m_response_fifo.push(response);
318  else
319  m_response_fifo.notify();
320  }
321 }
response
const std::string response
sick_scan::ColaAsciiBinaryConverter::ColaBinaryTelegramLength
static uint32_t ColaBinaryTelegramLength(const std::vector< uint8_t > &cola_telegram)
Decodes the header and returns the length of a Cola-Binary telegram.
Definition: cola_converter.cpp:354
sick_scan::ColaTransmitter::startReceiverThread
virtual bool startReceiverThread(void)
Definition: cola_transmitter.cpp:254
sick_scan::ColaParser::binaryETX
static std::vector< uint8_t > binaryETX(void)
Returns the binary "end of text" tag in a Cola-Binary command, i.e. {0x03}.
Definition: cola_parser.h:176
ROS::sleep
void sleep(double seconds)
Definition: ros_wrapper.cpp:110
sick_scan::ColaTransmitter::connect
virtual bool connect(void)
Definition: cola_transmitter.cpp:95
sick_scan::ColaTransmitter::~ColaTransmitter
virtual ~ColaTransmitter()
Definition: cola_transmitter.cpp:84
sick_scan::ColaTransmitter::close
virtual bool close(void)
Definition: cola_transmitter.cpp:104
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::ColaTransmitter::stopReceiverThread
virtual bool stopReceiverThread(void)
Definition: cola_transmitter.cpp:266
data
data
sick_scan::ColaTransmitter::runReceiverThreadCb
void runReceiverThreadCb(void)
Definition: cola_transmitter.cpp:311
ROS::now
ROS::Time now(void)
Definition: ros_wrapper.cpp:116
ROS_WARN_STREAM
#define ROS_WARN_STREAM(args)
utils.h
sick_scan::ColaTransmitter::send
virtual bool send(const std::vector< uint8_t > &data, ROS::Time &send_timestamp)
Definition: cola_transmitter.cpp:125
sick_scan::ColaTransmitter::ColaTransmitter
ColaTransmitter(const std::string &server_adress="192.168.0.1", int tcp_port=2111, double default_receive_timeout=1)
Definition: cola_transmitter.cpp:74
sick_scan::ColaTransmitter::dataEndWithETX
static bool dataEndWithETX(const std::vector< uint8_t > &data, const std::vector< uint8_t > &etx)
Definition: cola_transmitter.cpp:237
colaa::detail::read
T read(const std::string &str)
General template which is unimplemented; implemented specializations follow below.
Definition: colaa.hpp:72
sick_scan::ColaTransmitter::waitPopResponse
virtual bool waitPopResponse(std::vector< uint8_t > &telegram, double timeout, ROS::Time &receive_timestamp)
Definition: cola_transmitter.cpp:288
sick_scan::ColaTransmitter::ColaResponseContainer
Definition: cola_transmitter.h:164
ROS::seconds
double seconds(ROS::Duration duration)
Definition: ros_wrapper.cpp:180
sick_generic_device_finder.timeout
timeout
Definition: sick_generic_device_finder.py:113
cola_transmitter.h
s_ColaTransmitterSendMutex
static std::mutex s_ColaTransmitterSendMutex
Definition: cola_transmitter.cpp:66
sick_scan::ColaTransmitter::receive
virtual bool receive(std::vector< uint8_t > &telegram, double timeout, ROS::Time &receive_timestamp)
Definition: cola_transmitter.cpp:166
ros_wrapper.h
cola_parser.h


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