00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019 #include <sstream>
00020
00021 #include "variant_topic_tools/DataTypeRegistry.h"
00022 #include "variant_topic_tools/Exceptions.h"
00023 #include "variant_topic_tools/MessageConstant.h"
00024 #include "variant_topic_tools/MessageDataType.h"
00025 #include "variant_topic_tools/MessageDefinitionParser.h"
00026 #include "variant_topic_tools/MessageMember.h"
00027 #include "variant_topic_tools/MessageTypeParser.h"
00028 #include "variant_topic_tools/MessageVariable.h"
00029 #include "variant_topic_tools/MessageVariant.h"
00030
00031 namespace variant_topic_tools {
00032
00033
00034
00035
00036
00037 MessageDataType::MessageDataType() {
00038 }
00039
00040 MessageDataType::MessageDataType(const std::string& identifier,
00041 const MessageFieldCollection<MessageConstant>& constantMembers,
00042 const MessageFieldCollection<MessageVariable>& variableMembers) {
00043 impl.reset(new boost::shared_ptr<DataType::Impl>(
00044 new ImplV(identifier, constantMembers, variableMembers)));
00045 }
00046
00047 MessageDataType::MessageDataType(const std::string& identifier, const
00048 std::string& definition) {
00049 impl.reset(new boost::shared_ptr<DataType::Impl>(
00050 new ImplV(identifier, definition)));
00051 }
00052
00053 MessageDataType::MessageDataType(const MessageDataType& src) :
00054 DataType(src) {
00055 }
00056
00057 MessageDataType::MessageDataType(const DataType& src) :
00058 DataType(src) {
00059 if (impl)
00060 BOOST_ASSERT(boost::dynamic_pointer_cast<Impl>(*impl));
00061 }
00062
00063 MessageDataType::~MessageDataType() {
00064 }
00065
00066 MessageDataType::Impl::Impl(const MessageFieldCollection<MessageConstant>&
00067 constantMembers, const MessageFieldCollection<MessageVariable>&
00068 variableMembers) :
00069 constantMembers(constantMembers),
00070 variableMembers(variableMembers) {
00071 for (size_t i = 0; i < constantMembers.getNumFields(); ++i)
00072 if (!constantMembers[i].isValid())
00073 throw InvalidMessageMemberException();
00074
00075 for (size_t i = 0; i < variableMembers.getNumFields(); ++i)
00076 if (!variableMembers[i].isValid())
00077 throw InvalidMessageMemberException();
00078 }
00079
00080 MessageDataType::Impl::Impl(const std::string& identifier, const std::string&
00081 definition) {
00082 BOOST_ASSERT(!definition.empty());
00083
00084 std::string package, plainType;
00085 if (!MessageTypeParser::matchType(identifier, package, plainType))
00086 throw InvalidMessageTypeException(identifier);
00087
00088 DataTypeRegistry registry;
00089 std::istringstream stream(definition);
00090 std::string line;
00091
00092 while (std::getline(stream, line)) {
00093 std::string memberName, memberType, memberValue;
00094
00095 if (MessageDefinitionParser::matchVariable(line, memberName,
00096 memberType)) {
00097 std::string bareMemberType, memberPackage, plainMemberType;
00098 bool isArrayMember = false;
00099 size_t memberSize;
00100
00101 if (MessageDefinitionParser::matchArray(line, memberName,
00102 bareMemberType, memberSize))
00103 isArrayMember = true;
00104 else
00105 bareMemberType = memberType;
00106
00107 if (!MessageTypeParser::matchType(bareMemberType, memberPackage,
00108 plainMemberType))
00109 throw InvalidMessageTypeException(bareMemberType);
00110
00111 if (!registry.getDataType(bareMemberType).isBuiltin()) {
00112 if (memberPackage.empty()) {
00113 if (plainMemberType == "Header")
00114 memberPackage = "std_msgs";
00115 else
00116 memberPackage = package;
00117
00118 if (isArrayMember) {
00119 if (memberSize)
00120 memberType = memberPackage+"/"+plainMemberType+"["+
00121 boost::lexical_cast<std::string>(memberSize)+"]";
00122 else
00123 memberType = memberPackage+"/"+plainMemberType+"[]";
00124 }
00125 else
00126 memberType = memberPackage+"/"+plainMemberType;
00127 }
00128 }
00129
00130 if (registry.getDataType(memberType).isValid()) {
00131 MessageVariable member(memberName, memberType);
00132 variableMembers.appendField(memberName, member);
00133 }
00134 else
00135 throw NoSuchDataTypeException(memberType);
00136 }
00137 else if (MessageDefinitionParser::matchConstant(line, memberName,
00138 memberType, memberValue)) {
00139 if (registry.getDataType(memberType).isValid()) {
00140 MessageConstant member(memberName, memberType, memberValue);
00141 constantMembers.appendField(memberName, member);
00142 }
00143 else
00144 throw NoSuchDataTypeException(memberType);
00145 }
00146 else if (MessageDefinitionParser::matchSeparator(line))
00147 break;
00148 }
00149 }
00150
00151 MessageDataType::Impl::~Impl() {
00152 }
00153
00154 MessageDataType::ImplV::ImplV(const std::string& identifier,
00155 const MessageFieldCollection<MessageConstant>& constantMembers,
00156 const MessageFieldCollection<MessageVariable>& variableMembers) :
00157 Impl(constantMembers, variableMembers),
00158 identifier(identifier) {
00159 std::ostringstream stream;
00160
00161 for (size_t i = 0; i < constantMembers.getNumFields(); ++i)
00162 stream << constantMembers[i] << "\n";
00163
00164 for (size_t i = 0; i < variableMembers.getNumFields(); ++i)
00165 stream << variableMembers[i] << "\n";
00166
00167 definition = stream.str();
00168
00169 recalculateMD5Sum();
00170 }
00171
00172 MessageDataType::ImplV::ImplV(const std::string& identifier, const
00173 std::string& definition) :
00174 Impl(identifier, definition),
00175 identifier(identifier),
00176 definition(definition) {
00177 recalculateMD5Sum();
00178 }
00179
00180 MessageDataType::ImplV::~ImplV() {
00181 }
00182
00183
00184
00185
00186
00187 std::string MessageDataType::getMD5Sum() const {
00188 if (impl)
00189 return boost::static_pointer_cast<Impl>(*impl)->getMD5Sum();
00190 else
00191 return std::string("*");
00192 }
00193
00194 const std::string& MessageDataType::getDefinition() const {
00195 if (!impl) {
00196 static std::string definition;
00197 return definition;
00198 }
00199 else
00200 return boost::static_pointer_cast<Impl>(*impl)->getDefinition();
00201 }
00202
00203 size_t MessageDataType::getNumMembers() const {
00204 return getNumConstantMembers()+getNumVariableMembers();
00205 }
00206
00207 size_t MessageDataType::getNumConstantMembers() const {
00208 if (impl)
00209 return boost::static_pointer_cast<Impl>(*impl)->constantMembers.
00210 getNumFields();
00211 else
00212 return 0;
00213 }
00214
00215 size_t MessageDataType::getNumVariableMembers() const {
00216 if (impl)
00217 return boost::static_pointer_cast<Impl>(*impl)->variableMembers.
00218 getNumFields();
00219 else
00220 return 0;
00221 }
00222
00223 const MessageMember& MessageDataType::getMember(const std::string& name)
00224 const {
00225 if (hasConstantMember(name))
00226 return boost::static_pointer_cast<Impl>(*impl)->constantMembers[name].
00227 getValue();
00228 else if (hasVariableMember(name))
00229 return boost::static_pointer_cast<Impl>(*impl)->variableMembers[name].
00230 getValue();
00231 else
00232 throw NoSuchMemberException(name);
00233 }
00234
00235 const MessageMember& MessageDataType::getMember(int index) const {
00236 if (index >= 0) {
00237 if (index < getNumConstantMembers())
00238 return boost::static_pointer_cast<Impl>(*impl)->constantMembers[index].
00239 getValue();
00240 else if (index < getNumConstantMembers()+getNumVariableMembers())
00241 return boost::static_pointer_cast<Impl>(*impl)->variableMembers[
00242 index-getNumConstantMembers()].getValue();
00243 }
00244
00245 throw NoSuchMemberException(index);
00246 }
00247
00248 const MessageConstant& MessageDataType::getConstantMember(const std::string&
00249 name) const {
00250 if (hasConstantMember(name))
00251 return boost::static_pointer_cast<Impl>(*impl)->constantMembers[name].
00252 getValue();
00253 else
00254 throw NoSuchMemberException(name);
00255 }
00256
00257 const MessageConstant& MessageDataType::getConstantMember(int index) const {
00258 if ((index >= 0) && (index < getNumConstantMembers()))
00259 return boost::static_pointer_cast<Impl>(*impl)->constantMembers[index].
00260 getValue();
00261 else
00262 throw NoSuchMemberException(index);
00263 }
00264
00265 const MessageVariable& MessageDataType::getVariableMember(const std::string&
00266 name) const {
00267 if (hasVariableMember(name))
00268 return boost::static_pointer_cast<Impl>(*impl)->variableMembers[name].
00269 getValue();
00270 else
00271 throw NoSuchMemberException(name);
00272 }
00273
00274 const MessageVariable& MessageDataType::getVariableMember(int index) const {
00275 if ((index >= 0) && (index < getNumVariableMembers()))
00276 return boost::static_pointer_cast<Impl>(*impl)->variableMembers[index].
00277 getValue();
00278 else
00279 throw NoSuchMemberException(index);
00280 }
00281
00282 bool MessageDataType::hasMember(const std::string& name) const {
00283 return hasConstantMember(name) || hasVariableMember(name);
00284 }
00285
00286 bool MessageDataType::hasConstantMember(const std::string& name) const {
00287 if (impl)
00288 return boost::static_pointer_cast<Impl>(*impl)->constantMembers.
00289 hasField(name);
00290 else
00291 return false;
00292 }
00293
00294 bool MessageDataType::hasVariableMember(const std::string& name) const {
00295 if (impl)
00296 return boost::static_pointer_cast<Impl>(*impl)->variableMembers.
00297 hasField(name);
00298 else
00299 return false;
00300 }
00301
00302 bool MessageDataType::hasHeader() const {
00303 if (impl)
00304 return (boost::static_pointer_cast<Impl>(*impl)->variableMembers.
00305 hasField("header") &&
00306 (boost::static_pointer_cast<Impl>(*impl)->variableMembers["header"].
00307 getValue().getType().getIdentifier() == "std_msgs/Header"));
00308 else
00309 return false;
00310 }
00311
00312 const std::string& MessageDataType::ImplV::getIdentifier() const {
00313 return identifier;
00314 }
00315
00316 std::string MessageDataType::ImplV::getMD5Sum() const {
00317 return md5Sum.toString();
00318 }
00319
00320 const std::string& MessageDataType::ImplV::getDefinition() const {
00321 return definition;
00322 }
00323
00324 size_t MessageDataType::ImplV::getSize() const {
00325 if (isFixedSize()) {
00326 size_t size = 0;
00327
00328 for (size_t i = 0; i < variableMembers.getNumFields(); ++i)
00329 size += variableMembers[i].getValue().getType().getSize();
00330
00331 return size;
00332 }
00333 else
00334 return 0;
00335 }
00336
00337 bool MessageDataType::ImplV::isFixedSize() const {
00338 bool fixedSize = true;
00339
00340 for (size_t i = 0; i < variableMembers.getNumFields(); ++i)
00341 fixedSize &= variableMembers[i].getValue().getType().isFixedSize();
00342
00343 return fixedSize;
00344 }
00345
00346 bool MessageDataType::ImplV::isSimple() const {
00347 return false;
00348 }
00349
00350
00351
00352
00353
00354 void MessageDataType::addConstantMember(const MessageConstant& member) {
00355 if (impl)
00356 boost::static_pointer_cast<Impl>(*impl)->addConstantMember(member);
00357 else
00358 throw InvalidDataTypeException();
00359 }
00360
00361 MessageConstant MessageDataType::addConstantMember(const std::string& name,
00362 const Variant& value) {
00363 MessageConstant member(name, value);
00364 addConstantMember(member);
00365
00366 return member;
00367 }
00368
00369 void MessageDataType::addVariableMember(const MessageVariable& member) {
00370 if (impl)
00371 boost::static_pointer_cast<Impl>(*impl)->addVariableMember(member);
00372 else
00373 throw InvalidDataTypeException();
00374 }
00375
00376 MessageVariable MessageDataType::addVariableMember(const std::string& name,
00377 const DataType& type) {
00378 MessageVariable member(name, type);
00379 addVariableMember(member);
00380
00381 return member;
00382 }
00383
00384 Serializer MessageDataType::ImplV::createSerializer(const DataType& type)
00385 const {
00386 MessageFieldCollection<Serializer> memberSerializers;
00387
00388 for (size_t i = 0; i < variableMembers.getNumFields(); ++i)
00389 memberSerializers.appendField(variableMembers[i].getName(),
00390 variableMembers[i].getValue().getType().createSerializer());
00391
00392 return MessageSerializer(memberSerializers);
00393 }
00394
00395 Variant MessageDataType::ImplV::createVariant(const DataType& type) const {
00396 MessageFieldCollection<Variant> members;
00397
00398 for (size_t i = 0; i < variableMembers.getNumFields(); ++i)
00399 members.appendField(variableMembers[i].getName(),
00400 variableMembers[i].getValue().getType().createVariant());
00401
00402 return MessageVariant(type, members);
00403 }
00404
00405 void MessageDataType::ImplV::addConstantMember(const MessageConstant&
00406 member) {
00407 constantMembers.appendField(member.getName(), member);
00408
00409 std::ostringstream stream;
00410 stream << member << "\n";
00411
00412 definition += stream.str();
00413
00414 recalculateMD5Sum();
00415 }
00416
00417 void MessageDataType::ImplV::addVariableMember(const MessageVariable&
00418 member) {
00419 variableMembers.appendField(member.getName(), member);
00420
00421 std::ostringstream stream;
00422 stream << member << "\n";
00423
00424 definition += stream.str();
00425
00426 recalculateMD5Sum();
00427 }
00428
00429 void MessageDataType::ImplV::recalculateMD5Sum() {
00430 std::ostringstream stream;
00431
00432 for (size_t i = 0; i < constantMembers.getNumFields(); ++i) {
00433 const MessageConstant& constantMember = constantMembers[i].getValue();
00434
00435 stream << constantMember.getType().getIdentifier() << " " <<
00436 constantMember.getName() << "=" << constantMember.getValue() << "\n";
00437 }
00438
00439 for (size_t i = 0; i < variableMembers.getNumFields(); ++i) {
00440 const MessageVariable& variableMember = variableMembers[i].getValue();
00441
00442 DataType memberType = variableMember.getType();
00443 DataType bareMemberType = memberType;
00444
00445 while (bareMemberType.isArray()) {
00446 ArrayDataType arrayMemberType = bareMemberType;
00447 bareMemberType = arrayMemberType.getMemberType();
00448 }
00449
00450 if (bareMemberType.isBuiltin()) {
00451 stream << memberType.getIdentifier() << " " <<
00452 variableMember.getName() << "\n";
00453 }
00454 else if (bareMemberType.isMessage()) {
00455 MessageDataType messageMemberType = bareMemberType;
00456 stream << messageMemberType.getMD5Sum() << " " <<
00457 variableMember.getName() << "\n";
00458 }
00459 }
00460
00461 std::string md5SumText = stream.str();
00462 if (!md5SumText.empty())
00463 md5SumText.erase(md5SumText.size()-1);
00464
00465 md5Sum.clear();
00466 md5Sum.update(md5SumText);
00467 }
00468
00469
00470
00471
00472
00473 MessageDataType& MessageDataType::operator=(const DataType& src) {
00474 DataType::operator=(src);
00475
00476 if (impl)
00477 BOOST_ASSERT(boost::dynamic_pointer_cast<MessageDataType::Impl>(*impl));
00478
00479 return *this;
00480 }
00481
00482 const MessageMember& MessageDataType::operator[](int index) const {
00483 return getMember(index);
00484 }
00485
00486 }