query.hh

Go to the documentation of this file.

 
#ifndef _LibMultiSense_details_query_hh
#define _LibMultiSense_details_query_hh
 
#include "MultiSense/details/channel.hh"
 
namespace crl {
namespace multisense {
namespace details {
 
//
// Publishes the given message to the sensor (message must
// serialize into a single MTU)
 
template<class T> void impl::publish(const T& message)
{
    const wire::IdType      id      = T::ID;
    const wire::VersionType version = T::VERSION;
 
    //
    // An output stream to serialize the data
 
    utility::BufferStreamWriter stream(m_sensorMtu -
                                       wire::COMBINED_HEADER_LENGTH);
 
    //
    // Hide the header area
 
    stream.seek(sizeof(wire::Header));
 
    //
    // Set the ID and version
 
    stream & id;
    stream & version;
 
    //
    // Add the message payload. We cast away const here because
    // we have a single serialize() for both directions, and cannot
    // mark it const.
 
    const_cast<T*>(&message)->serialize(stream, version);
 
    //
    // Publish the stream
 
    publish(stream);
}
 
//
// Send a message, wait for a particular response, re-trying if
// necessary
 
template <class T> Status impl::waitAck(const T&      msg,
                                        wire::IdType  ackId,
                                        const double& timeout,
                                        int32_t       attempts)
{
    try {
        ScopedWatch ack(ackId, m_watch);
 
        while(attempts-- > 0) {
 
            publish(msg);
 
            Status status;
            if (false == ack.wait(status, timeout))
                continue;
            else
                return status;
        }
 
        return Status_TimedOut;
 
    } catch (const std::exception& e) {
        CRL_DEBUG("exception: %s\n", e.what());
        return Status_Exception;
    }
}
 
//
// Send a message (with retry), expecting data message,
// extract data payload for user
 
template <class T, class U> Status impl::waitData(const T&      command,
                                                  U&            data,
                                                  const double& timeout,
                                                  int32_t       attempts)
{
    try {
 
        //
        // Set up a watch on the command ID in case it is rejected or
        // unsupported.
 
        ScopedWatch commandAck(T::ID, m_watch);
 
        //
        // Send the command with retry, expecting the data message as a response.
 
        Status dataStatus = waitAck(command, MSG_ID(U::ID), timeout, attempts);
 
        //
        // Also store the response from the command. Do not block, as any
        // response would be registered by this time.
 
        Status commandStatus;
        if (false == commandAck.wait(commandStatus, 0.0))
            commandStatus = Status_TimedOut;
 
        //
        // If we did not receive the data message, return the response from
        // the command code instead, unless there was an exception or the
        // command ack'd OK.
 
        if (Status_Ok != dataStatus) {
            if (Status_Exception == dataStatus ||   // exception
                Status_Ok        == commandStatus)  // data payload timeout or MTU error
                return dataStatus;
            else
                return commandStatus; // command error
        }
 
        //
        // We have received the data message, extract it for the user.
 
        return m_messages.extract(data);
 
    } catch (const std::exception& e) {
        CRL_DEBUG("exception: %s\n", e.what());
        return Status_Exception;
    }
}
 
}}} // namespaces
 
#endif // _LibMultiSense_details_publish_hh