00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016 #ifndef SICK_BUFFER_MONITOR
00017 #define SICK_BUFFER_MONITOR
00018
00019
00020 #include "SickConfig.hh"
00021
00022
00023 #include <iostream>
00024 #include <pthread.h>
00025 #include "SickException.hh"
00026
00027
00028 namespace SickToolbox {
00029
00030
00031
00032
00033 template < class SICK_MONITOR_CLASS, class SICK_MSG_CLASS >
00034 class SickBufferMonitor {
00035
00036 public:
00037
00038
00039 SickBufferMonitor( SICK_MONITOR_CLASS * const monitor_instance ) throw( SickThreadException );
00040
00041
00042 void SetDataStream( const unsigned int sick_fd ) throw( SickThreadException );
00043
00044
00045 void StartMonitor( const unsigned int sick_fd ) throw( SickThreadException );
00046
00047
00048 bool GetNextMessageFromMonitor( SICK_MSG_CLASS &sick_message ) throw( SickThreadException );
00049
00050
00051 void StopMonitor( ) throw( SickThreadException );
00052
00053
00054 void AcquireDataStream( ) throw( SickThreadException );
00055
00056
00057 void GetNextMessageFromDataStream( SICK_MSG_CLASS &sick_message );
00058
00059
00060 void ReleaseDataStream( ) throw( SickThreadException );
00061
00062
00063 ~SickBufferMonitor( ) throw( SickThreadException );
00064
00065 protected:
00066
00067
00068 unsigned int _sick_fd;
00069
00070
00071 void _readBytes( uint8_t * const dest_buffer, const int num_bytes_to_read, const unsigned int timeout_value = 0 ) const throw ( SickTimeoutException, SickIOException );
00072
00073 private:
00074
00075
00076 SICK_MONITOR_CLASS *_sick_monitor_instance;
00077
00078
00079 bool _continue_grabbing;
00080
00081
00082 pthread_t _monitor_thread_id;
00083
00084
00085 pthread_mutex_t _container_mutex;
00086
00087
00088 pthread_mutex_t _stream_mutex;
00089
00090
00091 SICK_MSG_CLASS _recv_msg_container;
00092
00093
00094 void _acquireMessageContainer( ) throw( SickThreadException );
00095
00096
00097 void _releaseMessageContainer( ) throw( SickThreadException );
00098
00099
00100 static void * _bufferMonitorThread( void * thread_args );
00101
00102 };
00103
00104
00105
00106
00107
00108 template < class SICK_MONITOR_CLASS, class SICK_MSG_CLASS >
00109 SickBufferMonitor< SICK_MONITOR_CLASS, SICK_MSG_CLASS >::SickBufferMonitor( SICK_MONITOR_CLASS * const monitor_instance ) throw( SickThreadException ) :
00110 _sick_monitor_instance(monitor_instance), _continue_grabbing(true), _monitor_thread_id(0) {
00111
00112
00113 if (pthread_mutex_init(&_container_mutex,NULL) != 0) {
00114 throw SickThreadException("SickBufferMonitor::SickBufferMonitor: pthread_mutex_init() failed!");
00115 }
00116
00117
00118 if (pthread_mutex_init(&_stream_mutex,NULL) != 0) {
00119 throw SickThreadException("SickBufferMonitor::SickBufferMonitor: pthread_mutex_init() failed!");
00120 }
00121
00122 }
00123
00124
00125
00126
00127
00128 template < class SICK_MONITOR_CLASS, class SICK_MSG_CLASS >
00129 void SickBufferMonitor< SICK_MONITOR_CLASS, SICK_MSG_CLASS >::SetDataStream( const unsigned int sick_fd ) throw ( SickThreadException ) {
00130
00131 try {
00132
00133
00134 AcquireDataStream();
00135
00136
00137 _sick_fd = sick_fd;
00138
00139
00140 ReleaseDataStream();
00141
00142 }
00143
00144
00145 catch(SickThreadException &sick_thread_exception) {
00146 std::cerr << sick_thread_exception.what() << std::endl;
00147 }
00148
00149
00150 catch(...) {
00151 std::cerr << "SickBufferMonitor::SetDataStream: Unknown exception!" << std::endl;
00152 throw;
00153 }
00154
00155 }
00156
00157
00158
00159
00160
00161 template < class SICK_MONITOR_CLASS, class SICK_MSG_CLASS >
00162 void SickBufferMonitor< SICK_MONITOR_CLASS, SICK_MSG_CLASS >::StartMonitor( const unsigned int sick_fd ) throw ( SickThreadException ) {
00163
00164
00165 _sick_fd = sick_fd;
00166
00167
00168 if (pthread_create(&_monitor_thread_id,NULL,SickBufferMonitor< SICK_MONITOR_CLASS, SICK_MSG_CLASS >::_bufferMonitorThread,_sick_monitor_instance) != 0) {
00169 throw SickThreadException("SickBufferMonitor::StartMonitor: pthread_create() failed!");
00170 }
00171
00172 }
00173
00174
00175
00176
00177
00178
00179 template < class SICK_MONITOR_CLASS, class SICK_MSG_CLASS >
00180 bool SickBufferMonitor< SICK_MONITOR_CLASS, SICK_MSG_CLASS >::GetNextMessageFromMonitor( SICK_MSG_CLASS &sick_message ) throw( SickThreadException ) {
00181
00182 bool acquired_message = false;
00183
00184 try {
00185
00186
00187 _acquireMessageContainer();
00188
00189
00190 if (_recv_msg_container.IsPopulated()) {
00191
00192
00193 sick_message = _recv_msg_container;
00194 _recv_msg_container.Clear();
00195
00196
00197 acquired_message = true;
00198 }
00199
00200
00201 _releaseMessageContainer();
00202
00203 }
00204
00205
00206 catch(SickThreadException &sick_thread_exception) {
00207 std::cerr << sick_thread_exception.what() << std::endl;
00208 throw;
00209 }
00210
00211
00212 catch(...) {
00213 std::cerr << "SickBufferMonitor::CheckMessageContainer: Unknown exception!" << std::endl;
00214 throw;
00215 }
00216
00217
00218 return acquired_message;
00219 }
00220
00221
00222
00223
00224
00225 template < class SICK_MONITOR_CLASS, class SICK_MSG_CLASS >
00226 void SickBufferMonitor< SICK_MONITOR_CLASS, SICK_MSG_CLASS >::StopMonitor( ) throw ( SickThreadException ) {
00227
00228 try {
00229
00230
00231 void *monitor_result = NULL;
00232
00233
00234 AcquireDataStream();
00235 _continue_grabbing = false;
00236 ReleaseDataStream();
00237
00238
00239 if (pthread_join(_monitor_thread_id,&monitor_result) != 0) {
00240 throw SickThreadException("SickBufferMonitor::StopMonitor: pthread_join() failed!");
00241 }
00242
00243 }
00244
00245
00246 catch(SickThreadException &sick_thread_exception) {
00247 std::cerr << sick_thread_exception.what() << std::endl;
00248 }
00249
00250
00251 catch(...) {
00252 std::cerr << "SickBufferMonitor::StopMonitor: Unknown exception!" << std::endl;
00253 throw;
00254 }
00255
00256 }
00257
00258
00259
00260
00261 template < class SICK_MONITOR_CLASS, class SICK_MSG_CLASS >
00262 void SickBufferMonitor< SICK_MONITOR_CLASS, SICK_MSG_CLASS >::AcquireDataStream( ) throw( SickThreadException ) {
00263
00264
00265 if (pthread_mutex_lock(&_stream_mutex) != 0) {
00266 throw SickThreadException("SickBufferMonitor::AcquireDataStream: pthread_mutex_lock() failed!");
00267 }
00268
00269 }
00270
00271
00272
00273
00274 template < class SICK_MONITOR_CLASS, class SICK_MSG_CLASS >
00275 void SickBufferMonitor< SICK_MONITOR_CLASS, SICK_MSG_CLASS >::ReleaseDataStream( ) throw( SickThreadException ) {
00276
00277
00278 if (pthread_mutex_unlock(&_stream_mutex) != 0) {
00279 throw SickThreadException("SickBufferMonitor::ReleaseDataStream: pthread_mutex_unlock() failed!");
00280 }
00281
00282 }
00283
00284
00285
00286
00287 template < class SICK_MONITOR_CLASS, class SICK_MSG_CLASS >
00288 SickBufferMonitor< SICK_MONITOR_CLASS, SICK_MSG_CLASS >::~SickBufferMonitor( ) throw( SickThreadException ) {
00289
00290
00291 if (pthread_mutex_destroy(&_container_mutex) != 0) {
00292 throw SickThreadException("SickBufferMonitor::~SickBufferMonitor: pthread_mutex_destroy() failed!");
00293 }
00294
00295
00296 if (pthread_mutex_destroy(&_stream_mutex) != 0) {
00297 throw SickThreadException("SickBufferMonitor::~SickBufferMonitor: pthread_mutex_destroy() failed!");
00298 }
00299
00300 }
00301
00302
00303
00304
00305 template < class SICK_MONITOR_CLASS, class SICK_MSG_CLASS >
00306 void SickBufferMonitor< SICK_MONITOR_CLASS, SICK_MSG_CLASS >::_acquireMessageContainer( ) throw( SickThreadException ) {
00307
00308
00309 if (pthread_mutex_lock(&_container_mutex) != 0) {
00310 throw SickThreadException("SickBufferMonitor::_acquireMessageContainer: pthread_mutex_lock() failed!");
00311 }
00312
00313 }
00314
00315
00316
00317
00318 template < class SICK_MONITOR_CLASS, class SICK_MSG_CLASS >
00319 void SickBufferMonitor< SICK_MONITOR_CLASS, SICK_MSG_CLASS >::_releaseMessageContainer( ) throw( SickThreadException ) {
00320
00321
00322 if (pthread_mutex_unlock(&_container_mutex) != 0) {
00323 throw SickThreadException("SickBufferMonitor::_releaseMessageContainer: pthread_mutex_unlock() failed!");
00324 }
00325
00326 }
00327
00328
00329
00330
00331
00332
00333
00334
00335 template< class SICK_MONITOR_CLASS, class SICK_MSG_CLASS >
00336 void SickBufferMonitor< SICK_MONITOR_CLASS, SICK_MSG_CLASS >::_readBytes( uint8_t * const dest_buffer, const int num_bytes_to_read, const unsigned int timeout_value ) const
00337 throw ( SickTimeoutException, SickIOException ) {
00338
00339
00340 int num_bytes_read = 0;
00341 int total_num_bytes_read = 0;
00342 int num_active_files = 0;
00343
00344 struct timeval timeout_val;
00345 fd_set file_desc_set;
00346
00347
00348 while ( total_num_bytes_read < num_bytes_to_read ) {
00349
00350
00351 FD_ZERO(&file_desc_set);
00352 FD_SET(_sick_fd,&file_desc_set);
00353
00354
00355 memset(&timeout_val,0,sizeof(timeout_val));
00356 timeout_val.tv_usec = timeout_value;
00357
00358
00359 num_active_files = select(getdtablesize(),&file_desc_set,0,0,(timeout_value > 0) ? &timeout_val : 0);
00360
00361
00362 if (num_active_files > 0) {
00363
00364
00365
00366
00367
00368
00369
00370 if (FD_ISSET(_sick_fd,&file_desc_set)) {
00371
00372
00373 num_bytes_read = read(_sick_fd,&dest_buffer[total_num_bytes_read],1);
00374
00375
00376 if (num_bytes_read > 0) {
00377 total_num_bytes_read += num_bytes_read;
00378 }
00379 else {
00380
00381 throw SickIOException("SickBufferMonitor::_readBytes: read() failed!");
00382 }
00383
00384 }
00385
00386 }
00387 else if (num_active_files == 0) {
00388
00389
00390 throw SickTimeoutException("SickBufferMonitor::_readBytes: select() timeout!");
00391
00392 }
00393 else {
00394
00395
00396 throw SickIOException("SickBufferMonitor::_readBytes: select() failed!");
00397
00398 }
00399
00400 }
00401
00402 }
00403
00404
00405
00406
00407
00408 template < class SICK_MONITOR_CLASS, class SICK_MSG_CLASS >
00409 void * SickBufferMonitor< SICK_MONITOR_CLASS, SICK_MSG_CLASS >::_bufferMonitorThread( void * thread_args ) {
00410
00411
00412 SICK_MSG_CLASS curr_message;
00413
00414
00415 SICK_MONITOR_CLASS *buffer_monitor = (SICK_MONITOR_CLASS *)thread_args;
00416
00417
00418 for (;;) {
00419
00420 try {
00421
00422
00423 curr_message.Clear();
00424
00425
00426 buffer_monitor->AcquireDataStream();
00427
00428 if (!buffer_monitor->_continue_grabbing) {
00429 buffer_monitor->ReleaseDataStream();
00430 break;
00431 }
00432
00433 buffer_monitor->GetNextMessageFromDataStream(curr_message);
00434 buffer_monitor->ReleaseDataStream();
00435
00436
00437 buffer_monitor->_acquireMessageContainer();
00438 buffer_monitor->_recv_msg_container = curr_message;
00439 buffer_monitor->_releaseMessageContainer();
00440
00441 }
00442
00443
00444 catch(SickIOException &sick_io_exception) {
00445 std::cerr << sick_io_exception.what() << std::endl;
00446 }
00447
00448
00449 catch(SickThreadException &sick_thread_exception) {
00450 std::cerr << sick_thread_exception.what() << std::endl;
00451 }
00452
00453
00454 catch(...) {
00455 std::cerr << "SickBufferMonitor::_bufferMonitorThread: Unknown exception!" << std::endl;
00456 }
00457
00458
00459 usleep(1000);
00460
00461 }
00462
00463
00464 return NULL;
00465
00466 }
00467
00468 }
00469
00470 #endif