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)
112 catch(std::exception & exc)
114 ROS_WARN_STREAM(
"ColaTransmitter::closeTcpConnections(): exception " << exc.what() <<
" on closing connection.");
142 if (ROS::ok() && socket.is_open())
144 boost::system::error_code errorcode;
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())
149 ROS_WARN_STREAM(
"## ERROR ColaTransmitter::send: tcp socket write error, " << bytes_written <<
" of " << data.size() <<
" bytes written, errorcode " << errorcode.value() <<
" \"" << errorcode.message() <<
"\"");
152 catch(std::exception & exc)
154 ROS_WARN_STREAM(
"## ERROR ColaTransmitter::send(): exception " << exc.what());
182 telegram.reserve(1024);
187 while (ROS::ok() && socket.is_open())
190 uint8_t byte_received = 0;
191 boost::system::error_code errorcode;
193 if (socket.available() > 0 &&
boost::asio::read(socket, boost::asio::buffer(&byte_received, 1), boost::asio::transfer_exactly(1), errorcode) > 0 && !errorcode)
195 if (telegram.empty())
197 telegram.push_back(byte_received);
207 if(telegram_length > 0 && telegram.size() >= telegram_length)
224 catch(std::exception & exc)
226 ROS_WARN_STREAM(
"## ERROR ColaTransmitter::receive(): exception " << exc.what());
239 if(data.size() < etx.size())
241 for(
size_t n = data.size() - etx.size(), m = 0; m < etx.size(); n++, m++)
243 if(data[n] != etx[m])
static uint32_t ColaBinaryTelegramLength(const std::vector< uint8_t > &cola_telegram)
Decodes the header and returns the length of a Cola-Binary telegram.
virtual ~ColaTransmitter()
sick_scan::ClientSocket m_tcp_socket
tcp socket connected to the localization controller
int m_tcp_port
tcp port of the localization controller, default: 2111 for command requests and 2112 for command resp...
bool m_receiver_thread_running
true: m_receiver_thread is running, otherwise false
std::string m_server_adress
ip adress of the localization controller, default: 192.168.0.1
virtual bool connect(void)
virtual bool startReceiverThread(void)
T read(const std::string &str)
General template which is unimplemented; implemented specializations follow below.
virtual bool stopReceiverThread(void)
double m_receive_timeout
default timeout in seconds for receive functions
boost::asio::io_service m_ioservice
boost io service for tcp connections
virtual boost::asio::ip::tcp::socket & socket(void)
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...
static std::vector< uint8_t > binaryETX(void)
Returns the binary "end of text" tag in a Cola-Binary command, i.e. {0x03}.
static bool dataEndWithETX(const std::vector< uint8_t > &data, const std::vector< uint8_t > &etx)
void runReceiverThreadCb(void)
boost::thread * m_receiver_thread
thread to receive responses from localization server
virtual bool send(const std::vector< uint8_t > &data, ROS::Time &send_timestamp)
#define ROS_WARN_STREAM(args)
ColaTransmitter(const std::string &server_adress="192.168.0.1", int tcp_port=2111, double default_receive_timeout=1)
ROS::Time receive_timestamp
receive timestamp in seconds (ros timestamp immediately after first response byte received) ...
static std::mutex s_ColaTransmitterSendMutex
sick_scan::FifoBuffer< ColaResponseContainer, boost::mutex > m_response_fifo
fifo buffer for receiver thread for responses from localization server
virtual bool receive(std::vector< uint8_t > &telegram, double timeout, ROS::Time &receive_timestamp)
virtual bool close(bool force_shutdown=false)
std::vector< uint8_t > telegram_data
received telegram_data (Cola-Ascii or Cola-Binary)
virtual bool connect(boost::asio::io_service &io_service, const std::string &server_adress, int tcp_port)
virtual bool waitPopResponse(std::vector< uint8_t > &telegram, double timeout, ROS::Time &receive_timestamp)
const std::string response
void sleep(double seconds)
double seconds(ROS::Duration duration)