SickLMS1xxMessage.cc
Go to the documentation of this file.
00001 
00016 /* Auto-generated header */
00017 #include <sicktoolbox/SickConfig.hh>
00018 
00019 /* Implementation dependencies */
00020 #include <iomanip>
00021 #include <iostream>
00022 #include <arpa/inet.h> 
00023 
00024 #include <sicktoolbox/SickLMS1xxMessage.hh>
00025 #include <sicktoolbox/SickLMS1xxUtility.hh> // for byye-order conversions where necessary
00026 
00027 /* Associate the namespace */
00028 namespace SickToolbox {
00029 
00033   SickLMS1xxMessage::SickLMS1xxMessage( ) :
00034     SickMessage< SICK_LMS_1XX_MSG_HEADER_LEN, SICK_LMS_1XX_MSG_PAYLOAD_MAX_LEN, SICK_LMS_1XX_MSG_TRAILER_LEN >(),
00035     _command_type(""),
00036     _command("")
00037   {
00038 
00039     /* Initialize the object */
00040     Clear(); 
00041 
00042   }
00043   
00049   SickLMS1xxMessage::SickLMS1xxMessage( const uint8_t * const payload_buffer, const unsigned int payload_length ) :
00050     SickMessage< SICK_LMS_1XX_MSG_HEADER_LEN, SICK_LMS_1XX_MSG_PAYLOAD_MAX_LEN, SICK_LMS_1XX_MSG_TRAILER_LEN >(),
00051     _command_type("Unknown"),
00052     _command("Unknown")
00053   {
00054 
00055     /* Build the message object (implicit initialization) */
00056     BuildMessage(payload_buffer,payload_length); 
00057 
00058   }
00059   
00064   SickLMS1xxMessage::SickLMS1xxMessage( const uint8_t * const message_buffer ) :
00065     SickMessage< SICK_LMS_1XX_MSG_HEADER_LEN, SICK_LMS_1XX_MSG_PAYLOAD_MAX_LEN, SICK_LMS_1XX_MSG_TRAILER_LEN >(),
00066     _command_type("Unknown"),
00067     _command("Unknown")
00068   {
00069 
00070     /* Parse the message into the container (implicit initialization) */
00071     ParseMessage(message_buffer); 
00072 
00073   }
00074 
00080   void SickLMS1xxMessage::BuildMessage( const uint8_t * const payload_buffer, const unsigned int payload_length ) {
00081 
00082     /* Call the parent method
00083      * NOTE: The parent method resets the object and assigns _message_length, _payload_length,
00084      *       _populated and copies the payload into the message buffer at the correct position
00085      */
00086     SickMessage< SICK_LMS_1XX_MSG_HEADER_LEN, SICK_LMS_1XX_MSG_PAYLOAD_MAX_LEN, SICK_LMS_1XX_MSG_TRAILER_LEN >
00087       ::BuildMessage(payload_buffer,payload_length);
00088     
00089     /*
00090      * Set the message header!
00091      */
00092     _message_buffer[0] = 0x02;                 // STX
00093 
00094     /*
00095      * Set the message trailer! 
00096      */
00097     _message_buffer[_message_length-1] = 0x03; // ETX
00098     
00099     /* Grab the (3-byte) command type */
00100     char command_type[4] = {0};
00101     for (int i = 0; i < 3; i++) {
00102       command_type[i] = _message_buffer[i+1];
00103     }
00104     command_type[4] = '\0';
00105     _command_type = command_type;
00106     
00107     /* Grab the command (max length is 14 bytes) */
00108     char command[15] = {0};
00109     int i = 0;
00110     for (; (i < 14) && (_message_buffer[5+i] != 0x20); i++) {
00111       command[i] = _message_buffer[5+i];
00112     }
00113     command[i] = '\0';
00114     _command = command;
00115     
00116   }
00117   
00122   void SickLMS1xxMessage::ParseMessage( const uint8_t * const message_buffer ) throw (SickIOException) {
00123     
00124     /* Call the parent method
00125      * NOTE: This method resets the object and assigns _populated as true
00126      */
00127     SickMessage< SICK_LMS_1XX_MSG_HEADER_LEN, SICK_LMS_1XX_MSG_PAYLOAD_MAX_LEN, SICK_LMS_1XX_MSG_TRAILER_LEN >
00128       ::ParseMessage(message_buffer);
00129     
00130     /* Compute the message length */
00131     int i = 1;
00132     const char * token = NULL;
00133     while (message_buffer[i-1] != 0x03) {
00134 
00135       if (i == 1) {
00136       
00137         /* Grab the command type */
00138         if ((token = strtok((char *)&_message_buffer[1]," ")) == NULL) {
00139           throw SickIOException("SickLMS1xxMessage::ParseMessage: strtok() failed!");
00140         }
00141         
00142         _command_type = token;
00143         
00144         /* Grab the Command Code */
00145         if ((token = strtok(NULL," ")) == NULL) {
00146           throw SickIOException("SickLMS1xxMessage::ParseMessage: strtok() failed!");
00147         }
00148         
00149         _command = token;
00150         
00151       }
00152       
00153       i++; // Update message length
00154 
00155       /* A sanity check */
00156       if (i > SickLMS1xxMessage::MESSAGE_MAX_LENGTH) {
00157         throw SickIOException("SickLMS1xxMessage::ParseMessage: Message Exceeds Max Message Length!");  
00158       }
00159       
00160     }
00161 
00162     /* Compute the total message length */
00163     _payload_length = _message_length - MESSAGE_HEADER_LENGTH - MESSAGE_TRAILER_LENGTH;
00164     
00165     /* Copy the given packet into the buffer */
00166     memcpy(_message_buffer,message_buffer,_message_length);
00167 
00168   }
00169 
00173   void SickLMS1xxMessage::Clear( ) {    
00174 
00175     /* Call the parent method and clear out class' protected members */
00176     SickMessage< SICK_LMS_1XX_MSG_HEADER_LEN, SICK_LMS_1XX_MSG_PAYLOAD_MAX_LEN, SICK_LMS_1XX_MSG_TRAILER_LEN >::Clear();
00177 
00178     /* Reset the class' additional fields */
00179     _command_type = "Unknown";
00180     _command = "Unknown";
00181     
00182   }
00183   
00187   void SickLMS1xxMessage::Print( ) const {
00188 
00189     //std::cout.setf(std::ios::hex,std::ios::basefield);
00190     std::cout << "Command Type: " << GetCommandType() << std::endl;
00191     std::cout << "Command Code: " << GetCommand() << std::endl;
00192     std::cout << std::flush;
00193 
00194     /* Call parent's print function */
00195     SickMessage< SICK_LMS_1XX_MSG_HEADER_LEN, SICK_LMS_1XX_MSG_PAYLOAD_MAX_LEN, SICK_LMS_1XX_MSG_TRAILER_LEN >::Print();    
00196 
00197   }
00198   
00199   SickLMS1xxMessage::~SickLMS1xxMessage( ) { }
00200   
00201 } /* namespace SickToolbox */


sicktoolbox
Author(s): Jason Derenick , Thomas Miller
autogenerated on Sun May 5 2019 02:28:23