57 namespace multisense {
65 m_serverSocketPort(0),
67 m_sensorMtu(MAX_MTU_SIZE),
68 m_incomingBuffer(MAX_MTU_SIZE),
71 m_unWrappedRxSeqId(0),
72 m_udpTrackerCache(UDP_TRACKER_CACHE_DEPTH, 0),
73 m_rxLargeBufferPool(),
74 m_rxSmallBufferPool(),
75 m_imageMetaCache(IMAGE_META_CACHE_DEPTH, 0),
79 m_threadsRunning(false),
82 m_statusThreadP(NULL),
91 m_timeOffsetInit(false),
93 m_networkTimeSyncEnabled(true),
98 int result = WSAStartup (MAKEWORD (0x02, 0x02), &wsaData);
106 struct hostent *hostP = gethostbyname(address.c_str());
109 address.c_str(), strerror(errno));
116 memcpy(&(addr.s_addr), hostP->h_addr, hostP->h_length);
155 CRL_EXCEPTION(
"failed to establish comms with the sensor at \"%s\"",
171 CRL_EXCEPTION(
"failed to request version info from sensor at \"%s\"",
193 std::list<ImageListener*>::const_iterator iti;
198 std::list<LidarListener*>::const_iterator itl;
203 std::list<PpsListener*>::const_iterator itp;
208 std::list<ImuListener*>::const_iterator itm;
214 BufferPool::const_iterator it;
257 u_long ioctl_arg = 1;
259 CRL_EXCEPTION(
"failed to make a socket non-blocking: %d",WSAGetLastError ());
273 if (0 != setsockopt(
m_serverSocket, SOL_SOCKET, SO_REUSEADDR, (
char*) &reuseSocket,
274 sizeof(reuseSocket)))
281 int bufferSize = 48 * 1024 * 1024;
283 if (0 != setsockopt(
m_serverSocket, SOL_SOCKET, SO_RCVBUF, (
char*) &bufferSize,
284 sizeof(bufferSize)) ||
285 0 != setsockopt(
m_serverSocket, SOL_SOCKET, SO_SNDBUF, (
char*) &bufferSize,
287 CRL_EXCEPTION(
"failed to adjust socket buffer sizes (%d bytes): %s",
288 bufferSize, strerror(errno));
293 struct sockaddr_in address;
295 address.sin_family = AF_INET;
296 address.sin_port = htons(0);
297 address.sin_addr.s_addr = htonl(INADDR_ANY);
300 CRL_EXCEPTION(
"failed to bind the server socket to system-assigned port: %s",
306 int len =
sizeof(address);
308 socklen_t len =
sizeof(address);
310 if (0 != getsockname(
m_serverSocket, (
struct sockaddr*) &address, &len))
331 header.sequenceIdentifier = InterlockedIncrement16((
short*)&
m_txSeqId);
334 header.sequenceIdentifier = __sync_fetch_and_add(&
m_txSeqId, 1);
337 header.byteOffset = 0;
346 if (static_cast<size_t>(ret) != stream.
tell())
347 CRL_EXCEPTION(
"error sending data to sensor, %d/%d bytes written: %s",
348 ret, stream.
tell(), strerror(errno));
414 CRL_DEBUG(
"unknown API hardware type \"%d\"\n", a);
429 CRL_DEBUG(
"unknown WIRE hardware type \"%d\"\n", w);
442 CRL_DEBUG(
"unknown API imager type \"%d\"\n", a);
455 CRL_DEBUG(
"unknown WIRE imager type \"%d\"\n", w);
492 uint32_t& microseconds)
495 seconds =
static_cast<uint32_t
>(corrected);
496 microseconds =
static_cast<uint32_t
>(1e6 * (corrected -
static_cast<double>(seconds)));
508 impl *selfP =
reinterpret_cast<impl*
>(userDataP);
532 if (ack.
wait(status, 0.010)) {
549 const double latency = (pong - ping) / 2.0;
554 const double offset = (ping + latency) - static_cast<double>(msg.
uptime);
563 }
catch (
const std::exception& e) {
575 usleep(static_cast<unsigned int> (1e6));
589 }
catch (
const std::exception& e) {
603 }
catch (
const std::exception& e) {
621 return "Unknown Error";
static CRL_CONSTEXPR DataSource Source_Disparity_Cost
std::list< ImageListener * > m_imageListeners
static uint32_t imagerWireToApi(uint32_t h)
#define CRL_EXCEPTION(fmt,...)
static uint32_t hardwareWireToApi(uint32_t h)
static CRL_CONSTEXPR SourceType SOURCE_DISPARITY
BufferPool m_rxLargeBufferPool
static CRL_CONSTEXPR DataSource Source_Disparity_Right
double sensorToLocalTime(const double &sensorTime)
static CRL_CONSTEXPR uint32_t HARDWARE_REV_MULTISENSE_SL
static CRL_CONSTEXPR DataSource Source_Jpeg_Left
static CRL_CONSTEXPR uint32_t IMAGER_TYPE_CMV4000_COLOR
static const char * statusString(Status status)
static CRL_CONSTEXPR DataSource Source_Disparity
struct sockaddr_in m_sensorAddress
static CRL_CONSTEXPR uint32_t IMAGER_TYPE_CMV2000_GREY
static CRL_CONSTEXPR SourceType SOURCE_DISPARITY_RIGHT
static CRL_CONSTEXPR DataSource Source_Chroma_Left
static CRL_CONSTEXPR uint32_t HARDWARE_REV_MULTISENSE_S21
UdpAssemblerMap m_udpAssemblerMap
static CRL_CONSTEXPR uint32_t HARDWARE_REV_MULTISENSE_ST21
static Channel * Create(const std::string &sensorAddress)
static CRL_CONSTEXPR Status Status_Unsupported
void applySensorTimeOffset(const double &offset)
wire::StatusResponse m_statusResponseMessage
static wire::SourceType sourceApiToWire(DataSource mask)
static CRL_CONSTEXPR DataSource Source_Luma_Left
std::list< PpsListener * > m_ppsListeners
std_msgs::Header * header(M &m)
static CRL_CONSTEXPR Status Status_Failed
static CRL_CONSTEXPR SourceType SOURCE_LUMA_LEFT
utility::Thread * m_rxThreadP
static CRL_CONSTEXPR uint16_t HEADER_VERSION
static CRL_CONSTEXPR uint32_t HARDWARE_REV_MULTISENSE_S7
static CRL_CONSTEXPR SourceType SOURCE_CHROMA_RIGHT
static CRL_CONSTEXPR uint32_t RX_POOL_SMALL_BUFFER_COUNT
static CRL_CONSTEXPR uint32_t RX_POOL_SMALL_BUFFER_SIZE
static CRL_CONSTEXPR uint32_t HARDWARE_REV_MULTISENSE_SL
static CRL_CONSTEXPR SourceType SOURCE_LUMA_RECT_RIGHT
static CRL_CONSTEXPR DataSource Source_Lidar_Scan
static CRL_CONSTEXPR uint32_t HARDWARE_REV_MULTISENSE_M
static CRL_CONSTEXPR DataSource Source_Luma_Right
static TimeStamp getCurrentTime()
static CRL_CONSTEXPR DataSource Source_Imu
static CRL_CONSTEXPR uint32_t IMAGER_TYPE_CMV4000_GREY
static CRL_CONSTEXPR uint32_t HARDWARE_REV_MULTISENSE_S7S
static CRL_CONSTEXPR uint32_t HARDWARE_REV_MULTISENSE_ST21
static CRL_CONSTEXPR uint16_t DEFAULT_SENSOR_TX_PORT
static CRL_CONSTEXPR uint32_t IMAGER_TYPE_IMX104_COLOR
static CRL_CONSTEXPR uint32_t HARDWARE_REV_MULTISENSE_S21
static CRL_CONSTEXPR SourceType SOURCE_CHROMA_LEFT
static CRL_CONSTEXPR Status Status_TimedOut
uint16_t m_serverSocketPort
static CRL_CONSTEXPR uint32_t RX_POOL_LARGE_BUFFER_COUNT
static CRL_CONSTEXPR Status Status_Ok
static CRL_CONSTEXPR SourceType SOURCE_RAW_RIGHT
static CRL_CONSTEXPR DataSource Source_Pps
static CRL_CONSTEXPR DataSource Source_Rgb_Left
wire::VersionResponse m_sensorVersion
static CRL_CONSTEXPR uint32_t IMAGER_TYPE_CMV4000_GREY
static CRL_CONSTEXPR uint32_t HARDWARE_REV_MULTISENSE_S7S
static CRL_CONSTEXPR SourceType SOURCE_IMU
static uint32_t imagerApiToWire(uint32_t h)
utility::TimeStamp uptime
static CRL_CONSTEXPR uint32_t HARDWARE_REV_BCAM
static CRL_CONSTEXPR SourceType SOURCE_LUMA_RECT_LEFT
static CRL_CONSTEXPR IdType ID
static CRL_CONSTEXPR uint16_t HEADER_MAGIC
static CRL_CONSTEXPR SourceType SOURCE_DISPARITY_COST
static CRL_CONSTEXPR DataSource Source_Raw_Right
#define CRL_DEBUG(fmt,...)
utility::Mutex m_timeLock
static CRL_CONSTEXPR SourceType SOURCE_RGB_LEFT
static CRL_CONSTEXPR uint32_t RX_POOL_LARGE_BUFFER_SIZE
BufferPool m_rxSmallBufferPool
std::list< LidarListener * > m_lidarListeners
static CRL_CONSTEXPR DataSource Source_Raw_Left
bool wait(Status &status, const double &timeout)
static uint32_t hardwareApiToWire(uint32_t h)
static void Destroy(Channel *instanceP)
impl(const std::string &address)
static CRL_CONSTEXPR uint32_t HARDWARE_REV_MULTISENSE_M
static CRL_CONSTEXPR uint16_t HEADER_GROUP
static void assembler(utility::BufferStreamWriter &stream, const uint8_t *dataP, uint32_t offset, uint32_t length)
static CRL_CONSTEXPR DataSource Source_Luma_Rectified_Right
void publish(const T &message)
static CRL_CONSTEXPR SourceType SOURCE_RAW_LEFT
static CRL_CONSTEXPR uint32_t IMAGER_TYPE_CMV2000_GREY
static CRL_CONSTEXPR uint32_t IMAGER_TYPE_CMV4000_COLOR
static CRL_CONSTEXPR SourceType SOURCE_LIDAR_SCAN
static CRL_CONSTEXPR SourceType SOURCE_PPS
static void * rxThread(void *userDataP)
static CRL_CONSTEXPR SourceType SOURCE_LUMA_RIGHT
std::list< ImuListener * > m_imuListeners
static CRL_CONSTEXPR Status Status_Exception
Type decayedAverage(Type const &previous, Type const &samples, Type const &newest)
static CRL_CONSTEXPR uint32_t IMAGER_TYPE_CMV2000_COLOR
static CRL_CONSTEXPR uint32_t TIME_SYNC_OFFSET_DECAY
static CRL_CONSTEXPR uint32_t IMAGER_TYPE_CMV2000_COLOR
Status waitData(const T &command, U &data, const double &timeout=DEFAULT_ACK_TIMEOUT(), int32_t attempts=DEFAULT_ACK_ATTEMPTS)
static void * statusThread(void *userDataP)
static CRL_CONSTEXPR Status Status_Error
static CRL_CONSTEXPR uint32_t IMAGER_TYPE_IMX104_COLOR
static CRL_CONSTEXPR uint32_t HARDWARE_REV_BCAM
static CRL_CONSTEXPR SourceType SOURCE_JPEG_LEFT
utility::Thread * m_statusThreadP
static DataSource sourceWireToApi(wire::SourceType mask)
static CRL_CONSTEXPR DataSource Source_Chroma_Right
static CRL_CONSTEXPR Status Status_Unknown
static CRL_CONSTEXPR uint32_t HARDWARE_REV_MULTISENSE_S7
static CRL_CONSTEXPR DataSource Source_Luma_Rectified_Left