00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019 #include <algorithm>
00020 #include <fstream>
00021 #include <list>
00022 #include <set>
00023 #include <sstream>
00024
00025 #include <boost/unordered_map.hpp>
00026
00027 #include <ros/package.h>
00028
00029 #include "variant_topic_tools/DataTypeRegistry.h"
00030 #include "variant_topic_tools/Exceptions.h"
00031 #include "variant_topic_tools/MessageDataType.h"
00032 #include "variant_topic_tools/MessageDefinitionParser.h"
00033 #include "variant_topic_tools/MessageType.h"
00034 #include "variant_topic_tools/MessageTypeParser.h"
00035 #include "variant_topic_tools/Publisher.h"
00036 #include "variant_topic_tools/Subscriber.h"
00037
00038 namespace variant_topic_tools {
00039
00040
00041
00042
00043
00044 MessageType::MessageType(const std::string& dataType, const std::string&
00045 md5Sum, const std::string& definition) :
00046 dataType(dataType),
00047 md5Sum(md5Sum),
00048 definition(definition) {
00049 }
00050
00051 MessageType::MessageType(const MessageDataType& dataType) :
00052 dataType(dataType.getIdentifier()),
00053 md5Sum(dataType.getMD5Sum()),
00054 definition(dataType.getDefinition()) {
00055 }
00056
00057 MessageType::MessageType(const MessageType& src) :
00058 dataType(src.dataType),
00059 md5Sum(src.md5Sum),
00060 definition(src.definition) {
00061 }
00062
00063 MessageType::~MessageType() {
00064 }
00065
00066
00067
00068
00069
00070 void MessageType::setDataType(const std::string& dataType) {
00071 this->dataType = dataType;
00072 }
00073
00074 const std::string& MessageType::getDataType() const {
00075 return dataType;
00076 }
00077
00078 void MessageType::setMD5Sum(const std::string& md5Sum) {
00079 this->md5Sum = md5Sum;
00080 }
00081
00082 const std::string& MessageType::getMD5Sum() const {
00083 return md5Sum;
00084 }
00085
00086 void MessageType::setDefinition(const std::string& definition) {
00087 this->definition = definition;
00088 }
00089
00090 const std::string& MessageType::getDefinition() const {
00091 return definition;
00092 }
00093
00094 bool MessageType::isValid() const {
00095 return !md5Sum.empty() && ((md5Sum == "*") || (md5Sum.length() == 32)) &&
00096 !dataType.empty() && !definition.empty();
00097 }
00098
00099
00100
00101
00102
00103 void MessageType::load(const std::string& messageDataType) {
00104 clear();
00105
00106 std::string messagePackage, messagePlainType;
00107 if (!MessageTypeParser::matchType(messageDataType, messagePackage,
00108 messagePlainType))
00109 throw InvalidMessageTypeException(messageDataType);
00110
00111 DataTypeRegistry registry;
00112 boost::unordered_map<std::string, std::string> definitions;
00113 std::list<std::string> typesInOrder;
00114 std::set<std::string> requiredTypes;
00115
00116 requiredTypes.insert(messageDataType);
00117 typesInOrder.push_back(messageDataType);
00118
00119 std::list<std::string>::iterator it = typesInOrder.begin();
00120
00121 while (it != typesInOrder.end()) {
00122 std::string package, plainType;
00123
00124 if (!MessageTypeParser::matchType(*it, package, plainType))
00125 throw InvalidMessageTypeException(*it);
00126
00127 if (package.empty()) {
00128 if (plainType == "Header")
00129 package = "std_msgs";
00130 else
00131 throw InvalidMessageTypeException(*it);
00132 }
00133
00134 std::string packagePath = ros::package::getPath(package);
00135 if (packagePath.empty())
00136 throw PackageNotFoundException(package);
00137
00138 std::string messageFilename(packagePath+"/msg/"+plainType+".msg");
00139 std::ifstream messageFile(messageFilename.c_str());
00140 std::string messageDefinition;
00141
00142 if (messageFile.is_open()) {
00143 messageFile.seekg(0, std::ios::end);
00144 messageDefinition.reserve(messageFile.tellg());
00145 messageFile.seekg(0, std::ios::beg);
00146
00147 messageDefinition.assign((std::istreambuf_iterator<char>(messageFile)),
00148 std::istreambuf_iterator<char>());
00149 }
00150 else
00151 throw FileOpenException(messageFilename);
00152
00153 messageFile.close();
00154
00155 if (!messageDefinition.empty()) {
00156 std::istringstream stream(messageDefinition);
00157 std::string line;
00158
00159 while (std::getline(stream, line)) {
00160 std::string memberName, memberType;
00161 size_t memberSize;
00162
00163 if (MessageDefinitionParser::matchArray(line, memberName, memberType,
00164 memberSize) || MessageDefinitionParser::match(line, memberName,
00165 memberType)) {
00166 std::string memberPackage, plainMemberType;
00167
00168 if (!MessageTypeParser::matchType(memberType, memberPackage,
00169 plainMemberType))
00170 throw InvalidMessageTypeException(memberType);
00171
00172 if (!registry.getDataType(memberType).isBuiltin()) {
00173 if (memberPackage.empty()) {
00174 if (plainMemberType == "Header")
00175 memberPackage = "std_msgs";
00176 else
00177 memberPackage = package;
00178
00179 memberType = memberPackage+"/"+plainMemberType;
00180 }
00181
00182 if (requiredTypes.find(memberType) == requiredTypes.end()) {
00183 requiredTypes.insert(memberType);
00184 typesInOrder.push_back(memberType);
00185 }
00186 }
00187 }
00188 }
00189 }
00190
00191 definitions[*it] = messageDefinition;
00192 ++it;
00193 }
00194
00195 for (std::list<std::string>::const_iterator it = typesInOrder.begin();
00196 it != typesInOrder.end(); ++it) {
00197 if (!definition.empty()) {
00198 definition += "\n"+std::string(80, '=')+"\n";
00199 definition += "MSG: "+*it+"\n";
00200 }
00201
00202 definition += definitions[*it];
00203 }
00204
00205 if (!definition.empty())
00206 dataType = messageDataType;
00207 }
00208
00209 void MessageType::clear() {
00210 dataType.clear();
00211 md5Sum = "*";
00212 definition.clear();
00213 }
00214
00215 void MessageType::write(std::ostream& stream) const {
00216 stream << dataType;
00217 }
00218
00219 Publisher MessageType::advertise(ros::NodeHandle& nodeHandle,
00220 const std::string& topic, size_t queueSize, bool latch, const
00221 ros::SubscriberStatusCallback& connectCallback) {
00222 Publisher publisher;
00223
00224 if (isValid())
00225 publisher.impl.reset(new Publisher::Impl(nodeHandle, *this, topic,
00226 queueSize, latch, connectCallback));
00227
00228 return publisher;
00229 }
00230
00231 Subscriber MessageType::subscribe(ros::NodeHandle& nodeHandle, const
00232 std::string& topic, size_t queueSize, const SubscriberCallback&
00233 callback) {
00234 Subscriber subscriber;
00235
00236 subscriber.impl.reset(new Subscriber::Impl(nodeHandle, *this, topic,
00237 queueSize, callback));
00238
00239 return subscriber;
00240 }
00241
00242
00243
00244
00245
00246 bool MessageType::operator==(const MessageType& type) const {
00247 return (dataType == type.dataType) && (md5Sum == type.md5Sum);
00248 }
00249
00250 bool MessageType::operator!=(const MessageType& type) const {
00251 return (dataType != type.dataType) || (md5Sum != type.md5Sum);
00252 }
00253
00254 std::ostream& operator<<(std::ostream& stream, const MessageType&
00255 messageType) {
00256 messageType.write(stream);
00257 return stream;
00258 }
00259
00260 }