00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016 #ifndef SICK_MESSAGE
00017 #define SICK_MESSAGE
00018
00019
00020 #include "SickConfig.hh"
00021
00022
00023 #include <arpa/inet.h>
00024 #include <iomanip>
00025 #include <iostream>
00026
00027
00028 namespace SickToolbox {
00029
00030
00031
00032
00033
00034 template < unsigned int MSG_HEADER_LENGTH, unsigned int MSG_PAYLOAD_MAX_LENGTH, unsigned int MSG_TRAILER_LENGTH >
00035 class SickMessage {
00036
00037 public:
00038
00039
00040 static const unsigned int MESSAGE_HEADER_LENGTH = MSG_HEADER_LENGTH;
00041 static const unsigned int MESSAGE_TRAILER_LENGTH = MSG_TRAILER_LENGTH;
00042 static const unsigned int MESSAGE_PAYLOAD_MAX_LENGTH = MSG_PAYLOAD_MAX_LENGTH;
00043 static const unsigned int MESSAGE_MAX_LENGTH = MESSAGE_HEADER_LENGTH + MESSAGE_PAYLOAD_MAX_LENGTH + MESSAGE_TRAILER_LENGTH;
00044
00045
00046 SickMessage( );
00047
00048
00049 void BuildMessage( const uint8_t * const payload_buffer, const unsigned int payload_length );
00050
00051
00052 virtual void ParseMessage( const uint8_t * const message_buffer ) = 0;
00053
00054
00055 void GetMessage( uint8_t * const message_buffer ) const;
00056
00057
00058 unsigned int GetMessageLength( ) const { return _message_length; }
00059
00060
00061 void GetPayload( uint8_t * const payload_buffer ) const;
00062
00063
00064 void GetPayloadSubregion( uint8_t * const payload_sub_buffer, const unsigned int start_idx,
00065 const unsigned int stop_idx ) const;
00066
00067
00068 unsigned int GetPayloadLength( ) const { return _payload_length; }
00069
00070
00071 bool IsPopulated( ) const { return _populated; };
00072
00073
00074 virtual void Clear( );
00075
00076
00077 virtual void Print( ) const;
00078
00079
00080 virtual ~SickMessage( );
00081
00082 protected:
00083
00084
00085 unsigned int _payload_length;
00086
00087
00088 unsigned int _message_length;
00089
00090
00091 uint8_t _message_buffer[MESSAGE_MAX_LENGTH];
00092
00093
00094 bool _populated;
00095
00096 };
00097
00098
00099
00100
00101
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
00105
00106
00107
00108
00109
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
00128
00129
00130
00131
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
00142
00143
00144
00145
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
00151
00152
00153
00154
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
00160
00161
00162
00163
00164
00165
00166 template< unsigned int MSG_HEADER_LENGTH, unsigned int MSG_PAYLOAD_MAX_LENGTH, unsigned int MSG_TRAILER_LENGTH >
00167 void SickMessage< MSG_HEADER_LENGTH, MSG_PAYLOAD_MAX_LENGTH, MSG_TRAILER_LENGTH >::GetPayloadSubregion( uint8_t * const payload_sub_buffer,
00168 const unsigned int start_idx,
00169 const unsigned int stop_idx ) const {
00170
00171 memcpy(payload_sub_buffer,&_message_buffer[MESSAGE_HEADER_LENGTH+start_idx],stop_idx+1);
00172 }
00173
00174
00175
00176
00177 template< unsigned int MSG_HEADER_LENGTH, unsigned int MSG_PAYLOAD_MAX_LENGTH, unsigned int MSG_TRAILER_LENGTH >
00178 void SickMessage< MSG_HEADER_LENGTH, MSG_PAYLOAD_MAX_LENGTH, MSG_TRAILER_LENGTH >::Clear( ) {
00179
00180
00181 _message_length = _payload_length = 0;
00182
00183
00184 memset(_message_buffer,0,MESSAGE_MAX_LENGTH);
00185
00186
00187 _populated = false;
00188 }
00189
00190
00191
00192
00193 template< unsigned int MSG_HEADER_LENGTH, unsigned int MSG_PAYLOAD_MAX_LENGTH, unsigned int MSG_TRAILER_LENGTH >
00194 void SickMessage< MSG_HEADER_LENGTH, MSG_PAYLOAD_MAX_LENGTH, MSG_TRAILER_LENGTH >::Print( ) const {
00195
00196 std::cout << "Payload length: " << GetPayloadLength() << std::endl;
00197 std::cout << "Message length: " << GetMessageLength() << std::endl;
00198 std::cout << std::flush;
00199
00200 std::cout << "Message (hex):" << std::endl;
00201 std::cout.setf(std::ios::hex,std::ios::basefield);
00202 for (unsigned int i = 0; i < _message_length; i++) {
00203 std::cout << (int)_message_buffer[i] << " ";
00204 }
00205 std::cout << std::endl << std::flush;
00206
00207 std::cout << "Message (ASCII):" << std::endl;
00208 std::cout.setf(std::ios::dec,std::ios::basefield);
00209 for (unsigned int i = 0; i < _message_length; i++) {
00210 std::cout << _message_buffer[i] << " ";
00211 }
00212 std::cout << std::endl << std::flush;
00213 }
00214
00215
00216
00217
00218 template< unsigned int MSG_HEADER_LENGTH, unsigned int MSG_PAYLOAD_MAX_LENGTH, unsigned int MSG_TRAILER_LENGTH >
00219 SickMessage< MSG_HEADER_LENGTH, MSG_PAYLOAD_MAX_LENGTH, MSG_TRAILER_LENGTH >::~SickMessage() { }
00220
00221 }
00222
00223 #endif