Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032 #ifdef ROS
00033 #include "simple_message/simple_message.h"
00034 #include "simple_message/log_wrapper.h"
00035 #endif
00036
00037 #ifdef MOTOPLUS
00038 #include "motoPlus.h"
00039 #include "simple_message.h"
00040 #include "log_wrapper.h"
00041 #endif
00042
00043
00044
00045 using namespace industrial::byte_array;
00046
00047 namespace industrial
00048 {
00049
00050 namespace simple_message
00051 {
00052
00053 SimpleMessage::SimpleMessage(void)
00054 {
00055 }
00056
00057 SimpleMessage::~SimpleMessage(void)
00058 {
00059 }
00060
00061
00062
00063 bool SimpleMessage::init(int msgType, int commType, int replyCode)
00064 {
00065 ByteArray data;
00066 data.init();
00067 return this->init(msgType, commType, replyCode, data);
00068 }
00069
00070 bool SimpleMessage::init(int msgType, int commType, int replyCode, ByteArray & data )
00071 {
00072 LOG_COMM("SimpleMessage::init(type: %d, comm: %d, reply: %d, data[%d]...)",
00073 msgType, commType, replyCode, data.getBufferSize());
00074 this->setMessageType(msgType);
00075 this->setCommType(commType);
00076 this->setReplyCode(replyCode);
00077 this->data_.copyFrom(data);
00078
00079 return this->validateMessage();
00080 }
00081
00082 bool SimpleMessage::init(ByteArray & msg)
00083 {
00084 int dataSize = 0;
00085 bool rtn = false;
00086
00087 if (msg.getBufferSize() >= this->getHeaderSize())
00088 {
00089
00090
00091 if (msg.getBufferSize() > this->getHeaderSize())
00092 {
00093 dataSize = msg.getBufferSize() - this->getHeaderSize();
00094 LOG_COMM("Unloading data portion of message: %d bytes", dataSize);
00095 msg.unload(this->data_, dataSize);
00096 }
00097 LOG_COMM("Unloading header data");
00098 msg.unload(this->reply_code_);
00099 msg.unload(this->comm_type_);
00100 msg.unload(this->message_type_);
00101 LOG_COMM("SimpleMessage::init(type: %d, comm: %d, reply: %d, data[%d]...)",
00102 this->message_type_, this->comm_type_, this->reply_code_, this->data_.getBufferSize());
00103 rtn = this->validateMessage();
00104 }
00105 else
00106 {
00107 LOG_ERROR("Failed to init message, buffer size too small: %u", msg.getBufferSize());
00108 rtn = false;
00109 }
00110 return rtn;
00111 }
00112
00113 void SimpleMessage::toByteArray(ByteArray & msg)
00114 {
00115 msg.init();
00116
00117 msg.load(this->getMessageType());
00118 msg.load(this->getCommType());
00119 msg.load(this->getReplyCode());
00120 if (this->data_.getBufferSize() > 0 )
00121 {
00122 msg.load(this->getData().getRawDataPtr(), this->data_.getBufferSize());
00123 }
00124
00125 }
00126
00127
00128 void SimpleMessage::setData( ByteArray & data)
00129 {
00130 this->data_.copyFrom(data);
00131 }
00132
00133
00134 bool SimpleMessage::validateMessage()
00135 {
00136
00137 if ( StandardMsgTypes::INVALID == this->getMessageType())
00138 {
00139 LOG_WARN("Invalid message type: %u", this->getMessageType());
00140 return false;
00141 }
00142
00143 if ( CommTypes::INVALID == this->getCommType())
00144 {
00145 LOG_WARN("Invalid comms. type: %u", this->getCommType());
00146 return false;
00147 }
00148
00149 if (
00150 (CommTypes::SERVICE_REPLY == this->getCommType() &&
00151 ReplyTypes::INVALID == this->getReplyCode()) ||
00152 ((CommTypes::SERVICE_REPLY != this->getCommType() &&
00153 ReplyTypes::INVALID != this->getReplyCode()))
00154 )
00155 {
00156 LOG_WARN("Invalid reply. Comm type: %u, Reply type: %u",
00157 this->getCommType(), this->getReplyCode());
00158 return false;
00159 }
00160
00161 return true;
00162 }
00163
00164
00165
00166
00167 }
00168 }