param.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/param.h>
00030 #include <rosmatlab/exception.h>
00031 
00032 #include <rosmatlab/options.h>
00033 #include <ros/param.h>
00034 
00035 #include <mex.h>
00036 
00037 namespace rosmatlab {
00038 namespace param {
00039 
00040 using namespace XmlRpc;
00041 
00042 mxArray *toMatlab(XmlRpcValue& value) {
00043   mxArray *array = 0;
00044 
00045   switch(value.getType()) {
00046     case XmlRpcValue::TypeBoolean:
00047       array = mxCreateLogicalScalar(static_cast<bool>(value));
00048       break;
00049 
00050     case XmlRpcValue::TypeInt:
00051       array = mxCreateDoubleScalar(static_cast<int>(value));
00052       break;
00053 
00054     case XmlRpcValue::TypeDouble:
00055       array = mxCreateDoubleScalar(static_cast<double>(value));
00056       break;
00057 
00058     case XmlRpcValue::TypeString:
00059       array = mxCreateString(static_cast<std::string>(value).c_str());
00060       break;
00061 
00062     case XmlRpcValue::TypeDateTime:
00063       // not supported
00064       break;
00065 
00066     case XmlRpcValue::TypeBase64:
00067       // not supported
00068       break;
00069 
00070     case XmlRpcValue::TypeArray:
00071       {
00072         mxArray *a = mxCreateCellMatrix(value.size(), 1);
00073         for(int i = 0; i < value.size(); ++i) {
00074           mxSetCell(a, i, toMatlab(value[i]));
00075         }
00076         array = a;
00077       }
00078       break;
00079 
00080     case XmlRpcValue::TypeStruct:
00081       {
00082         std::vector<const char *> fieldnames;
00083         XmlRpcValue::iterator it;
00084         for(it = value.begin(); it != value.end(); ++it) {
00085           fieldnames.push_back(it->first.c_str());
00086         }
00087         mxArray *s = mxCreateStructMatrix(1, 1, fieldnames.size(), fieldnames.data());
00088         for(it = value.begin(); it != value.end(); ++it) {
00089           mxSetField(s, 0, it->first.c_str(), toMatlab(it->second));
00090         }
00091         array = s;
00092       }
00093       break;
00094 
00095     default:
00096       break;
00097   }
00098 
00099   return array;
00100 }
00101 
00102 void fromMatlabBool(const mxArray *array, XmlRpcValue& value) {
00103   size_t n = mxGetNumberOfElements(array);
00104   mxLogical *data = mxGetLogicals(array);
00105 
00106   if (n == 1) {
00107     value = XmlRpcValue(data[0]);
00108   } else {
00109     value.setSize(n);
00110     for(int i = 0; i < n; ++i) value[i] = data[i];
00111   }
00112 }
00113 
00114 void fromMatlabString(const mxArray *array, XmlRpcValue& value) {
00115   size_t n = mxGetNumberOfElements(array);
00116   std::vector<char> buffer(n + 1);
00117   if (mxGetString(array, buffer.data(), buffer.size()) == 0) {
00118     value = buffer.data();
00119   }
00120 }
00121 
00122 template <typename From, typename To>
00123 void fromMatlabNumeric(const mxArray *array, XmlRpcValue& value) {
00124   size_t n = mxGetNumberOfElements(array);
00125   const From *data = static_cast<const From *>(mxGetData(array));
00126 
00127   if (n == 1) {
00128     value = static_cast<To>(*data);
00129   } else {
00130     value.setSize(n);
00131     for(int i = 0; i < n; ++i) value[i] = static_cast<To>(data[i]);
00132   }
00133 }
00134 
00135 bool fromMatlab(const mxArray *array, XmlRpcValue& value) {
00136   // invalidate value
00137   value.clear();
00138 
00139   switch(mxGetClassID(array)) {
00140     case mxCELL_CLASS:
00141       // not supported
00142       break;
00143 
00144     case mxSTRUCT_CLASS:
00145       // not supported
00146       break;
00147 
00148     case mxLOGICAL_CLASS:
00149       fromMatlabBool(array, value);
00150       break;
00151 
00152     case mxCHAR_CLASS:
00153       fromMatlabString(array, value);
00154       break;
00155 
00156     case mxDOUBLE_CLASS:
00157       fromMatlabNumeric<double, double>(array, value);
00158       break;
00159 
00160     case mxSINGLE_CLASS:
00161       fromMatlabNumeric<float, double>(array, value);
00162       break;
00163 
00164     case mxINT8_CLASS:
00165       fromMatlabNumeric<int8_t, int>(array, value);
00166       break;
00167 
00168     case mxUINT8_CLASS:
00169       fromMatlabNumeric<uint8_t, int>(array, value);
00170       break;
00171 
00172     case mxINT16_CLASS:
00173       fromMatlabNumeric<int16_t, int>(array, value);
00174       break;
00175 
00176     case mxUINT16_CLASS:
00177       fromMatlabNumeric<uint16_t, int>(array, value);
00178       break;
00179 
00180     case mxINT32_CLASS:
00181       fromMatlabNumeric<int32_t, int>(array, value);
00182       break;
00183 
00184     case mxUINT32_CLASS:
00185       fromMatlabNumeric<uint32_t, int>(array, value);
00186       break;
00187 
00188     case mxINT64_CLASS:
00189       fromMatlabNumeric<int64_t, int>(array, value);
00190       break;
00191 
00192     case mxUINT64_CLASS:
00193       fromMatlabNumeric<uint64_t, int>(array, value);
00194       break;
00195 
00196     case mxFUNCTION_CLASS:
00197       // not supported
00198       break;
00199 
00200     default:
00201       break;
00202   }
00203 
00204   return value.valid();
00205 }
00206 
00207 void get(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
00208 {
00209   std::string key;
00210   XmlRpcValue value;
00211 
00212   if (nrhs > 0) key = Options::getString(prhs[0]);
00213 
00214   // get parameter
00215   try {
00216     if (!ros::param::get(key, value)) {
00217       plhs[0] = mxCreateDoubleMatrix(0, 0, mxREAL);
00218       return;
00219     }
00220   } catch (ros::InvalidNameException& e) {
00221     throw Exception("ros.param.get", e.what());
00222   }
00223 
00224   // convert parameter value to Matlab
00225   plhs[0] = toMatlab(value);
00226   if (!plhs[0]) {
00227     throw Exception("ros.param.get", "Could not convert the parameter value of " + key + " to Matlab (unknown type)");
00228     plhs[0] = mxCreateDoubleMatrix(0, 0, mxREAL);
00229     return;
00230   }
00231 
00232   return;
00233 }
00234 
00235 void set(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
00236 {
00237   if (nrhs < 2) {
00238     throw ArgumentException("ros.param.set", 2);
00239   }
00240 
00241   std::string key = Options::getString(prhs[0]);
00242   XmlRpcValue value;
00243 
00244   // convert parameter value from Matlab
00245   if (!fromMatlab(prhs[1], value)) {
00246     throw Exception("ros.param.set", "Could not convert the parameter value for " + key + " to a XmlRpcValue");
00247     return;
00248   }
00249 
00250   // set parameter
00251   try {
00252     ros::param::set(key, value);
00253   } catch (ros::InvalidNameException& e) {
00254     throw Exception("ros.param.set", e.what());
00255   }
00256 }
00257 
00258 void del(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
00259 {
00260   if (nrhs < 1) {
00261     throw ArgumentException("ros.param.delete", 1);
00262   }
00263 
00264   std::string key = Options::getString(prhs[0]);
00265   bool result = ros::param::del(key);
00266   if (nlhs > 0) plhs[0] = mxCreateLogicalScalar(result);
00267 }
00268 
00269 void has(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
00270 {
00271   if (nrhs < 1) {
00272     throw ArgumentException("ros.param.has", 1);
00273   }
00274 
00275   std::string key = Options::getString(prhs[0]);
00276   plhs[0] = mxCreateLogicalScalar(ros::param::has(key));
00277 }
00278 
00279 } // namespace param
00280 } // namespace rosmatlab


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