23 #include "sick_tim55x_ethernet_aqt.h"
25 #include <core/threading/mutex.h>
26 #include <core/threading/mutex_locker.h>
27 #include <utils/math/angle.h>
28 #include <utils/misc/string_split.h>
30 #include <boost/lambda/bind.hpp>
31 #include <boost/lambda/lambda.hpp>
32 #include <boost/lexical_cast.hpp>
33 #if BOOST_VERSION < 104800
34 # include <boost/bind.hpp>
43 #define RECONNECT_INTERVAL 1000
44 #define RECEIVE_TIMEOUT 500
57 std::string &cfg_prefix)
60 deadline_(io_service_),
61 soft_deadline_(io_service_)
63 set_name(
"SickTiM55x(%s)", cfg_name.c_str());
74 socket_mutex_ =
new Mutex();
76 deadline_.expires_at(boost::posix_time::pos_infin);
79 soft_deadline_.expires_at(boost::posix_time::pos_infin);
102 if (socket_.is_open()) {
104 deadline_.expires_from_now(boost::posix_time::milliseconds(RECEIVE_TIMEOUT));
106 ec_ = boost::asio::error::would_block;
109 boost::asio::async_read_until(socket_,
112 #
if BOOST_VERSION >= 104800
113 (boost::lambda::var(ec_) = boost::lambda::_1,
114 boost::lambda::var(bytes_read_) = boost::lambda::_2));
116 boost::bind(&SickTiM55xEthernetAcquisitionThread::handle_read,
118 boost::asio::placeholders::error,
119 boost::asio::placeholders::bytes_transferred));
123 io_service_.run_one();
124 while (ec_ == boost::asio::error::would_block);
130 if (ec_.value() == boost::system::errc::operation_canceled) {
142 deadline_.expires_at(boost::posix_time::pos_infin);
144 unsigned char recv_buf[bytes_read_];
145 std::istream in_stream(&input_buffer_);
146 in_stream.read((
char *)recv_buf, bytes_read_);
148 if (bytes_read_ > 0) {
158 }
catch (boost::system::system_error &e) {
159 if (e.code() == boost::asio::error::eof) {
161 logger->
log_warn(
name(),
"Sick TiM55x/Ethernet connection lost, trying to reconnect");
172 usleep(RECONNECT_INTERVAL * 1000);
180 SickTiM55xEthernetAcquisitionThread::open_device()
183 boost::asio::ip::tcp::resolver resolver(io_service_);
184 boost::asio::ip::tcp::resolver::query query(cfg_host_, cfg_port_);
185 boost::asio::ip::tcp::resolver::iterator iter = resolver.resolve(query);
191 deadline_.expires_from_now(boost::posix_time::seconds(5));
193 for (; iter != boost::asio::ip::tcp::resolver::iterator(); ++iter) {
195 ec_ = boost::asio::error::would_block;
196 #if BOOST_VERSION >= 104800
197 socket_.async_connect(iter->endpoint(), boost::lambda::var(ec_) = boost::lambda::_1);
199 socket_.async_connect(iter->endpoint(),
200 boost::bind(&SickTiM55xEthernetAcquisitionThread::handle_read,
202 boost::asio::placeholders::error,
208 io_service_.run_one();
209 while (ec_ == boost::asio::error::would_block);
212 if (ec_ || !socket_.is_open()) {
213 if (ec_.value() == boost::system::errc::operation_canceled) {
214 throw Exception(
"Sick TiM55X Ethernet: connection timed out");
216 throw Exception(
"Connection failed: %s", ec_.message().c_str());
219 deadline_.expires_at(boost::posix_time::pos_infin);
221 }
catch (boost::system::system_error &e) {
222 throw Exception(
"Connection failed: %s", e.what());
227 SickTiM55xEthernetAcquisitionThread::close_device()
229 boost::system::error_code err;
230 if (socket_.is_open()) {
231 socket_.shutdown(boost::asio::ip::tcp::socket::shutdown_both, err);
237 SickTiM55xEthernetAcquisitionThread::flush_device()
239 if (socket_.is_open()) {
241 soft_deadline_.expires_from_now(boost::posix_time::milliseconds(RECEIVE_TIMEOUT));
243 ec_ = boost::asio::error::would_block;
246 boost::asio::async_read_until(socket_,
249 #
if BOOST_VERSION >= 104800
250 (boost::lambda::var(ec_) = boost::lambda::_1,
251 boost::lambda::var(bytes_read_) = boost::lambda::_2));
253 boost::bind(&SickTiM55xEthernetAcquisitionThread::handle_read,
255 boost::asio::placeholders::error,
256 boost::asio::placeholders::bytes_transferred));
260 io_service_.run_one();
261 while (ec_ == boost::asio::error::would_block);
263 }
while (bytes_read_ > 0);
264 soft_deadline_.expires_from_now(boost::posix_time::pos_infin);
265 }
catch (boost::system::system_error &e) {
273 SickTiM55xEthernetAcquisitionThread::send_with_reply(
const char *request, std::string *reply)
277 int request_length = strlen(request);
280 boost::asio::write(socket_, boost::asio::buffer(request, request_length));
282 deadline_.expires_from_now(boost::posix_time::milliseconds(RECEIVE_TIMEOUT));
284 ec_ = boost::asio::error::would_block;
286 boost::asio::async_read_until(socket_,
289 #
if BOOST_VERSION >= 104800
290 (boost::lambda::var(ec_) = boost::lambda::_1,
291 boost::lambda::var(bytes_read_) = boost::lambda::_2));
293 boost::bind(&SickTiM55xEthernetAcquisitionThread::handle_read,
295 boost::asio::placeholders::error,
296 boost::asio::placeholders::bytes_transferred));
300 io_service_.run_one();
301 while (ec_ == boost::asio::error::would_block);
304 if (ec_.value() == boost::system::errc::operation_canceled) {
305 throw Exception(
"Timeout waiting for message reply");
307 throw Exception(
"Failed to read reply: %s", ec_.message().c_str());
311 deadline_.expires_at(boost::posix_time::pos_infin);
314 char recv_buf[bytes_read_];
315 std::istream in_stream(&input_buffer_);
316 in_stream.read(recv_buf, bytes_read_);
317 *reply = std::string(recv_buf, bytes_read_);
319 input_buffer_.consume(bytes_read_);
321 }
catch (boost::system::system_error &e) {
322 throw Exception(
"Sick TiM55x/Ethernet failed I/O: %s", e.what());
332 SickTiM55xEthernetAcquisitionThread::check_deadline()
334 if (deadline_.expires_at() <= boost::asio::deadline_timer::traits_type::now()) {
336 deadline_.expires_at(boost::posix_time::pos_infin);
339 #if BOOST_VERSION >= 104800
340 deadline_.async_wait(
341 boost::lambda::bind(&SickTiM55xEthernetAcquisitionThread::check_deadline,
this));
343 deadline_.async_wait(boost::bind(&SickTiM55xEthernetAcquisitionThread::check_deadline,
this));
353 SickTiM55xEthernetAcquisitionThread::check_soft_timeout()
355 if (soft_deadline_.expires_at() <= boost::asio::deadline_timer::traits_type::now()) {
357 soft_deadline_.expires_at(boost::posix_time::pos_infin);
360 #if BOOST_VERSION >= 104800
361 soft_deadline_.async_wait(
362 boost::lambda::bind(&SickTiM55xEthernetAcquisitionThread::check_soft_timeout,
this));
364 soft_deadline_.async_wait(
365 boost::bind(&SickTiM55xEthernetAcquisitionThread::check_soft_timeout,
this));
float * _distances
Allocate a float array and copy your distance values measured in meters here.
fawkes::Mutex * _data_mutex
Lock while writing to distances or echoes array or marking new data.
bool _new_data
Set to true in your loop if new data is available.
void reset_distances()
Reset all distance values to NaN.
float * _echoes
Allocate a float array and copy your echo values here.
fawkes::Time * _timestamp
Time when the most recent data was received.
void reset_echoes()
Reset all distance values to NaN.
Laser acqusition thread for Sick TiM55x laser range finders.
void read_common_config()
Read common configuration parameters.
void resync()
Resynchronize to laser data.
std::string cfg_prefix_
Configuration path prefix for this configuration.
void init_device()
Initialize device.
virtual void pre_init(fawkes::Configuration *config, fawkes::Logger *logger)
Pre initialization.
void parse_datagram(const unsigned char *datagram, size_t datagram_length)
Parse incoming message from device.
virtual void init()
Initialize the thread.
virtual void loop()
Code to execute in the thread.
virtual void finalize()
Finalize the thread.
SickTiM55xEthernetAcquisitionThread(std::string &cfg_name, std::string &cfg_prefix)
Constructor.
Configuration * config
This is the Configuration member used to access the configuration.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
Base class for exceptions in Fawkes.
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
virtual void log_error(const char *component, const char *format,...)=0
Log error message.
Logger * logger
This is the Logger member used to access the logger.
Mutex mutual exclusion lock.
void lock()
Lock this mutex.
void unlock()
Unlock the mutex.
const char * name() const
Get name of thread.
void yield()
Yield the processor to another thread or process.
void set_name(const char *format,...)
Set name of thread.
Time & stamp()
Set this time to the current time.
Fawkes library namespace.