sick_scan_common_tcp.h
Go to the documentation of this file.
00001 /*
00002  * Copyright (C) 2013, Freiburg University
00003  * All rights reserved.
00004  * 
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  * 
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of Osnabrück University nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  * 
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  *
00029  *  Created on: 15.11.2013
00030  *
00031  *      Authors:
00032  *         Christian Dornhege <c.dornhege@googlemail.com>
00033  */
00034 
00035 #ifndef SICK_TIM3XX_COMMON_TCP_H
00036 #define SICK_TIM3XX_COMMON_TCP_H
00037 
00038 #include <stdio.h>
00039 #include <stdlib.h>
00040 #include <string.h>
00041 #include <boost/asio.hpp>
00042 
00043 #undef NOMINMAX // to get rid off warning C4005: "NOMINMAX": Makro-Neudefinition
00044 
00045 #include "sick_scan_common.h"
00046 #include "sick_generic_parser.h"
00047 #include "template_queue.h"
00048 namespace sick_scan
00049 {
00050 /* class prepared for optimized time stamping */
00051 
00052 class DatagramWithTimeStamp
00053 {
00054 public:
00055                 DatagramWithTimeStamp(ros::Time timeStamp_, std::vector<unsigned char> datagram_)
00056                 {
00057        timeStamp = timeStamp_;
00058                          datagram = datagram_;
00059                 }
00060 // private:
00061           ros::Time timeStamp;
00062                 std::vector<unsigned char> datagram;
00063 };
00064 
00065 
00066 class SickScanCommonTcp : public SickScanCommon
00067     {
00068 public:
00069     static void disconnectFunctionS(void *obj);
00070 
00071     SickScanCommonTcp(const std::string &hostname, const std::string &port, int &timelimit, SickGenericParser *parser, char cola_dialect_id);
00072 
00073     virtual ~SickScanCommonTcp();
00074 
00075     static void readCallbackFunctionS(void *obj, UINT8 *buffer, UINT32 &numOfBytes);
00076 
00077     void readCallbackFunction(UINT8 *buffer, UINT32 &numOfBytes);
00078 
00079     void setReplyMode(int _mode);
00080 
00081     int getReplyMode();
00082 
00083                 void setEmulSensor(bool _emulFlag);
00084 
00085                 bool getEmulSensor();
00086 
00087                 bool stopScanData();
00088 
00089                 int numberOfDatagramInInputFifo();
00090         SopasEventMessage findFrameInReceiveBuffer();
00091         void processFrame(ros::Time timeStamp, SopasEventMessage& frame);
00092         // Queue<std::vector<unsigned char> > recvQueue;
00093     Queue<DatagramWithTimeStamp> recvQueue;
00094     UINT32 m_alreadyReceivedBytes;
00095     UINT32 m_lastPacketSize;
00096     UINT8  m_packetBuffer[480000];
00102     protected:
00103         void disconnectFunction();
00104 
00105                 void readCallbackFunctionOld(UINT8* buffer, UINT32& numOfBytes);
00106                 virtual int init_device();
00107 
00108         virtual int close_device();
00109 
00111         virtual int sendSOPASCommand(const char *request, std::vector<unsigned char> *reply, int cmdLen);
00112 
00114 
00121         virtual int
00122         get_datagram(ros::Time &recvTimeStamp, unsigned char *receiveBuffer, int bufferSize, int *actual_length,
00123                      bool isBinaryProtocol, int *numberOfRemainingFifoEntries);
00124 
00125         // Helpers for boost asio
00126         int readWithTimeout(size_t timeout_ms, char *buffer, int buffer_size, int *bytes_read = 0,
00127                             bool *exception_occured = 0, bool isBinary = false);
00128 
00129         void handleRead(boost::system::error_code error, size_t bytes_transfered);
00130 
00131         void checkDeadline();
00132 
00133     private:
00134 
00135 
00136 
00137                 // Response buffer
00138                 UINT32 m_numberOfBytesInResponseBuffer; 
00139                 UINT8 m_responseBuffer[1024]; 
00140                 Mutex m_receiveDataMutex; 
00141 
00142                                                                   // Receive buffer
00143                 UINT32 m_numberOfBytesInReceiveBuffer; 
00144                 UINT8 m_receiveBuffer[480000]; 
00145 
00146                 bool m_beVerbose;
00147                                 bool m_emulSensor;
00148 
00149         boost::asio::io_service io_service_;
00150         boost::asio::ip::tcp::socket socket_;
00151         boost::asio::deadline_timer deadline_;
00152         boost::asio::streambuf input_buffer_;
00153         boost::system::error_code ec_;
00154         size_t bytes_transfered_;
00155 
00156         std::string hostname_;
00157         std::string port_;
00158         int timelimit_;
00159         int m_replyMode;
00160 };
00161 
00162 
00163 } /* namespace sick_scan */
00164 #endif /* SICK_TIM3XX_COMMON_TCP_H */
00165 


sick_scan
Author(s): Michael Lehning , Jochen Sprickerhof , Martin Günther
autogenerated on Tue Jul 9 2019 05:05:35