conversion.cpp
Go to the documentation of this file.
00001 //=================================================================================================
00002 // Copyright (c) 2012, Johannes Meyer, TU Darmstadt
00003 // All rights reserved.
00004 
00005 // Redistribution and use in source and binary forms, with or without
00006 // modification, are permitted provided that the following conditions are met:
00007 //     * Redistributions of source code must retain the above copyright
00008 //       notice, this list of conditions and the following disclaimer.
00009 //     * Redistributions in binary form must reproduce the above copyright
00010 //       notice, this list of conditions and the following disclaimer in the
00011 //       documentation and/or other materials provided with the distribution.
00012 //     * Neither the name of the Flight Systems and Automatic Control group,
00013 //       TU Darmstadt, nor the names of its contributors may be used to
00014 //       endorse or promote products derived from this software without
00015 //       specific prior written permission.
00016 
00017 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00018 // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00019 // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00020 // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY
00021 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00022 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00023 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00024 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00025 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00026 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00027 //=================================================================================================
00028 
00029 #include <rosmatlab/conversion.h>
00030 #include <rosmatlab/exception.h>
00031 #include <rosmatlab/log.h>
00032 
00033 #include <introspection/message.h>
00034 #include <introspection/type.h>
00035 
00036 #include <ros/time.h>
00037 #include <ros/duration.h>
00038 
00039 #include <stdio.h>
00040 #include <ros/message_traits.h>
00041 #include <boost/algorithm/string.hpp>
00042 
00043 #include <mex.h>
00044 
00045 namespace rosmatlab {
00046 
00047 Conversion::Conversion(const MessagePtr &message) : message_(message), options_(defaultOptions())
00048 {
00049   options_.merge(perMessageOptions(message));
00050 }
00051 
00052 Conversion::Conversion(const MessagePtr &message, const ConversionOptions& options) : message_(message), options_(defaultOptions())
00053 {
00054   options_.merge(perMessageOptions(message));
00055   options_.merge(options);
00056 }
00057 
00058 Conversion::Conversion(const Conversion &other, const MessagePtr &message)
00059   : message_(message ? message : other.message_)
00060   , options_(other.options_)
00061 {
00062   options_.merge(perMessageOptions(message));
00063 }
00064 
00065 Conversion::~Conversion() {}
00066 
00067 Array Conversion::toMatlab() {
00068   return toMatlab(0);
00069 }
00070 
00071 Array Conversion::toMatlab(Array target, std::size_t index, std::size_t size) {
00072   switch(options_.conversionType()) {
00073     case ConversionOptions::MATLAB_STRUCT:
00074       return toStruct(target, index, size);
00075     case ConversionOptions::MATLAB_MATRIX:
00076       return toDoubleMatrix(target, index, size);
00077     case ConversionOptions::MATLAB_EXTENDED_STRUCT:
00078       return toExtendedStruct(target, index, size);
00079   }
00080 
00081   throw Exception("Unsupported conversion type " + boost::lexical_cast<std::string>(options_.conversionType()));
00082 }
00083 
00084 Array Conversion::toDoubleMatrix() {
00085   return toDoubleMatrix(0);
00086 }
00087 
00088 Array Conversion::toDoubleMatrix(Array target, std::size_t index, std::size_t size) {
00089   if (!target) target = mxCreateDoubleMatrix(expanded()->size(), size > 0 ? size : index + 1, mxREAL);
00090 
00091   bool do_realloc = false;
00092   if (mxGetM(target) < expanded()->size()) { mxSetM(target, expanded()->size()); do_realloc = true; }
00093   if (mxGetN(target) < index + 1) { mxSetN(target, index + 1); do_realloc = true; }
00094   if (do_realloc)
00095   {
00096     mxSetData(target, mxRealloc(mxGetData(target), mxGetN(target) * mxGetN(target) * sizeof(double)));
00097   }
00098 
00099   double *data = mxGetPr(target) + mxGetM(target) * index;
00100   for(Message::const_iterator field = expanded()->begin(); field != expanded()->end(); ++field) {
00101     // if (!(*field)->getType()->isNumeric()) continue;
00102     *data++ = (*field)->getType()->as_double((*field)->get());
00103   }
00104   return target;
00105 }
00106 
00107 Array Conversion::toStruct() {
00108   return toStruct(0);
00109 }
00110 
00111 Array Conversion::toStruct(Array target, std::size_t index, std::size_t size) {
00112 //  ROSMATLAB_PRINTF("Constructing message %s (%s)...", message_->getName(), message_->getDataType());
00113 
00114   if (!target) target = mxCreateStructMatrix(1, size > 0 ? size : index + 1,
00115                                              message_->getFieldNames().size(),
00116                                              const_cast<const char **>(message_->getFieldNames().data())
00117                                              );
00118 
00119   // add fields if number of fields is 0
00120   if (mxGetNumberOfFields(target) == 0) {
00121     for(V_FieldName::const_iterator it = message_->getFieldNames().begin(); it != message_->getFieldNames().end(); ++it) {
00122       mxAddField(target, *it);
00123     }
00124   }
00125 
00126   // iterate through all fields
00127   for(Message::const_iterator field = message_->begin(); field != message_->end(); ++field) {
00128     const char *field_name = (*field)->getName();
00129 
00130     if ((*field)->isMessage()) {
00131       MessagePtr field_message = messageByDataType((*field)->getValueType());
00132 
00133       if (field_message) {
00134         Array child = 0; /* mxCreateStructMatrix(1, (*field)->size(), field_message->getFieldNames().size(), const_cast<const char **>(field_message->getFieldNames().data())); */
00135 
00136         // iterate over array
00137         for(std::size_t j = 0; j < (*field)->size(); j++) {
00138 //          ROSMATLAB_PRINTF("Expanding field %s[%u] (%s)...", (*field)->getName(), j, (*field)->getDataType());
00139           MessagePtr expanded = (*field)->expand(j);
00140           if (expanded) {
00141             child = Conversion(expanded).toMatlab(child, j, (*field)->size());
00142           } else {
00143             ROSMATLAB_PRINTF("Error during expansion of %s[%u] (%s)...", (*field)->getName(), j, (*field)->getDataType());
00144           }
00145         }
00146 
00147         mxSetField(target, index, field_name, child);
00148 
00149       } else {
00150         ROSMATLAB_PRINTF("Error during conversion of field %s[%u] (%s): unknown datatype", (*field)->getName(), (*field)->size(), (*field)->getDataType());
00151         const char **field_names = { 0 };
00152         mxSetField(target, index, field_name, mxCreateStructMatrix(1, (*field)->size(), 0, field_names));
00153       }
00154 
00155     } else {
00156       mxSetField(target, index, field_name, convertToMatlab(*field));
00157     }
00158   }
00159 
00160   // add meta data to the struct
00161   if (options_.addMetaData()) {
00162     if (mxGetFieldNumber(target, "DATATYPE") == -1) mxAddField(target, "DATATYPE");
00163     mxSetField(target, index, "DATATYPE", mxCreateString(message_->getDataType()));
00164     if (mxGetFieldNumber(target, "MD5SUM") == -1) mxAddField(target, "MD5SUM");
00165     mxSetField(target, index, "MD5SUM", mxCreateString(message_->getMD5Sum()));
00166   }
00167 
00168   return target;
00169 }
00170 
00171 Array Conversion::toExtendedStruct() {
00172   return toStruct(0);
00173 }
00174 
00175 Array Conversion::toExtendedStruct(Array target, std::size_t index, std::size_t size) {
00176   static const char *fieldnames[] = { "count", "stamps", "data", "fields", "strings", "string_fields" /*, "arrays", "array_fields" */ };
00177   if (!target) target = mxCreateStructMatrix(1, 1, sizeof(fieldnames)/sizeof(*fieldnames), fieldnames);
00178 
00179   // set count
00180   if (size == 0) size = index + 1;
00181   mxSetField(target, 0, "count", mxCreateDoubleScalar(size));
00182 
00183   // set stamps
00184   mxArray *stamps = mxGetField(target, 0, "stamps");
00185   if (message_->hasHeader()) {
00186     if (!stamps) stamps = mxCreateDoubleMatrix(1, size, mxREAL);
00187     *(mxGetPr(stamps) + index) = message_->getHeader(message_->getConstInstance())->stamp.toSec();
00188     mxSetField(target, 0, "stamps", stamps);
00189   }
00190 
00191   // set data
00192   mxArray *data = mxGetField(target, 0, "data");
00193   data = toDoubleMatrix(data, index, size);
00194   mxSetField(target, 0, "data", data);
00195 
00196   // set fields
00197   mxArray *fields = mxGetField(target, 0, "fields");
00198   if (!fields) {
00199     const V_FieldName& fieldnames = expanded()->getFieldNames();
00200     fields = mxCreateCellMatrix(fieldnames.size(), 1);
00201     for(int i = 0; i < fieldnames.size(); i++) {
00202       mxSetCell(fields, i, mxCreateString(fieldnames.at(i)));
00203     }
00204     mxSetField(target, 0, "fields", fields);
00205   }
00206 
00207   // set strings
00208   mxArray *strings = mxGetField(target, 0, "strings");
00209   mxArray *string_fields = mxGetField(target, 0, "string_fields");
00210 
00211   std::size_t string_count = 0;
00212   for(Message::const_iterator field_it = expanded()->begin(); field_it != expanded()->end(); ++field_it) {
00213     const FieldPtr& field = *field_it;
00214     if (field->getType()->isString()) string_count++;
00215   }
00216 
00217   if (string_count > 0) {
00218     if (!string_fields) {
00219       string_fields = mxCreateCellMatrix(string_count, 1);
00220       std::size_t string_index = 0;
00221       for(Message::const_iterator field_it = expanded()->begin(); field_it != expanded()->end(); ++field_it, ++string_index) {
00222         const FieldPtr& field = *field_it;
00223         if (!field->getType()->isString()) continue;
00224         mxSetCell(string_fields, string_index, mxCreateString(field->getName()));
00225       }
00226     }
00227 
00228     if (!strings) {
00229       strings = mxCreateCellMatrix(string_count, size);
00230     } else if (mxGetM(strings) < string_count) {
00231       throw Exception("string_fields cell has wrong size");
00232     }
00233 
00234     std::size_t string_index = 0;
00235     for(Message::const_iterator field_it = expanded()->begin(); field_it != expanded()->end(); ++field_it, ++string_index) {
00236       const FieldPtr& field = *field_it;
00237       if (!field->getType()->isString()) continue;
00238       mxSetCell(strings, index * string_count + string_index, mxCreateString(field->getType()->as_string(field->get(0)).c_str()));
00239     }
00240 
00241     mxSetField(target, 0, "strings", strings);
00242     mxSetField(target, 0, "string_fields", string_fields);
00243   }
00244 
00245   // set arrays
00246 
00247 
00248   return target;
00249 }
00250 
00251 std::size_t Conversion::numberOfInstances(ConstArray source)
00252 {
00253   if (mxIsStruct(source)) {
00254     return mxGetNumberOfElements(source);
00255   }
00256 
00257   if (mxIsDouble(source)) {
00258     return mxGetN(source);
00259   }
00260 
00261   if (mxIsChar(source)) {
00262     return 1;
00263   }
00264 
00265   throw Exception("Cannot parse an array of class " + std::string(mxGetClassName(source)) + " as ROS message");
00266 }
00267 
00268 MessagePtr Conversion::fromMatlab(ConstArray source, std::size_t index)
00269 {
00270   MessagePtr target = message_->introspect(message_->createInstance());
00271   fromMatlab(target, source, index);
00272   return target;
00273 }
00274 
00275 void Conversion::fromMatlab(const MessagePtr& target, ConstArray source, std::size_t index)
00276 {
00277   if (mxIsStruct(source)) {
00278     fromStruct(target, source, index);
00279     return;
00280   }
00281 
00282   if (mxIsDouble(source)) {
00283     fromDoubleMatrix(target, source, index);
00284     return;
00285   }
00286 
00287   if (mxIsChar(source) && target->hasType<std_msgs::String>()) {
00288     std_msgs::StringPtr data = target->getInstanceAs<std_msgs::String>();
00289     if (data) {
00290       data->data = Options::getString(source);
00291       return;
00292     }
00293   }
00294 
00295   throw Exception("Cannot parse an array of class " + std::string(mxGetClassName(source)) + " as " + std::string(target->getDataType()) + " message");
00296 }
00297 
00298 void Conversion::fromDoubleMatrix(const MessagePtr& target, ConstArray source, std::size_t n)
00299 {
00300   if (!mxIsDouble(source)) return;
00301 
00302   const double *begin = 0;
00303   const double *end = 0;
00304 
00305   if (mxGetM(source) == 1 && n == 0) {
00306     begin = mxGetPr(source);
00307     end   = begin + mxGetN(source);
00308   } else {
00309     if (n >= mxGetN(source)) throw Exception("Column index out of bounds");
00310     begin = mxGetPr(source) + mxGetM(source) * n;
00311     end   = mxGetPr(source) + mxGetM(source) * (n + 1);
00312   }
00313 
00314   fromDoubleMatrix(target, begin, end);
00315 }
00316 
00317 void Conversion::fromDoubleMatrix(const MessagePtr &target, const double *begin, const double *end)
00318 {
00319   for(Message::const_iterator field = target->begin(); field != target->end(); ++field) {
00320     begin = convertFromDouble(*field, begin, end);
00321   }
00322   if (begin != end) throw Exception("Failed to parse an array of type " + std::string(target->getDataType()) + ": vector is too long");
00323 }
00324 
00325 void Conversion::fromStruct(const MessagePtr &target, ConstArray source, std::size_t index)
00326 {
00327   if (!mxIsStruct(source)) return;
00328   if (index >= mxGetNumberOfElements(source)) throw Exception("Index out of bounds");
00329 
00330   for(Message::const_iterator field = target->begin(); field != target->end(); ++field) {
00331     ConstArray field_source = mxGetField(source, index, (*field)->getName());
00332     if (!field_source) continue;
00333     convertFromMatlab(*field, field_source);
00334   }
00335 }
00336 
00337 Array Conversion::convertToMatlab(const FieldPtr& field) {
00338   Array target = 0;
00339   TypePtr field_type = field->getType();
00340 
00341 //  ROSMATLAB_PRINTF("Constructing field %s (%s)...", field->getName(), field->getDataType());
00342   try {
00343     if (field_type->isString()) {
00344       if (field->isArray() || field->isVector()) {
00345         target = mxCreateCellMatrix(1, field->size());
00346         for(std::size_t i = 0; i < field->size(); i++) {
00347           mxSetCell(target, i, mxCreateString(field_type->as_string(field->get(i)).c_str()));
00348         }
00349       } else {
00350         target = mxCreateString(field_type->as_string(field->get()).c_str());
00351       }
00352       return target;
00353     }
00354 
00355 //    ROSMATLAB_PRINTF("Constructing double vector with dimension %u for field %s", unsigned(field->size()), field->getName());
00356     target = mxCreateDoubleMatrix(1, field->size(), mxREAL);
00357     double *x = mxGetPr(target);
00358 
00359     for(std::size_t i = 0; i < field->size(); i++) {
00360       x[i] = field_type->as_double(field->get(i));
00361     }
00362 
00363   } catch(boost::bad_any_cast &e) {
00364     ROSMATLAB_PRINTF("Catched bad_any_cast exception for field %s: %s", field->getName(), e.what());
00365     target = mxCreateEmpty();
00366   }
00367 
00368   return target;
00369 }
00370 
00371 void Conversion::convertFromMatlab(const FieldPtr &field, ConstArray source) {
00372   const char *field_name = field->getName();
00373   std::size_t field_size = mxIsChar(source) ? 1 : mxGetNumberOfElements(source);
00374 
00375   // check size
00376   if (field->isArray() && field->size() != field_size) throw Exception("Failed to parse field " + std::string(field_name) + ": Array field must have length " + boost::lexical_cast<std::string>(field->size()));
00377   if (!field->isContainer() && field_size != 1) throw Exception("Failed to parse field " + std::string(field_name) + ": Scalar field must have exactly length 1");
00378   if (field->isVector()) field->resize(field_size);
00379 
00380   // parse ROS message
00381   if (field->isMessage()) {
00382     MessagePtr field_message = messageByDataType(field->getValueType());
00383     if (!field_message) throw Exception("Failed to parse field " + std::string(field_name) + ": unknown datatype " + field->getDataType());
00384     Conversion child_conversion(field_message);
00385 
00386     // iterate over array
00387     for(std::size_t i = 0; i < field_size; i++) {
00388 //     ROSMATLAB_PRINTF("Expanding field %s[%u] (%s)... %u", field->getName(), i, field->getDataType());
00389       MessagePtr expanded = field->expand(i);
00390       if (expanded) {
00391         child_conversion.fromMatlab(expanded, source, i);
00392       } else {
00393         ROSMATLAB_PRINTF("Error during expansion of %s[%u] (%s)... %u", field->getName(), i, field->getDataType());
00394       }
00395     }
00396 
00397     return;
00398   }
00399 
00400   // parse string
00401   if (field->getType()->isString()) {
00402     std::vector<char> buffer;
00403 
00404     for(std::size_t i = 0; i < field->size(); i++) {
00405       if (mxIsCell(source) && mxIsChar(mxGetCell(source, i))) {
00406         buffer.resize(mxGetNumberOfElements(mxGetCell(source, i)) + 1);
00407         mxGetString(mxGetCell(source, i), buffer.data(), buffer.size());
00408         field->set(std::string(buffer.data(), buffer.size() - 1), i);
00409 
00410       } else if (mxIsChar(source) && i == 0) {
00411         buffer.resize(mxGetN(source) + 1);
00412         mxGetString(source, buffer.data(), buffer.size());
00413         field->set(std::string(buffer.data(), buffer.size() - 1));
00414 
00415       } else {
00416         throw Exception("Failed to parse string field " + std::string(field->getDataType()) + " " + std::string(field->getName()) + ": Array must be a cell string or a character array");
00417       }
00418     }
00419 
00420     return;
00421   }
00422 
00423   // For all other types source must be a double array...
00424   if (!mxIsDouble(source)) throw Exception("Failed to parse field " + std::string(field->getDataType()) + " " + std::string(field->getName()) + ": Array must be a double array");
00425   const double *x = mxGetPr(source);
00426   convertFromDouble(field, x, x + mxGetN(source));
00427 }
00428 
00429 const double *Conversion::convertFromDouble(const FieldPtr& field, const double *begin, const double *end)
00430 {
00431   const double *data = begin;
00432 
00433   if (field->isVector() && end >= begin) {
00434     // eat up all the input (roar!!!)
00435     field->resize(end - begin);
00436   }
00437 
00438   // check size
00439   if (end - begin < field->size()) {
00440     throw Exception("Failed to parse field " + std::string(field->getName()) + ": vector is too short");
00441   }
00442 
00443   // parse ROS message
00444   if (field->isMessage()) {
00445     // TODO
00446   }
00447 
00448   // ignore strings (set to empty string)
00449   if (field->getType()->isString()) {
00450     for(std::size_t i = 0; i < field->size(); i++) {
00451       field->set(std::string(), i);
00452       data++;
00453     }
00454     return data;
00455   }
00456 
00457   // read doubles
00458   for(std::size_t i = 0; i < field->size(); i++) {
00459     field->set(*data++, i);
00460   }
00461 
00462   return data;
00463 }
00464 
00465 const MessagePtr& Conversion::expanded() {
00466   if (!expanded_) {
00467     expanded_ = expand(message_);
00468 //    ROSMATLAB_PRINTF("Expanded an instance of %s to %u fields", message_->getDataType(), expanded_->size());
00469   }
00470   return expanded_;
00471 }
00472 
00473 ConversionOptions &Conversion::defaultOptions() {
00474   static boost::shared_ptr<ConversionOptions> default_options;
00475   if (!default_options) {
00476     default_options.reset(new ConversionOptions());
00477   }
00478   return *default_options;
00479 }
00480 
00481 std::map<const char *,ConversionOptions> Conversion::per_message_options_;
00482 ConversionOptions &Conversion::perMessageOptions(const MessagePtr &message) {
00483   return per_message_options_[message->getDataType()];
00484 }
00485 
00486 ConversionOptions::ConversionOptions()
00487 {
00488 }
00489 
00490 ConversionOptions::ConversionOptions(int nrhs, const mxArray *prhs[])
00491 {
00492   init(nrhs, prhs);
00493 }
00494 
00495 ConversionOptions::~ConversionOptions()
00496 {
00497 }
00498 
00499 void ConversionOptions::init(int nrhs, const mxArray *prhs[])
00500 {
00501   Options::init(nrhs, prhs, true);
00502 
00503   std::string type = getString("type");
00504   if (!type.empty()) {
00505     if (boost::algorithm::iequals(type, "struct"))
00506       setConversionType(MATLAB_STRUCT);
00507     else if (boost::algorithm::iequals(type, "matrix"))
00508       setConversionType(MATLAB_MATRIX);
00509     else if (boost::algorithm::iequals(type, "extended"))
00510       setConversionType(MATLAB_EXTENDED_STRUCT);
00511     else
00512       throw Exception("unknown conversion type '" + type + "'");
00513   }
00514 
00515   if (conversionType() >= MATLAB_TYPE_MAX) {
00516     throw Exception("illegal conversion type " + boost::lexical_cast<std::string>(conversionType()));
00517   }
00518 }
00519 
00520 ConversionOptions::MatlabType ConversionOptions::conversionType() const
00521 {
00522   return static_cast<ConversionOptions::MatlabType>(getInteger("type"));
00523 }
00524 
00525 std::string ConversionOptions::conversionTypeString() const
00526 {
00527   switch(conversionType()) {
00528     case MATLAB_STRUCT: return "struct";
00529     case MATLAB_MATRIX: return "matrix";
00530     case MATLAB_EXTENDED_STRUCT: return "extended";
00531   }
00532   return std::string();
00533 }
00534 
00535 ConversionOptions &ConversionOptions::setConversionType(ConversionOptions::MatlabType type)
00536 {
00537   set("type", static_cast<int>(type));
00538   return *this;
00539 }
00540 
00541 bool ConversionOptions::addMetaData() const
00542 {
00543   return getBool("meta");
00544 }
00545 
00546 ConversionOptions &ConversionOptions::setAddMetaData(bool value)
00547 {
00548   set("meta", value);
00549   return *this;
00550 }
00551 
00552 bool ConversionOptions::addConnectionHeader() const
00553 {
00554   return getBool("connectionheader");
00555 }
00556 
00557 ConversionOptions &ConversionOptions::setAddConnectionHeader(bool value)
00558 {
00559   set("meta", value);
00560   return *this;
00561 }
00562 
00563 mxArray *ConversionOptions::toMatlab() const {
00564   const char *fieldnames[] = { "Type", "Meta", "ConnectionHeader" };
00565   mxArray *result = mxCreateStructMatrix(1, 1, sizeof(fieldnames)/sizeof(*fieldnames), fieldnames);
00566   mxSetField(result, 0, "Type", mxCreateString(conversionTypeString().c_str()));
00567   mxSetField(result, 0, "Meta", mxCreateLogicalScalar(addMetaData()));
00568   mxSetField(result, 0, "ConnectionHeader", mxCreateLogicalScalar(addConnectionHeader()));
00569   return result;
00570 }
00571 
00572 }


rosmatlab
Author(s): Johannes Meyer
autogenerated on Fri Jul 25 2014 06:08:36