query.hh
Go to the documentation of this file.
00001 
00037 #ifndef _LibMultiSense_details_query_hh
00038 #define _LibMultiSense_details_query_hh
00039 
00040 #include "details/channel.hh"
00041 
00042 namespace crl {
00043 namespace multisense {
00044 namespace details {
00045 
00046 //
00047 // Publishes the given message to the sensor (message must
00048 // serialize into a single MTU)
00049 
00050 template<class T> void impl::publish(const T& message)
00051 {
00052     const wire::IdType      id      = T::ID;
00053     const wire::VersionType version = T::VERSION;
00054 
00055     //
00056     // An output stream to serialize the data
00057 
00058     utility::BufferStreamWriter stream(m_sensorMtu - 
00059                                        wire::COMBINED_HEADER_LENGTH);
00060 
00061     //
00062     // Hide the header area
00063 
00064     stream.seek(sizeof(wire::Header));
00065 
00066     //
00067     // Set the ID and version
00068     
00069     stream & id;
00070     stream & version;
00071     
00072     //
00073     // Add the message payload. We cast away const here because
00074     // we have a single serialize() for both directions, and cannot
00075     // mark it const.
00076 
00077     const_cast<T*>(&message)->serialize(stream, version);
00078 
00079     //
00080     // Publish the stream
00081 
00082     publish(stream);
00083 }
00084 
00085 //
00086 // Send a message, wait for a particular repsonse, re-trying if
00087 // necessary
00088 
00089 template <class T> Status impl::waitAck(const T&      msg, 
00090                                         wire::IdType  ackId,
00091                                         const double& timeout,
00092                                         int32_t       attempts)
00093 {
00094     try {
00095         ScopedWatch ack(ackId, m_watch);
00096 
00097         while(attempts-- > 0) {
00098 
00099             publish(msg);
00100             
00101             Status status;
00102             if (false == ack.wait(status, timeout))
00103                 continue;
00104             else
00105                 return status;
00106         }
00107 
00108         return Status_TimedOut;
00109 
00110     } catch (const std::exception& e) {
00111         CRL_DEBUG("exception: %s\n", e.what());
00112         return Status_Exception;
00113     }
00114 }
00115 
00116 //
00117 // Send a message (with retry), expecting data message, 
00118 // extract data payload for user
00119 
00120 template <class T, class U> Status impl::waitData(const T&      command,
00121                                                   U&            data,
00122                                                   const double& timeout,
00123                                                   int32_t       attempts)
00124 {
00125     try {
00126 
00127         //
00128         // Set up a watch on the command ID in case it is rejected or
00129         // unsupported.
00130 
00131         ScopedWatch commandAck(T::ID, m_watch);
00132 
00133         //
00134         // Send the command with retry, expecting the data message as a response.
00135 
00136         Status dataStatus = waitAck(command, MSG_ID(U::ID), timeout, attempts);
00137 
00138         //
00139         // Also store the response from the command. Do not block, as any
00140         // response would be registered by this time.
00141 
00142         Status commandStatus;
00143         if (false == commandAck.wait(commandStatus, 0.0))
00144             commandStatus = Status_TimedOut;
00145     
00146         //
00147         // If we did not receive the data message, return the response from
00148         // the command code instead, unless there was an exception or the 
00149         // command ack'd OK.
00150 
00151         if (Status_Ok != dataStatus) {
00152             if (Status_Exception == dataStatus ||   // exception 
00153                 Status_Ok        == commandStatus)  // data payload timeout or MTU error
00154                 return dataStatus;
00155             else
00156                 return commandStatus; // command error
00157         }
00158 
00159         //
00160         // We have received the data message, extract it for the user.
00161         
00162         return m_messages.extract(data);
00163         
00164     } catch (const std::exception& e) {
00165         CRL_DEBUG("exception: %s\n", e.what());
00166         return Status_Exception;
00167     }
00168 }
00169 
00170 }}}; // namespaces
00171 
00172 #endif // _LibMultiSense_details_publish_hh


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