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 #include <list>
00020 #include <set>
00021
00022 #include <boost/unordered_map.hpp>
00023
00024 #include "variant_topic_tools/DataTypeRegistry.h"
00025 #include "variant_topic_tools/Exceptions.h"
00026 #include "variant_topic_tools/MessageDataType.h"
00027 #include "variant_topic_tools/MessageDefinition.h"
00028 #include "variant_topic_tools/MessageDefinitionParser.h"
00029 #include "variant_topic_tools/MessageTypeParser.h"
00030
00031 namespace variant_topic_tools {
00032
00033
00034
00035
00036
00037 MessageDefinition::MessageDefinition() {
00038 }
00039
00040 MessageDefinition::MessageDefinition(const MessageType& messageType) {
00041 setMessageType(messageType);
00042 }
00043
00044 MessageDefinition::MessageDefinition(const MessageDataType& messageDataType) :
00045 messageDataType(messageDataType) {
00046 }
00047
00048 MessageDefinition::MessageDefinition(const MessageDefinition& src) :
00049 MessageFieldCollection(src),
00050 messageDataType(src.messageDataType) {
00051 }
00052
00053 MessageDefinition::~MessageDefinition() {
00054 }
00055
00056
00057
00058
00059
00060 void MessageDefinition::setMessageType(const MessageType& messageType) {
00061 clear();
00062
00063 if (messageType.isValid()) {
00064 DataType dataType(messageType.getDataType());
00065
00066 if (!dataType.isValid()) {
00067 DataTypeRegistry registry;
00068
00069 std::vector<MessageType> messageTypes;
00070 boost::unordered_map<std::string, size_t> definedTypes;
00071 boost::unordered_map<size_t, std::set<size_t> > requiredTypes;
00072
00073 MessageDefinitionParser::parse(messageType.getDataType(),
00074 messageType.getDefinition(), messageTypes);
00075
00076 for (size_t i = 0; i < messageTypes.size(); ++i)
00077 definedTypes[messageTypes[i].getDataType()] = i;
00078
00079 for (size_t i = 0; i < messageTypes.size(); ++i) {
00080 std::string package, plainType;
00081
00082 if (!MessageTypeParser::matchType(messageTypes[i].getDataType(),
00083 package, plainType))
00084 throw InvalidMessageTypeException(messageTypes[i].getDataType());
00085
00086 std::istringstream stream(messageTypes[i].getDefinition());
00087 std::string line;
00088
00089 while (std::getline(stream, line)) {
00090 std::string memberName, memberType;
00091 size_t memberSize;
00092
00093 if (MessageDefinitionParser::matchArray(line, memberName, memberType,
00094 memberSize) || MessageDefinitionParser::match(line, memberName,
00095 memberType)) {
00096 std::string memberPackage, plainMemberType;
00097
00098 if (!MessageTypeParser::matchType(memberType, memberPackage,
00099 plainMemberType))
00100 throw InvalidMessageTypeException(memberType);
00101
00102 if (memberPackage.empty()) {
00103 if (plainMemberType == "Header")
00104 memberPackage = "std_msgs";
00105 else
00106 memberPackage = package;
00107
00108 memberType = memberPackage+"/"+plainMemberType;
00109 }
00110
00111 boost::unordered_map<std::string, size_t>::iterator it =
00112 definedTypes.find(memberType);
00113
00114 if (it != definedTypes.end())
00115 requiredTypes[i].insert(it->second);
00116 }
00117 }
00118 }
00119
00120 std::list<size_t> typesToBeDefined;
00121 typesToBeDefined.push_back(definedTypes[messageType.getDataType()]);
00122
00123 while (!typesToBeDefined.empty()) {
00124 size_t i = typesToBeDefined.back();
00125 const std::set<size_t>& currentRequiredTypes = requiredTypes[i];
00126 bool allRequiredTypesDefined = true;
00127
00128 for (std::set<size_t>::const_iterator it = currentRequiredTypes.
00129 begin(); it != currentRequiredTypes.end(); ++it) {
00130 if (!registry.getDataType(messageTypes[*it].getDataType()).
00131 isValid()) {
00132 typesToBeDefined.push_back(*it);
00133 allRequiredTypesDefined = false;
00134
00135 break;
00136 }
00137 }
00138
00139 if (allRequiredTypesDefined) {
00140 registry.addMessageDataType(messageTypes[i].getDataType(),
00141 messageTypes[i].getDefinition());
00142 typesToBeDefined.pop_back();
00143 }
00144 }
00145 }
00146
00147 messageDataType = MessageDataType(messageType.getDataType());
00148 fill(messageDataType, *this);
00149 }
00150 }
00151
00152 void MessageDefinition::setMessageDataType(const MessageDataType&
00153 messageDataType) {
00154 setMessageType(messageDataType);
00155 }
00156
00157 MessageDataType MessageDefinition::getMessageDataType() const {
00158 return messageDataType;
00159 }
00160
00161 bool MessageDefinition::isValid() const {
00162 return messageDataType.isValid();
00163 }
00164
00165
00166
00167
00168
00169 void MessageDefinition::load(const std::string& messageDataType) {
00170 clear();
00171
00172 MessageType messageType;
00173 messageType.load(messageDataType);
00174
00175 setMessageType(messageType);
00176 }
00177
00178 void MessageDefinition::clear() {
00179 MessageFieldCollection::clear();
00180 messageDataType = MessageDataType();
00181 }
00182
00183 void MessageDefinition::fill(const MessageDataType& currentDataType,
00184 MessageFieldCollection<DataType>& currentCollection) {
00185 for (size_t i = 0; i < currentDataType.getNumMembers(); ++i) {
00186 currentCollection.appendField(currentDataType[i].getName(),
00187 currentDataType[i].getType());
00188
00189 if (currentDataType[i].getType().isMessage())
00190 fill(currentDataType[i].getType(), currentCollection[i]);
00191 }
00192 }
00193
00194 void MessageDefinition::write(std::ostream& stream) const {
00195 stream << MessageDataType(messageDataType).getDefinition();
00196 }
00197
00198
00199
00200
00201
00202 std::ostream& operator<<(std::ostream& stream, const MessageDefinition&
00203 messageDefinition) {
00204 messageDefinition.write(stream);
00205 return stream;
00206 }
00207
00208 }