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
00033
00034
00035 #include <boost/algorithm/string.hpp>
00036 #include <boost/utility/string_ref.hpp>
00037 #include <boost/lexical_cast.hpp>
00038 #include <ros/ros.h>
00039 #include <iostream>
00040 #include <sstream>
00041 #include <functional>
00042 #include <boost/regex.hpp>
00043 #include <boost/algorithm/string/regex.hpp>
00044
00045 #include "ros_type_introspection/parser.hpp"
00046
00047
00048 namespace RosIntrospection{
00049
00050
00051 inline bool isSeparator(const std::string& line)
00052 {
00053 if(line.size() != 80 ) return false;
00054 for (int i=0; i<80; i++)
00055 {
00056 if( line[i] != '=') return false;
00057 }
00058 return true;
00059 }
00060
00061
00062 inline SString strippedTypeName(const boost::string_ref& line )
00063 {
00064 boost::string_ref output( line );
00065 int pos = line.find_last_of('/');
00066 if( pos != output.npos )
00067 {
00068 output.remove_prefix( pos+1 );
00069 }
00070 return SString( output.data(), output.length() );
00071 }
00072
00073
00074 ROSType::ROSType(const std::string &name):
00075 _base_name(name)
00076 {
00077
00078 std::vector<std::string> split;
00079 std::string type_field;
00080 boost::split(split, name, boost::is_any_of("/"));
00081
00082 if( split.size() == 1)
00083 {
00084 type_field = split[0];
00085 }
00086 else{
00087 _pkg_name = split[0];
00088 type_field = split[1];
00089 }
00090
00091
00092 static const boost::regex array_regex("(.+)(\\[([0-9]*)\\])");
00093
00094 boost::smatch what;
00095 if (boost::regex_search(type_field, what, array_regex))
00096 {
00097 _msg_name = std::string(what[1].first, what[1].second);
00098
00099 if (what.size() == 3) {
00100 _array_size = -1;
00101 }
00102 else if (what.size() == 4) {
00103 std::string size(what[3].first, what[3].second);
00104 _array_size = size.empty() ? -1 : atoi(size.c_str());
00105 }
00106 else {
00107 throw std::runtime_error("Didn't catch bad type string: " + name);
00108 }
00109 } else {
00110 _msg_name = type_field;
00111 _array_size = 1;
00112 }
00113
00114 _id = RosIntrospection::OTHER;
00115
00116 if( _msg_name.compare( "bool" ) == 0 ) {
00117 _id = RosIntrospection::BOOL;
00118 _deserialize_impl = [](uint8_t** buffer) {
00119 return ReadFromBuffer<bool>(buffer);
00120 };
00121 }
00122 else if(_msg_name.compare( "byte" ) == 0 ) {
00123 _id = RosIntrospection::BYTE;
00124 _deserialize_impl = [](uint8_t** buffer) {
00125 return ReadFromBuffer<int8_t>(buffer);
00126 };
00127 }
00128 else if(_msg_name.compare( "char" ) == 0 ) {
00129 _id = RosIntrospection::CHAR;
00130 _deserialize_impl = [](uint8_t** buffer) {
00131 return ReadFromBuffer<char>(buffer);
00132 };
00133 }
00134 else if(_msg_name.compare( "uint8" ) == 0 ) {
00135 _id = RosIntrospection::UINT8;
00136 _deserialize_impl = [](uint8_t** buffer) {
00137 return ReadFromBuffer<uint8_t>(buffer);
00138 };
00139 }
00140 else if(_msg_name.compare( "uint16" ) == 0 ) {
00141 _id = RosIntrospection::UINT16;
00142 _deserialize_impl = [](uint8_t** buffer) {
00143 return ReadFromBuffer<uint16_t>(buffer);
00144 };
00145 }
00146 else if(_msg_name.compare( "uint32" ) == 0 ) {
00147 _id = RosIntrospection::UINT32;
00148 _deserialize_impl = [](uint8_t** buffer) {
00149 return ReadFromBuffer<uint32_t>(buffer);
00150 };
00151 }
00152 else if(_msg_name.compare( "uint64" ) == 0 ) {
00153 _id = RosIntrospection::UINT64;
00154 _deserialize_impl = [](uint8_t** buffer) {
00155 return ReadFromBuffer<uint64_t>(buffer);
00156 };
00157 }
00158 else if(_msg_name.compare( "int8" ) == 0 ) {
00159 _id = RosIntrospection::INT8;
00160 _deserialize_impl = [](uint8_t** buffer) {
00161 return ReadFromBuffer<int8_t>(buffer);
00162 };
00163 }
00164 else if(_msg_name.compare( "int16" ) == 0 ) {
00165 _id = RosIntrospection::INT16;
00166 _deserialize_impl = [](uint8_t** buffer) {
00167 return ReadFromBuffer<int16_t>(buffer);
00168 };
00169 }
00170 else if(_msg_name.compare( "int32" ) == 0 ) {
00171 _id = RosIntrospection::INT32;
00172 _deserialize_impl = [](uint8_t** buffer) {
00173 return ReadFromBuffer<int32_t>(buffer);
00174 };
00175 }
00176 else if(_msg_name.compare( "int64" ) == 0 ) {
00177 _id = RosIntrospection::INT64;
00178 _deserialize_impl = [](uint8_t** buffer) {
00179 return ReadFromBuffer<int64_t>(buffer);
00180 };
00181 }
00182 else if(_msg_name.compare( "float32" ) == 0 ) {
00183 _id = RosIntrospection::FLOAT32;
00184 _deserialize_impl = [](uint8_t** buffer) {
00185 return ReadFromBuffer<float>(buffer);
00186 };
00187 }
00188 else if(_msg_name.compare( "float64" ) == 0 ) {
00189 _id = RosIntrospection::FLOAT64;
00190 _deserialize_impl = [](uint8_t** buffer) {
00191 return ReadFromBuffer<double>(buffer);
00192 };
00193 }
00194 else if(_msg_name.compare( "time" ) == 0 ) {
00195 _id = RosIntrospection::TIME;
00196 _deserialize_impl = [](uint8_t** buffer) {
00197 ros::Time tmp;
00198 tmp.sec = ReadFromBuffer<uint32_t>(buffer);
00199 tmp.nsec = ReadFromBuffer<uint32_t>(buffer);
00200 return tmp;
00201 };
00202 }
00203 else if(_msg_name.compare( "duration" ) == 0 ) {
00204 _id = RosIntrospection::DURATION;
00205 _deserialize_impl = [](uint8_t** buffer) {
00206 ros::Time tmp;
00207 tmp.sec = ReadFromBuffer<int32_t>(buffer);
00208 tmp.nsec = ReadFromBuffer<int32_t>(buffer);
00209 return tmp;
00210 };
00211 }
00212 else if(_msg_name.compare( "string" ) == 0 ) {
00213 _id = RosIntrospection::STRING;
00214 }
00215 }
00216
00217
00218 ROSTypeList buildROSTypeMapFromDefinition(
00219 const std::string & type_name,
00220 const std::string & msg_definition)
00221 {
00222 static const boost::regex msg_separation_regex("^=+\\n+");
00223
00224 ROSTypeList type_list;
00225
00226 std::vector<std::string> split;
00227 boost::split_regex(split, msg_definition, msg_separation_regex);
00228
00229 std::vector<ROSType> all_types;
00230
00231 for (size_t i = 0; i < split.size(); ++i) {
00232
00233 ROSMessage msg( split[i] );
00234 if( i == 0)
00235 {
00236 msg.mutateType( ROSType(type_name) );
00237 }
00238
00239 type_list.push_back( msg );
00240 all_types.push_back( msg.type() );
00241 }
00242
00243 for( ROSMessage& msg: type_list )
00244 {
00245 msg.updateTypes( all_types );
00246 }
00247
00248 return type_list;
00249 }
00250
00251
00252 std::ostream& operator<<(std::ostream& ss, const ROSTypeList& type_list)
00253 {
00254 for (const ROSMessage& msg: type_list)
00255 {
00256 ss<< "\n" << msg.type().baseName() <<" : " << std::endl;
00257
00258 for (const ROSField& field : msg.fields() )
00259 {
00260 ss << "\t" << field.name()
00261 <<" : " << field.type().baseName() << std::endl;
00262 }
00263 }
00264 return ss;
00265 }
00266
00267
00268 const SString &ROSType::baseName() const
00269 {
00270 return _base_name;
00271 }
00272
00273 const SString &ROSType::msgName() const
00274 {
00275 return _msg_name;
00276 }
00277
00278 const SString &ROSType::pkgName() const
00279 {
00280 return _pkg_name;
00281 }
00282
00283 void ROSType::setPkgName(const SString &new_pkg)
00284 {
00285 assert(_pkg_name.size() == 0);
00286 _pkg_name = new_pkg;
00287 _base_name = SString(new_pkg).append("/").append(_base_name);
00288 }
00289
00290 bool ROSType::isArray() const
00291 {
00292 return _array_size != 1;
00293 }
00294
00295 bool ROSType::isBuiltin() const
00296 {
00297 return _id != RosIntrospection::OTHER;
00298 }
00299
00300 int ROSType::arraySize() const
00301 {
00302 return _array_size;
00303 }
00304
00305 int ROSType::typeSize() const
00306 {
00307 const int sizes[] = {1, 1, 1,
00308 1, 2, 4, 8,
00309 1, 2, 4, 8,
00310 4, 8,
00311 8, 8,
00312 -1, -1};
00313 return sizes[ _id ];
00314 }
00315
00316 BuiltinType ROSType::typeID() const
00317 {
00318 return this->_id;
00319 }
00320
00321
00322
00323 ROSMessage::ROSMessage(const std::string &msg_def)
00324 {
00325 std::istringstream messageDescriptor(msg_def);
00326 boost::match_results<std::string::const_iterator> what;
00327
00328 for (std::string line; std::getline(messageDescriptor, line, '\n') ; )
00329 {
00330 std::string::const_iterator begin = line.begin(), end = line.end();
00331
00332
00333 if (boost::regex_search( begin, end, what,
00334 boost::regex("(^\\s*$|^\\s*#)")))
00335 {
00336 continue;
00337 }
00338
00339 if( line.compare(0, 5, "MSG: ") == 0)
00340 {
00341 line.erase(0,5);
00342 _type = ROSType(line);
00343 }
00344 else{
00345 auto new_field = ROSField(line);
00346 _fields.push_back(new_field);
00347 }
00348 }
00349 }
00350
00351 void ROSMessage::updateTypes(const std::vector<ROSType> &all_types)
00352 {
00353 for (ROSField& field: _fields)
00354 {
00355
00356 if( field.type().pkgName().size() == 0 )
00357 {
00358 for (const ROSType& known_type: all_types)
00359 {
00360 if( field.type().msgName() == known_type.msgName() )
00361 {
00362 field._type.setPkgName( known_type.pkgName() );
00363 break;
00364 }
00365 }
00366 }
00367 }
00368 }
00369
00370 ROSField::ROSField(const std::string &definition)
00371 {
00372 static const boost::regex type_regex("[a-zA-Z][a-zA-Z0-9_]*"
00373 "(/[a-zA-Z][a-zA-Z0-9_]*){0,1}"
00374 "(\\[[0-9]*\\]){0,1}");
00375
00376 static const boost::regex field_regex("[a-zA-Z][a-zA-Z0-9_]*");
00377
00378 using boost::regex;
00379 std::string::const_iterator begin = definition.begin();
00380 std::string::const_iterator end = definition.end();
00381 boost::match_results<std::string::const_iterator> what;
00382
00383
00384 std::string type, fieldname, value;
00385
00386 if( regex_search(begin, end, what, type_regex)) {
00387 type = what[0];
00388 begin = what[0].second;
00389 }
00390 else {
00391 throw std::runtime_error("Bad type when parsing message ----\n" + definition);
00392 }
00393
00394 if (regex_search(begin, end, what, field_regex))
00395 {
00396 fieldname = what[0];
00397 begin = what[0].second;
00398 }
00399 else {
00400 throw std::runtime_error("Bad field when parsing message ----\n" + definition);
00401 }
00402
00403
00404
00405 if (regex_search(begin, end, what, boost::regex("\\S")))
00406 {
00407 if (what[0] == "=")
00408 {
00409 begin = what[0].second;
00410
00411 if (type == "string") {
00412 value.assign(begin, end);
00413 }
00414 else {
00415 if (regex_search(begin, end, what, boost::regex("\\s*#")))
00416 {
00417 value.assign(begin, what[0].first);
00418 }
00419 else {
00420 value.assign(begin, end);
00421 }
00422
00423 }
00424
00425 boost::algorithm::trim(value);
00426 } else if (what[0] == "#") {
00427
00428 } else {
00429
00430 throw std::runtime_error("Unexpected character after type and field ----\n" +
00431 definition);
00432 }
00433 }
00434 _type = ROSType( type );
00435 _name = fieldname;
00436 _value = value;
00437 }
00438
00439
00440
00441
00442 }
00443
00444