00001
00016 #ifndef SICK_MESSAGE
00017 #define SICK_MESSAGE
00018
00019
00020 #include <arpa/inet.h>
00021 #include <iomanip>
00022 #include <iostream>
00023
00024
00025 namespace SickToolbox {
00026
00031 template < unsigned int MSG_HEADER_LENGTH, unsigned int MSG_PAYLOAD_MAX_LENGTH, unsigned int MSG_TRAILER_LENGTH >
00032 class SickMessage {
00033
00034 public:
00035
00037 static const unsigned int MESSAGE_HEADER_LENGTH = MSG_HEADER_LENGTH;
00038 static const unsigned int MESSAGE_TRAILER_LENGTH = MSG_TRAILER_LENGTH;
00039 static const unsigned int MESSAGE_PAYLOAD_MAX_LENGTH = MSG_PAYLOAD_MAX_LENGTH;
00040 static const unsigned int MESSAGE_MAX_LENGTH = MESSAGE_HEADER_LENGTH + MESSAGE_PAYLOAD_MAX_LENGTH + MESSAGE_TRAILER_LENGTH;
00041
00043 SickMessage( );
00044
00046 void BuildMessage( const uint8_t * const payload_buffer, const unsigned int payload_length );
00047
00049 virtual void ParseMessage( const uint8_t * const message_buffer ) = 0;
00050
00052 void GetMessage( uint8_t * const message_buffer ) const;
00053
00055 unsigned int GetMessageLength( ) const { return _message_length; }
00056
00058 void GetPayload( uint8_t * const payload_buffer ) const;
00059
00061 void GetPayloadAsCStr( char * const payload_str ) const;
00062
00064 void GetPayloadSubregion( uint8_t * const payload_sub_buffer, const unsigned int start_idx,
00065 const unsigned int stop_idx ) const;
00066
00068 unsigned int GetPayloadLength( ) const { return _payload_length; }
00069
00071 bool IsPopulated( ) const { return _populated; };
00072
00074 virtual void Clear( );
00075
00077 virtual void Print( ) const;
00078
00080 virtual ~SickMessage( );
00081
00082 protected:
00083
00085 unsigned int _payload_length;
00086
00088 unsigned int _message_length;
00089
00091 uint8_t _message_buffer[MESSAGE_MAX_LENGTH];
00092
00094 bool _populated;
00095
00096 };
00097
00098
00102 template< unsigned int MSG_HEADER_LENGTH, unsigned int MSG_PAYLOAD_MAX_LENGTH, unsigned int MSG_TRAILER_LENGTH >
00103 SickMessage< MSG_HEADER_LENGTH, MSG_PAYLOAD_MAX_LENGTH, MSG_TRAILER_LENGTH >::SickMessage( ) { }
00104
00110 template< unsigned int MSG_HEADER_LENGTH, unsigned int MSG_PAYLOAD_MAX_LENGTH, unsigned int MSG_TRAILER_LENGTH >
00111 void SickMessage< MSG_HEADER_LENGTH, MSG_PAYLOAD_MAX_LENGTH, MSG_TRAILER_LENGTH >::BuildMessage( const uint8_t * const payload_buffer, const unsigned int payload_length ) {
00112
00113
00114 Clear();
00115
00116
00117 _payload_length = payload_length;
00118 _message_length = MESSAGE_HEADER_LENGTH + MESSAGE_TRAILER_LENGTH + _payload_length;
00119
00120
00121 memcpy(&_message_buffer[MESSAGE_HEADER_LENGTH],payload_buffer,_payload_length);
00122
00123
00124 _populated = true;
00125
00126 }
00127
00132 template< unsigned int MSG_HEADER_LENGTH, unsigned int MSG_PAYLOAD_MAX_LENGTH, unsigned int MSG_TRAILER_LENGTH >
00133 void SickMessage< MSG_HEADER_LENGTH, MSG_PAYLOAD_MAX_LENGTH, MSG_TRAILER_LENGTH >::ParseMessage( const uint8_t * const message_buffer ) {
00134
00135
00136 Clear();
00137
00138
00139 _populated = true;
00140 }
00141
00146 template< unsigned int MSG_HEADER_LENGTH, unsigned int MSG_PAYLOAD_MAX_LENGTH, unsigned int MSG_TRAILER_LENGTH >
00147 void SickMessage< MSG_HEADER_LENGTH, MSG_PAYLOAD_MAX_LENGTH, MSG_TRAILER_LENGTH >::GetMessage( uint8_t * const message_buffer ) const {
00148 memcpy(message_buffer,_message_buffer,_message_length);
00149 }
00150
00155 template< unsigned int MSG_HEADER_LENGTH, unsigned int MSG_PAYLOAD_MAX_LENGTH, unsigned int MSG_TRAILER_LENGTH >
00156 void SickMessage< MSG_HEADER_LENGTH, MSG_PAYLOAD_MAX_LENGTH, MSG_TRAILER_LENGTH >::GetPayload( uint8_t * const payload_buffer ) const {
00157 memcpy(payload_buffer,&_message_buffer[MESSAGE_HEADER_LENGTH],_payload_length);
00158 }
00159
00161 template< unsigned int MSG_HEADER_LENGTH, unsigned int MSG_PAYLOAD_MAX_LENGTH, unsigned int MSG_TRAILER_LENGTH >
00162 void SickMessage< MSG_HEADER_LENGTH, MSG_PAYLOAD_MAX_LENGTH, MSG_TRAILER_LENGTH >::GetPayloadAsCStr( char * const payload_buffer ) const {
00163 memcpy(payload_buffer,&_message_buffer[MESSAGE_HEADER_LENGTH],_payload_length);
00164 payload_buffer[_payload_length] = '\0';
00165 }
00166
00173 template< unsigned int MSG_HEADER_LENGTH, unsigned int MSG_PAYLOAD_MAX_LENGTH, unsigned int MSG_TRAILER_LENGTH >
00174 void SickMessage< MSG_HEADER_LENGTH, MSG_PAYLOAD_MAX_LENGTH, MSG_TRAILER_LENGTH >::GetPayloadSubregion( uint8_t * const payload_sub_buffer,
00175 const unsigned int start_idx,
00176 const unsigned int stop_idx ) const {
00177
00178 memcpy(payload_sub_buffer,&_message_buffer[MESSAGE_HEADER_LENGTH+start_idx],stop_idx+1);
00179 }
00180
00184 template< unsigned int MSG_HEADER_LENGTH, unsigned int MSG_PAYLOAD_MAX_LENGTH, unsigned int MSG_TRAILER_LENGTH >
00185 void SickMessage< MSG_HEADER_LENGTH, MSG_PAYLOAD_MAX_LENGTH, MSG_TRAILER_LENGTH >::Clear( ) {
00186
00187
00188 _message_length = _payload_length = 0;
00189
00190
00191 memset(_message_buffer,0,MESSAGE_MAX_LENGTH);
00192
00193
00194 _populated = false;
00195 }
00196
00200 template< unsigned int MSG_HEADER_LENGTH, unsigned int MSG_PAYLOAD_MAX_LENGTH, unsigned int MSG_TRAILER_LENGTH >
00201 void SickMessage< MSG_HEADER_LENGTH, MSG_PAYLOAD_MAX_LENGTH, MSG_TRAILER_LENGTH >::Print( ) const {
00202
00203 std::cout << "Payload length: " << GetPayloadLength() << std::endl;
00204 std::cout << "Message length: " << GetMessageLength() << std::endl;
00205 std::cout << std::flush;
00206
00207 std::cout << "Message (hex):" << std::endl;
00208 std::cout.setf(std::ios::hex,std::ios::basefield);
00209 for (unsigned int i = 0; i < _message_length; i++) {
00210 std::cout << (int)_message_buffer[i] << " ";
00211 }
00212 std::cout << std::endl << std::flush;
00213
00214 std::cout << "Message (ASCII):" << std::endl;
00215 std::cout.setf(std::ios::dec,std::ios::basefield);
00216 for (unsigned int i = 0; i < _message_length; i++) {
00217 std::cout << _message_buffer[i] << " ";
00218 }
00219 std::cout << std::endl << std::flush;
00220 }
00221
00225 template< unsigned int MSG_HEADER_LENGTH, unsigned int MSG_PAYLOAD_MAX_LENGTH, unsigned int MSG_TRAILER_LENGTH >
00226 SickMessage< MSG_HEADER_LENGTH, MSG_PAYLOAD_MAX_LENGTH, MSG_TRAILER_LENGTH >::~SickMessage() { }
00227
00228 }
00229
00230 #endif