channel.hh
Go to the documentation of this file.
00001 
00036 #ifndef _LibMultiSense_details_channel_hh
00037 #define _LibMultiSense_details_channel_hh
00038 
00039 #include "MultiSenseChannel.hh"
00040 
00041 #include "details/utility/Portability.hh"
00042 #include "details/utility/Thread.hh"
00043 #include "details/utility/BufferStream.hh"
00044 #include "details/utility/Units.hh"
00045 #include "details/listeners.hh"
00046 #include "details/signal.hh"
00047 #include "details/storage.hh"
00048 #include "details/wire/Protocol.h"
00049 #include "details/wire/ImageMetaMessage.h"
00050 #include "details/wire/StatusResponseMessage.h"
00051 #include "details/wire/VersionResponseMessage.h"
00052 
00053 #ifndef WIN32
00054 #include <netinet/ip.h>
00055 #include <unistd.h>
00056 #endif
00057 
00058 #include <vector>
00059 #include <list>
00060 #include <set>
00061 #include <map>
00062 #include <iostream>
00063 #include <fstream>
00064 
00065 namespace crl {
00066 namespace multisense {
00067 namespace details {
00068 
00069 //
00070 // The implementation details
00071 
00072 class impl : public Channel {
00073 public:
00074 
00075     //
00076     // Construction
00077 
00078     impl(const std::string& address);
00079     ~impl();
00080 
00081     //
00082     // Public API
00083 
00084     virtual Status addIsolatedCallback   (image::Callback callback,
00085                                           DataSource      imageSourceMask,
00086                                           void           *userDataP);
00087     virtual Status addIsolatedCallback   (lidar::Callback callback,
00088                                           void           *userDataP);
00089     virtual Status addIsolatedCallback   (pps::Callback   callback,
00090                                           void           *userDataP);
00091     virtual Status addIsolatedCallback   (imu::Callback   callback,
00092                                           void           *userDataP);
00093 
00094     virtual Status removeIsolatedCallback(image::Callback callback);
00095     virtual Status removeIsolatedCallback(lidar::Callback callback);
00096     virtual Status removeIsolatedCallback(pps::Callback   callback);
00097     virtual Status removeIsolatedCallback(imu::Callback   callback);
00098 
00099     virtual void*  reserveCallbackBuffer ();
00100     virtual Status releaseCallbackBuffer (void *referenceP);
00101 
00102     virtual Status networkTimeSynchronization(bool enabled);
00103 
00104     virtual Status startStreams          (DataSource mask);
00105     virtual Status stopStreams           (DataSource mask);
00106     virtual Status getEnabledStreams     (DataSource& mask);
00107 
00108     virtual Status startDirectedStream   (const DirectedStream& stream);
00109     virtual Status startDirectedStreams  (const std::vector<DirectedStream>& streams);
00110     virtual Status stopDirectedStream    (const DirectedStream& stream);
00111     virtual Status getDirectedStreams    (std::vector<DirectedStream>& streams);
00112     virtual Status maxDirectedStreams    (uint32_t& maximum);
00113 
00114     virtual Status setTriggerSource      (TriggerSource s);
00115 
00116     virtual Status setMotorSpeed         (float rpm);
00117 
00118     virtual Status getLightingConfig     (lighting::Config& c);
00119     virtual Status setLightingConfig     (const lighting::Config& c);
00120 
00121     virtual Status getLightingSensorStatus (lighting::SensorStatus& status);
00122 
00123     virtual Status getSensorVersion      (VersionType& version);
00124     virtual Status getApiVersion         (VersionType& version);
00125     virtual Status getVersionInfo        (system::VersionInfo& v);
00126 
00127     virtual Status getImageConfig        (image::Config& c);
00128     virtual Status setImageConfig        (const image::Config& c);
00129 
00130     virtual Status getImageCalibration   (image::Calibration& c);
00131     virtual Status setImageCalibration   (const image::Calibration& c);
00132 
00133         virtual Status getSensorCalibration   (image::SensorCalibration& c);
00134         virtual Status setSensorCalibration   (const image::SensorCalibration& c);
00135 
00136         virtual Status getTransmitDelay   (image::TransmitDelay& c);
00137         virtual Status setTransmitDelay   (const image::TransmitDelay& c);
00138 
00139     virtual Status getLidarCalibration   (lidar::Calibration& c);
00140     virtual Status setLidarCalibration   (const lidar::Calibration& c);
00141 
00142     virtual Status getImageHistogram     (int64_t frameId, image::Histogram& histogram);
00143 
00144     virtual Status getDeviceModes        (std::vector<system::DeviceMode>& modes);
00145 
00146     virtual Status getMtu                (int32_t& mtu);
00147     virtual Status setMtu                (int32_t mtu);
00148 
00149     virtual Status getNetworkConfig      (system::NetworkConfig& c);
00150     virtual Status setNetworkConfig      (const system::NetworkConfig& c);
00151 
00152     virtual Status getDeviceInfo         (system::DeviceInfo& info);
00153     virtual Status setDeviceInfo         (const std::string& key,
00154                                           const system::DeviceInfo& i);
00155 
00156     virtual Status getDeviceStatus       (system::StatusMessage& status);
00157 
00158     virtual Status getExternalCalibration (system::ExternalCalibration& calibration);
00159     virtual Status setExternalCalibration (const system::ExternalCalibration& calibration);
00160 
00161     virtual Status flashBitstream        (const std::string& file);
00162     virtual Status flashFirmware         (const std::string& file);
00163 
00164     virtual Status verifyBitstream       (const std::string& file);
00165     virtual Status verifyFirmware        (const std::string& file);
00166 
00167     virtual Status getImuInfo            (uint32_t& maxSamplesPerMessage,
00168                                           std::vector<imu::Info>& info);
00169     virtual Status getImuConfig          (uint32_t& samplesPerMessage,
00170                                           std::vector<imu::Config>& c);
00171     virtual Status setImuConfig          (bool storeSettingsInFlash,
00172                                           uint32_t samplesPerMessage,
00173                                           const std::vector<imu::Config>& c);
00174 
00175     virtual Status getLargeBufferDetails (uint32_t& bufferCount,
00176                                           uint32_t& bufferSize);
00177     virtual Status setLargeBuffers       (const std::vector<uint8_t*>& buffers,
00178                                           uint32_t                     bufferSize);
00179     virtual Status getLocalUdpPort       (uint16_t& port);
00180 
00181 private:
00182 
00183     //
00184     // A handler prototype for custom UDP datagram reassembly
00185 
00186     typedef void (*UdpAssembler)(utility::BufferStreamWriter& stream,
00187                                  const uint8_t               *dataP,
00188                                  uint32_t                     offset,
00189                                  uint32_t                     length);
00190 
00191     //
00192     // The version of this API
00193 
00194     static CRL_CONSTEXPR VersionType API_VERSION = 0x0308; // 3.8
00195 
00196     //
00197     // Misc. internal constants
00198 
00199     static CRL_CONSTEXPR uint32_t MAX_MTU_SIZE               = 9000;
00200     static CRL_CONSTEXPR uint16_t DEFAULT_SENSOR_TX_PORT     = 9001;
00201     static CRL_CONSTEXPR uint32_t RX_POOL_LARGE_BUFFER_SIZE  = (10 * (1024 * 1024));
00202     static CRL_CONSTEXPR uint32_t RX_POOL_LARGE_BUFFER_COUNT = 50;
00203     static CRL_CONSTEXPR uint32_t RX_POOL_SMALL_BUFFER_SIZE  = (10 * (1024));
00204     static CRL_CONSTEXPR uint32_t RX_POOL_SMALL_BUFFER_COUNT = 100;
00205 
00206     static CRL_CONSTEXPR double DEFAULT_ACK_TIMEOUT ()         { return 0.2; }
00207     static CRL_CONSTEXPR uint32_t DEFAULT_ACK_ATTEMPTS       = 5;
00208     static CRL_CONSTEXPR uint32_t IMAGE_META_CACHE_DEPTH     = 20;
00209     static CRL_CONSTEXPR uint32_t UDP_TRACKER_CACHE_DEPTH    = 10;
00210     static CRL_CONSTEXPR uint32_t TIME_SYNC_OFFSET_DECAY     = 8;
00211 
00212     //
00213     // We must protect ourselves from user callbacks misbehaving
00214     // and gobbling up all of our RX buffers
00215     //
00216     // These define the maximum number of datums that we will
00217     // queue up in a user dispatch thread.
00218 
00219     static CRL_CONSTEXPR uint32_t MAX_USER_IMAGE_QUEUE_SIZE = 5;
00220     static CRL_CONSTEXPR uint32_t MAX_USER_LASER_QUEUE_SIZE = 20;
00221 
00222     //
00223     // PPS and IMU callbacks do not reserve an RX buffer, so queue
00224     // depths are limited by RAM (via heap.)
00225 
00226     static CRL_CONSTEXPR uint32_t MAX_USER_PPS_QUEUE_SIZE = 2;
00227     static CRL_CONSTEXPR uint32_t MAX_USER_IMU_QUEUE_SIZE = 50;
00228 
00229     //
00230     // The maximum number of directed streams
00231 
00232     static CRL_CONSTEXPR uint32_t MAX_DIRECTED_STREAMS = 8;
00233 
00234     //
00235     // A re-assembler for multi-packet messages
00236 
00237     class UdpTracker {
00238     public:
00239 
00240         UdpTracker(uint32_t                     t,
00241                    UdpAssembler                 a,
00242                    utility::BufferStreamWriter& s) :
00243             m_totalBytesInMessage(t),
00244             m_bytesAssembled(0),
00245             m_packetsAssembled(0),
00246             m_lastByteOffset(-1),
00247             m_assembler(a),
00248             m_stream(s) {};
00249 
00250         utility::BufferStreamWriter& stream() { return m_stream;           };
00251         uint32_t packets()                    { return m_packetsAssembled; };
00252 
00253         bool assemble(uint32_t       bytes,
00254                       uint32_t       offset,
00255                       const uint8_t *dataP) {
00256 
00257             if (offset <= m_lastByteOffset)
00258                 CRL_EXCEPTION("out-of-order or duplicate packet");
00259 
00260             m_assembler(m_stream, dataP, offset, bytes);
00261 
00262             m_bytesAssembled   += bytes;
00263             m_lastByteOffset    = offset;
00264             m_packetsAssembled ++;
00265 
00266             if (m_bytesAssembled == m_totalBytesInMessage)
00267                 return true;
00268             return false;
00269         }
00270 
00271     private:
00272 
00273         uint32_t                    m_totalBytesInMessage;
00274         uint32_t                    m_bytesAssembled;
00275         uint32_t                    m_packetsAssembled;
00276         int64_t                     m_lastByteOffset;
00277         UdpAssembler                m_assembler;
00278         utility::BufferStreamWriter m_stream;
00279     };
00280 
00281     //
00282     // The socket identifier and local port
00283 
00284     int32_t  m_serverSocket;
00285     uint16_t m_serverSocketPort;
00286 
00287     //
00288     // The address of the sensor
00289 
00290     struct sockaddr_in m_sensorAddress;
00291 
00292     //
00293     // The operating MTU of the sensor
00294 
00295     int32_t m_sensorMtu;
00296 
00297     //
00298     // A buffer to receive incoming UDP packets
00299 
00300     std::vector<uint8_t> m_incomingBuffer;
00301 
00302     //
00303     // Sequence ID for multi-packet message reassembly
00304 
00305     uint16_t m_txSeqId;
00306     int32_t  m_lastRxSeqId;
00307     int64_t  m_unWrappedRxSeqId;
00308 
00309     //
00310     // A cache to track incoming messages by sequence ID
00311 
00312     DepthCache<int64_t, UdpTracker> m_udpTrackerCache;
00313 
00314     //
00315     // A pool of RX buffers, to reduce the amount of internal copying
00316 
00317     typedef std::vector<utility::BufferStreamWriter*> BufferPool;
00318 
00319     BufferPool m_rxLargeBufferPool;
00320     BufferPool m_rxSmallBufferPool;
00321 
00322     //
00323     // A cache of image meta data
00324 
00325     DepthCache<int64_t, wire::ImageMeta> m_imageMetaCache;
00326 
00327     //
00328     // A map of custom UDP assemblers
00329 
00330     typedef std::map<wire::IdType, UdpAssembler> UdpAssemblerMap;
00331 
00332     UdpAssemblerMap m_udpAssemblerMap;
00333 
00334     //
00335     // Mutex for callback registration and dispatching
00336 
00337     utility::Mutex m_dispatchLock;
00338 
00339     //
00340     // Mutex for stream control
00341 
00342     utility::Mutex m_streamLock;
00343 
00344     //
00345     // A flag to shut down the internal threads
00346 
00347     bool m_threadsRunning;
00348 
00349     //
00350     // Internal UDP reception thread
00351 
00352     utility::Thread *m_rxThreadP;
00353     utility::Mutex   m_rxLock;
00354 
00355     //
00356     // Internal status thread
00357 
00358     utility::Thread *m_statusThreadP;
00359 
00360     //
00361     // The lists of user callbacks
00362 
00363     std::list<ImageListener*> m_imageListeners;
00364     std::list<LidarListener*> m_lidarListeners;
00365     std::list<PpsListener*>   m_ppsListeners;
00366     std::list<ImuListener*>   m_imuListeners;
00367 
00368     //
00369     // A message signal interface
00370 
00371     MessageWatch m_watch;
00372 
00373     //
00374     // A message storage interface
00375 
00376     MessageMap m_messages;
00377 
00378     //
00379     // The mask of currently enabled streams (desired)
00380 
00381     DataSource m_streamsEnabled;
00382 
00383     //
00384     // The current sensor time offset
00385 
00386     utility::Mutex m_timeLock;
00387     bool           m_timeOffsetInit;
00388     double         m_timeOffset;
00389     bool           m_networkTimeSyncEnabled;
00390 
00391     //
00392     // Cached version info from the device
00393 
00394     wire::VersionResponse m_sensorVersion;
00395 
00396     //
00397     // Cached StatusResponseMessage from the MultiSense
00398 
00399     wire::StatusResponse m_statusResponseMessage;
00400 
00401     //
00402     // Private procedures
00403 
00404     template<class T, class U> Status waitData(const T&      command,
00405                                                U&            data,
00406                                                const double& timeout=DEFAULT_ACK_TIMEOUT(),
00407                                                int32_t       attempts=DEFAULT_ACK_ATTEMPTS);
00408 #if defined (_MSC_VER)
00409     template<class T> Status          waitAck (const T&      msg,
00410                                                wire::IdType  id,
00411                                                const double& timeout,
00412                                                int32_t       attempts);
00413     template<class T> Status          waitAck (const T&      msg) {
00414         return waitAck (msg, MSG_ID(T::ID), double(DEFAULT_ACK_TIMEOUT()), DEFAULT_ACK_ATTEMPTS);
00415     }
00416 #else
00417     template<class T> Status          waitAck (const T&      msg,
00418                                                wire::IdType  id=MSG_ID(T::ID),
00419                                                const double& timeout=DEFAULT_ACK_TIMEOUT(),
00420                                                int32_t       attempts=DEFAULT_ACK_ATTEMPTS);
00421 #endif
00422 
00423     template<class T> void       publish      (const T& message);
00424     void                         publish      (const utility::BufferStreamWriter& stream);
00425     void                         dispatch     (utility::BufferStreamWriter& buffer);
00426     void                         dispatchImage(utility::BufferStream& buffer,
00427                                                image::Header&         header);
00428     void                         dispatchLidar(utility::BufferStream& buffer,
00429                                                lidar::Header&         header);
00430     void                         dispatchPps  (pps::Header& header);
00431     void                         dispatchImu  (imu::Header& header);
00432 
00433 
00434     utility::BufferStreamWriter& findFreeBuffer  (uint32_t messageLength);
00435     const int64_t&               unwrapSequenceId(uint16_t id);
00436     UdpAssembler                 getUdpAssembler (const uint8_t *udpDatagramP,
00437                                                   uint32_t       length);
00438 
00439     void                         eraseFlashRegion          (uint32_t region);
00440     void                         programOrVerifyFlashRegion(std::ifstream& file,
00441                                                             uint32_t       operation,
00442                                                             uint32_t       region);
00443     Status                       doFlashOp                 (const std::string& filename,
00444                                                             uint32_t           operation,
00445                                                             uint32_t           region);
00446 
00447     void                         applySensorTimeOffset(const double& offset);
00448     double                       sensorToLocalTime    (const double& sensorTime);
00449     void                         sensorToLocalTime    (const double& sensorTime,
00450                                                        uint32_t&     seconds,
00451                                                        uint32_t&     microseconds);
00452 
00453     void                         cleanup();
00454     void                         bind   ();
00455     void                         handle ();
00456 
00457     //
00458     // Static members
00459 
00460     static wire::SourceType      sourceApiToWire(DataSource mask);
00461     static DataSource            sourceWireToApi(wire::SourceType mask);
00462     static uint32_t              hardwareApiToWire(uint32_t h);
00463     static uint32_t              hardwareWireToApi(uint32_t h);
00464     static uint32_t              imagerApiToWire(uint32_t h);
00465     static uint32_t              imagerWireToApi(uint32_t h);
00466 #if WIN32
00467     static DWORD WINAPI          rxThread       (void *userDataP);
00468     static DWORD WINAPI          statusThread   (void *userDataP);
00469 #else
00470     static void                 *rxThread       (void *userDataP);
00471     static void                 *statusThread   (void *userDataP);
00472 #endif
00473 };
00474 
00475 
00476 }}}; // namespaces
00477 
00478 #endif // LibMultiSense_details_channel_hh


multisense_lib
Author(s):
autogenerated on Fri Apr 5 2019 02:28:24