23 #include "sick_tim55x_ethernet_aqt.h" 25 #include <core/threading/mutex.h> 26 #include <core/threading/mutex_locker.h> 27 #include <utils/misc/string_split.h> 28 #include <utils/math/angle.h> 30 #include <boost/lexical_cast.hpp> 31 #include <boost/lambda/bind.hpp> 32 #include <boost/lambda/lambda.hpp> 33 #if BOOST_VERSION < 104800 34 # include <boost/bind.hpp> 43 #define RECONNECT_INTERVAL 1000 44 #define RECEIVE_TIMEOUT 500 58 std::string &cfg_prefix)
60 socket_(io_service_), deadline_(io_service_), soft_deadline_(io_service_)
62 set_name(
"SickTiM55x(%s)", cfg_name.c_str());
73 socket_mutex_ =
new Mutex();
75 deadline_.expires_at(boost::posix_time::pos_infin);
78 soft_deadline_.expires_at(boost::posix_time::pos_infin);
100 if (socket_.is_open()) {
102 deadline_.expires_from_now(boost::posix_time::milliseconds(RECEIVE_TIMEOUT));
104 ec_ = boost::asio::error::would_block;
107 boost::asio::async_read_until(socket_, input_buffer_,
'\03',
108 #
if BOOST_VERSION >= 104800
109 (boost::lambda::var(ec_) = boost::lambda::_1,
110 boost::lambda::var(bytes_read_) = boost::lambda::_2));
113 &SickTiM55xEthernetAcquisitionThread::handle_read,
115 boost::asio::placeholders::error,
116 boost::asio::placeholders::bytes_transferred
120 do io_service_.run_one();
while (ec_ == boost::asio::error::would_block);
126 if (ec_.value() == boost::system::errc::operation_canceled) {
138 deadline_.expires_at(boost::posix_time::pos_infin);
140 unsigned char recv_buf[bytes_read_];
141 std::istream in_stream(&input_buffer_);
142 in_stream.read((
char *)recv_buf, bytes_read_);
144 if (bytes_read_ > 0) {
154 }
catch (boost::system::system_error &e) {
155 if (e.code() == boost::asio::error::eof) {
158 "Sick TiM55x/Ethernet connection lost, trying to reconnect");
169 usleep(RECONNECT_INTERVAL * 1000);
178 SickTiM55xEthernetAcquisitionThread::open_device()
181 boost::asio::ip::tcp::resolver resolver(io_service_);
182 boost::asio::ip::tcp::resolver::query
183 query(cfg_host_, cfg_port_);
184 boost::asio::ip::tcp::resolver::iterator iter = resolver.resolve(query);
190 deadline_.expires_from_now(boost::posix_time::seconds(5));
192 boost::system::error_code ec;
194 for (; iter != boost::asio::ip::tcp::resolver::iterator(); ++iter) {
196 ec_ = boost::asio::error::would_block;
197 #if BOOST_VERSION >= 104800 198 socket_.async_connect(iter->endpoint(), boost::lambda::var(ec_) = boost::lambda::_1);
200 socket_.async_connect(iter->endpoint(),
201 boost::bind(&SickTiM55xEthernetAcquisitionThread::handle_read,
this,
202 boost::asio::placeholders::error, 0));
206 do io_service_.run_one();
while (ec_ == boost::asio::error::would_block);
209 if (ec_ || ! socket_.is_open()) {
210 if (ec_.value() == boost::system::errc::operation_canceled) {
211 throw Exception(
"Sick TiM55X Ethernet: connection timed out");
213 throw Exception(
"Connection failed: %s", ec_.message().c_str());
216 deadline_.expires_at(boost::posix_time::pos_infin);
218 }
catch (boost::system::system_error &e) {
219 throw Exception(
"Connection failed: %s", e.what());
225 SickTiM55xEthernetAcquisitionThread::close_device()
227 boost::system::error_code err;
228 if (socket_.is_open()) {
229 socket_.shutdown(boost::asio::ip::tcp::socket::shutdown_both, err);
236 SickTiM55xEthernetAcquisitionThread::flush_device()
238 if (socket_.is_open()) {
240 soft_deadline_.expires_from_now(boost::posix_time::milliseconds(RECEIVE_TIMEOUT));
242 ec_ = boost::asio::error::would_block;
245 boost::asio::async_read_until(socket_, input_buffer_,
'\03',
246 #
if BOOST_VERSION >= 104800
247 (boost::lambda::var(ec_) = boost::lambda::_1,
248 boost::lambda::var(bytes_read_) = boost::lambda::_2));
251 &SickTiM55xEthernetAcquisitionThread::handle_read,
253 boost::asio::placeholders::error,
254 boost::asio::placeholders::bytes_transferred
258 do io_service_.run_one();
while (ec_ == boost::asio::error::would_block);
260 }
while (bytes_read_ > 0);
261 soft_deadline_.expires_from_now(boost::posix_time::pos_infin);
262 }
catch (boost::system::system_error &e) {
270 SickTiM55xEthernetAcquisitionThread::send_with_reply(
const char *request,
275 int request_length = strlen(request);
278 boost::asio::write(socket_, boost::asio::buffer(request, request_length));
280 deadline_.expires_from_now(boost::posix_time::milliseconds(RECEIVE_TIMEOUT));
282 ec_ = boost::asio::error::would_block;
284 boost::asio::async_read_until(socket_, input_buffer_,
'\03',
285 #
if BOOST_VERSION >= 104800
286 (boost::lambda::var(ec_) = boost::lambda::_1,
287 boost::lambda::var(bytes_read_) = boost::lambda::_2));
290 &SickTiM55xEthernetAcquisitionThread::handle_read,
292 boost::asio::placeholders::error,
293 boost::asio::placeholders::bytes_transferred
297 do io_service_.run_one();
while (ec_ == boost::asio::error::would_block);
300 if (ec_.value() == boost::system::errc::operation_canceled) {
301 throw Exception(
"Timeout waiting for message reply");
303 throw Exception(
"Failed to read reply: %s", ec_.message().c_str());
307 deadline_.expires_at(boost::posix_time::pos_infin);
310 char recv_buf[bytes_read_];
311 std::istream in_stream(&input_buffer_);
312 in_stream.read(recv_buf, bytes_read_);
313 *reply = std::string(recv_buf, bytes_read_);
315 input_buffer_.consume(bytes_read_);
317 }
catch (boost::system::system_error &e) {
318 throw Exception(
"Sick TiM55x/Ethernet failed I/O: %s", e.what());
329 SickTiM55xEthernetAcquisitionThread::check_deadline()
331 if (deadline_.expires_at() <= boost::asio::deadline_timer::traits_type::now()) {
333 deadline_.expires_at(boost::posix_time::pos_infin);
336 #if BOOST_VERSION >= 104800 337 deadline_.async_wait(boost::lambda::bind(&SickTiM55xEthernetAcquisitionThread::check_deadline,
this));
339 deadline_.async_wait(boost::bind(&SickTiM55xEthernetAcquisitionThread::check_deadline,
this));
349 SickTiM55xEthernetAcquisitionThread::check_soft_timeout()
351 if (soft_deadline_.expires_at() <= boost::asio::deadline_timer::traits_type::now()) {
353 soft_deadline_.expires_at(boost::posix_time::pos_infin);
356 #if BOOST_VERSION >= 104800 357 soft_deadline_.async_wait(boost::lambda::bind(&SickTiM55xEthernetAcquisitionThread::check_soft_timeout,
this));
359 soft_deadline_.async_wait(boost::bind(&SickTiM55xEthernetAcquisitionThread::check_soft_timeout,
this));
Laser acqusition thread for Sick TiM55x laser range finders.
void resync()
Resynchronize to laser data.
Fawkes library namespace.
void unlock()
Unlock the mutex.
void init_device()
Initialize device.
Logger * logger
This is the Logger member used to access the logger.
void reset_distances()
Reset all distance values to NaN.
virtual void finalize()
Finalize the thread.
virtual void pre_init(fawkes::Configuration *config, fawkes::Logger *logger)
Pre initialization.
fawkes::Time * _timestamp
Time when the most recent data was received.
fawkes::Mutex * _data_mutex
Lock while writing to distances or echoes array or marking new data.
void parse_datagram(const unsigned char *datagram, size_t datagram_length)
Parse incoming message from device.
void reset_echoes()
Reset all distance values to NaN.
void set_name(const char *format,...)
Set name of thread.
Base class for exceptions in Fawkes.
float * _distances
Allocate a float array and copy your distance values measured in meters here.
const char * name() const
Get name of thread.
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.
bool _new_data
Set to true in your loop if new data is available.
SickTiM55xEthernetAcquisitionThread(std::string &cfg_name, std::string &cfg_prefix)
Constructor.
virtual void init()
Initialize the thread.
void yield()
Yield the processor to another thread or process.
void read_common_config()
Read common configuration parameters.
virtual void loop()
Code to execute in the thread.
void lock()
Lock this mutex.
Time & stamp()
Set this time to the current time.
Mutex mutual exclusion lock.
Configuration * config
This is the Configuration member used to access the configuration.
std::string cfg_prefix_
Configuration path prefix for this configuration.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.