cola_transmitter.cpp

Go to the documentation of this file.

/*
 * @brief sim_loc_cola_transmitter connects to the localization controller, sends cola command requests,
 * and waits for the response with timeout.
 *
 * Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim
 * Copyright (C) 2019 SICK AG, Waldkirch
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *       http://www.apache.org/licenses/LICENSE-2.0
 *
 *   Unless required by applicable law or agreed to in writing, software
 *   distributed under the License is distributed on an "AS IS" BASIS,
 *   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 *   See the License for the specific language governing permissions and
 *   limitations under the License.
 *
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of SICK AG nor the names of its
 *       contributors may be used to endorse or promote products derived from
 *       this software without specific prior written permission
 *     * Neither the name of Ing.-Buero Dr. Michael Lehning nor the names of its
 *       contributors may be used to endorse or promote products derived from
 *       this software without specific prior written permission
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 *      Authors:
 *         Michael Lehning <michael.lehning@lehning.de>
 *
 *  Copyright 2019 SICK AG
 *  Copyright 2019 Ing.-Buero Dr. Michael Lehning
 *
 */
 
#include <thread>
#include <mutex>
 
#include "sick_scan/ros_wrapper.h"
 
#include "sick_scan/cola_parser.h"
#include "sick_scan/cola_transmitter.h"
#include "sick_scan/utils.h"
 
static std::mutex s_ColaTransmitterSendMutex; // mutex to lock ColaTransmitter::send
 
sick_scan_xd::ColaTransmitter::ColaTransmitter(const std::string & server_address, int tcp_port, double default_receive_timeout)
: m_server_address(server_address), m_tcp_port(tcp_port), 
  m_receive_timeout(default_receive_timeout), m_receiver_thread_running(false), m_receiver_thread(0)
{
 
}
 
sick_scan_xd::ColaTransmitter::~ColaTransmitter()
{
  m_receiver_thread_running = false;
  close();
  stopReceiverThread();
}
 
bool sick_scan_xd::ColaTransmitter::connect(void)
{
  return m_tcp_socket.connect(m_server_address, m_tcp_port);
}
 
bool sick_scan_xd::ColaTransmitter::close(void)
{
  try
  {
    m_tcp_socket.close();
    return true;
  }
  catch(std::exception & exc)
  {
    ROS_WARN_STREAM("ColaTransmitter::closeTcpConnections(): exception " << exc.what() << " on closing connection.");
  }
  return false;
}
 
bool sick_scan_xd::ColaTransmitter::send(const std::vector<uint8_t> & data, ROS::Time & send_timestamp)
{
  return send(m_tcp_socket.socket(), data, send_timestamp);
}
 
bool sick_scan_xd::ColaTransmitter::send(socket_t & socket, const std::vector<uint8_t> & data, ROS::Time & send_timestamp)
{
  std::lock_guard<std::mutex> send_lock_guard(s_ColaTransmitterSendMutex);
  try
  {
    if (ROS::ok() && socket != INVALID_SOCKET)
    {
      send_timestamp = ROS::now();
      size_t bytes_written = ::send(socket, (char*)data.data(), data.size(), 0);
      if (bytes_written == data.size())
        return true;
      ROS_WARN_STREAM("## ERROR ColaTransmitter::send: tcp socket write error, " << bytes_written << " of " << data.size() << " bytes written");
    }
  }
  catch(std::exception & exc)
  {
    ROS_WARN_STREAM("## ERROR ColaTransmitter::send(): exception " << exc.what());
  }
  return false;
}
 
bool sick_scan_xd::ColaTransmitter::receive(std::vector<uint8_t> & telegram, double timeout, ROS::Time & receive_timestamp)
{
  return receive(m_tcp_socket.socket(), telegram, timeout, receive_timestamp);
}
 
bool sick_scan_xd::ColaTransmitter::receive(socket_t & socket, std::vector<uint8_t> & telegram, double timeout, ROS::Time & receive_timestamp)
{
  telegram.clear();
  telegram.reserve(1024);
  try
  {
    std::vector<uint8_t> binETX = sick_scan_xd::ColaParser::binaryETX();
    ROS::Time start_time = ROS::now();
    while (ROS::ok() && socket != INVALID_SOCKET)
    {
      // Read 1 byte
      uint8_t byte_received = 0;
      // set socket to nonblocking mode
      int recv_flags = 0;
# ifdef _MSC_VER
      u_long recv_mode = 1; // FIONBIO enables or disables the blocking mode for the socket. If iMode = 0, blocking is enabled, if iMode != 0, non-blocking mode is enabled.
      ioctlsocket(socket, FIONBIO, &recv_mode);
# else
      recv_flags |= MSG_DONTWAIT;
# endif
 
      // Possibly better than receiving byte for byte: use boost::asio::read_until to read until <ETX> received
      if (::recv(socket, (char*)&byte_received, 1, recv_flags) == 1)
      {
        if (telegram.empty())
          receive_timestamp = ROS::now(); // timestamp after first byte received
        telegram.push_back(byte_received);
        // Check for "<ETX>" (message completed) and return if received data ends with "<ETX>"
        bool is_binary_cola = sick_scan_xd::ColaAsciiBinaryConverter::IsColaBinary(telegram);
        if(is_binary_cola)
        {
          // Cola-Binary: Check telegram length and return if all bytes received
          uint32_t telegram_length = sick_scan_xd::ColaAsciiBinaryConverter::ColaBinaryTelegramLength(telegram);
          if(telegram_length > 0 && telegram.size() >= telegram_length)
            return true; // all bytes received, telegram completed
        }
        else
        {
          // Cola-ASCII: Check for "<ETX>" (message completed) and return if received data ends with "<ETX>"
          if (dataEndWithETX(telegram, binETX))
            return true; // <ETX> received, telegram completed
        }
      }
      else
        ROS::sleep(0.0001);
      // Check for timeout
      if (ROS::seconds(ROS::now() - start_time) >= timeout)
      {
        // ROS_DEBUG_STREAM("ColaTransmitter::receive(): timeout, " << telegram.size() << " byte received: " << sick_scan_xd::Utils::toHexString(telegram));
        break;
      }
    }
  }
  catch(std::exception & exc)
  {
    ROS_WARN_STREAM("## ERROR ColaTransmitter::receive(): exception " << exc.what());
  }
  return false; // no tcp connection or timeout
}
 
bool sick_scan_xd::ColaTransmitter::dataEndWithETX(const std::vector<uint8_t> & data, const std::vector<uint8_t> & etx)
{
  if(data.size() < etx.size())
    return false;
  for(size_t n = data.size() - etx.size(), m = 0; m <  etx.size(); n++, m++)
  {
    if(data[n] != etx[m])
      return false;
  }
  return true;
}
 
bool sick_scan_xd::ColaTransmitter::startReceiverThread(void)
{
  stopReceiverThread();
  m_receiver_thread_running = true;
  m_receiver_thread = new std::thread(&sick_scan_xd::ColaTransmitter::runReceiverThreadCb, this);
  return true;
}
 
bool sick_scan_xd::ColaTransmitter::stopReceiverThread(void)
{
  m_receiver_thread_running = false;
  if(m_receiver_thread)
  {
    m_receiver_thread->join();
    delete(m_receiver_thread);
    m_receiver_thread = 0;
  }
  return true;
}
 
bool sick_scan_xd::ColaTransmitter::waitPopResponse(std::vector<uint8_t> & telegram, double timeout, ROS::Time & receive_timestamp)
{
  ROS::Time start_time = ROS::now();
  while(ROS::ok() && m_receiver_thread_running && m_response_fifo.empty())
  {
    ROS::sleep(0.0001);
    m_response_fifo.waitOnceForElement();
    if(ROS::seconds(ROS::now() - start_time) >= timeout)
      break;
  }
  if(!m_response_fifo.empty())
  {
    ColaResponseContainer response = m_response_fifo.pop();
    telegram = response.telegram_data;
    receive_timestamp = response.receive_timestamp;
    return true;
  }
  return false; // timeout
}
 
void sick_scan_xd::ColaTransmitter::runReceiverThreadCb(void)
{
  while(ROS::ok() && m_receiver_thread_running)
  {
    ColaResponseContainer response;
    if(receive(response.telegram_data, m_receive_timeout, response.receive_timestamp))
      m_response_fifo.push(response);
    else
      m_response_fifo.notify();
  }
}