rtt_rosparam_service.cpp
Go to the documentation of this file.
00001 #include <rtt/RTT.hpp>
00002 #include <rtt/Property.hpp>
00003 #include <rtt/plugin/ServicePlugin.hpp>
00004 #include <rtt/types/PropertyDecomposition.hpp>
00005 
00006 #include <boost/utility/enable_if.hpp>
00007 #include <boost/type_traits/is_convertible.hpp>
00008 
00009 #include <XmlRpcException.h>
00010 
00011 #include <Eigen/Dense>
00012 
00013 #include <ros/ros.h>
00014 
00015 using namespace RTT;
00016 using namespace std;
00017 
00018 class ROSParamService: public RTT::Service
00019 {
00020 public:
00021 
00022   typedef enum  {
00023     RELATIVE, 
00024     ABSOLUTE, 
00025     PRIVATE,  
00026     COMPONENT 
00027   }ResolutionPolicy;
00028 
00029   ROSParamService(TaskContext* owner) :
00030     Service("rosparam", owner)
00031   {
00032     this->doc("RTT Service for synchronizing ROS parameters with the properties of a corresponding RTT component");
00033 
00034     this->addConstant("RELATIVE",static_cast<int>(RELATIVE));
00035     this->addConstant("ABSOLUTE",static_cast<int>(ABSOLUTE));
00036     this->addConstant("PRIVATE",static_cast<int>(PRIVATE));
00037     this->addConstant("COMPONENT",static_cast<int>(COMPONENT));
00038 
00039     this->addOperation("getAllRelative", &ROSParamService::getParamsRelative, this)
00040       .doc("Gets all properties of this component (and its sub-services) from the ROS param server in the relative namespace.");
00041     this->addOperation("getAllAbsolute", &ROSParamService::getParamsAbsolute, this)
00042       .doc("Gets all properties of this component (and its sub-services) from the ROS param server in the absolute namespace.");
00043     this->addOperation("getAllPrivate", &ROSParamService::getParamsPrivate, this)
00044       .doc("Gets all properties of this component (and its sub-services) from the ROS param server in the node's private namespace.");
00045     this->addOperation("getAllComponentPrivate", &ROSParamService::getParamsComponentPrivate, this)
00046       .doc("Gets all properties of this component (and its sub-services) from the ROS param server in the component's private namespace.");
00047     this->addOperation("getAll", &ROSParamService::getParamsComponentPrivate, this)
00048       .doc("Gets all properties of this component (and its sub-services) from the ROS param server in the component's private namespace. This is an alias for getAllComponentPrivate().");
00049 
00050     this->addOperation("setAllRelative", &ROSParamService::setParamsRelative, this)
00051       .doc("Stores all properties of this component (and its sub-services) on the ROS param server from the similarly-named property in the relative namespace.");
00052     this->addOperation("setAllAbsolute", &ROSParamService::setParamsAbsolute, this)
00053       .doc("Stores all properties of this component (and its sub-services) on the ROS param server from the similarly-named property in the absolute namespace.");
00054     this->addOperation("setAllPrivate", &ROSParamService::setParamsPrivate, this)
00055       .doc("Stores all properties of this component (and its sub-services) on the ROS param server from the similarly-named property in the node's private namespace.");
00056     this->addOperation("setAllComponentPrivate", &ROSParamService::setParamsComponentPrivate, this)
00057       .doc("Stores all properties of this component (and its sub-services) on the ROS param server from the similarly-named property in the component's private namespace.");
00058     this->addOperation("setAll", &ROSParamService::setParamsComponentPrivate, this)
00059       .doc("Stores all properties of this component (and its sub-services) on the ROS param server from the similarly-named property in the component's private namespace. This is an alias for setAllComponentPrivate().");
00060 
00061     this->addOperation("get", &ROSParamService::get, this) 
00062       .doc("Gets one property of this component (or populates the properties of a named RTT sub-service) from the ROS param server based on the given resolution policy.")
00063       .arg("name", "Name of the property / service / parameter.")
00064       .arg("policy", "ROS parameter namespace resolution policy.");
00065     this->addOperation("getParam", &ROSParamService::getParam, this) 
00066       .doc("Gets one property of this component (or populates the properties of a named RTT sub-service) from the ROS param server based on the given ROS parameter name.")
00067       .arg("param_name", "Name of the ROS parameter. Use '~' and '/' leaders for private or absolute resolution.")
00068       .arg("name", "Name of the RTT property or service.");
00069 
00070     this->addOperation("getRelative", &ROSParamService::getParamRelative, this) 
00071       .doc("Gets one property of this component (or populates the properties of a named RTT sub-service) from the ROS param server in the relative namespace.")
00072       .arg("name", "Name of the property / service / parameter.");
00073     this->addOperation("getAbsolute", &ROSParamService::getParamAbsolute, this) 
00074       .doc("Gets one property of this component (or populates the properties of a named RTT sub-service) from the ROS param server in the absolute namespace.")
00075       .arg("name", "Name of the property / service / parameter.");
00076     this->addOperation("getPrivate", &ROSParamService::getParamPrivate, this) 
00077       .doc("Gets one property of this component (or populates the properties of a named RTT sub-service) from the ROS param server in the node's private namespace.")
00078       .arg("name", "Name of the property / service / parameter.");
00079     this->addOperation("getComponentPrivate", &ROSParamService::getParamComponentPrivate, this) 
00080       .doc("Gets one property of this component (or populates the properties of a named RTT sub-service) from the ROS param server in the component's private namespace.")
00081       .arg("name", "Name of the property / service / parameter.");
00082 
00083     this->addOperation("set", &ROSParamService::set, this) 
00084       .doc("Sets one parameter on the ROS param server from the similarly-named property of this component (or stores the properties of a named RTT sub-service) in the ROS parameter namespace based on the given resolution policy.")
00085       .arg("name", "Name of the property / service / parameter.")
00086       .arg("policy", "ROS parameter namespace resolution policy.");
00087     this->addOperation("setParam", &ROSParamService::setParam, this) 
00088       .doc("Sets one parameter on the ROS param server from the similarly-named property of this component (or stores the properties of a named RTT sub-service) in the ROS parameter namespace based on the given ROS parameter name.")
00089       .arg("param_name", "Name of the ROS parameter. Use '~' and '/' leaders for private or absolute resolution.")
00090       .arg("name", "Name of the RTT property or service.");
00091 
00092     this->addOperation("setRelative", &ROSParamService::setParamRelative, this) 
00093       .doc("Sets one parameter on the ROS param server from the similarly-named property of this component (or stores the properties of a named RTT sub-service) in the relative namespace.")
00094       .arg("name", "Name of the property / service / parameter.");
00095     this->addOperation("setAbsolute", &ROSParamService::setParamAbsolute, this) 
00096       .doc("Sets one parameter on the ROS param server from the similarly-named property of this component (or stores the properties of a named RTT sub-service) in the absolute namespace.")
00097       .arg("name", "Name of the property / service / parameter.");
00098     this->addOperation("setPrivate", &ROSParamService::setParamPrivate, this) 
00099       .doc("Sets one parameter on the ROS param server from the similarly-named property of this component (or stores the properties of a named RTT sub-service) in the node's private namespace.")
00100       .arg("name", "Name of the property / service / parameter.");
00101     this->addOperation("setComponentPrivate", &ROSParamService::setParamComponentPrivate, this) 
00102       .doc("Sets one parameter on the ROS param server from the similarly-named property of this component (or stores the properties of a named RTT sub-service) in the component's private namespace.")
00103       .arg("name", "Name of the property / service / parameter.");
00104   
00105   }
00106 private:
00107 
00109   const std::string resolvedName(
00110     const std::string &param_name, 
00111     const ROSParamService::ResolutionPolicy policy);
00112 
00113   bool getParams(RTT::Service::shared_ptr service, const std::string& ns);
00114   bool getParams(const ROSParamService::ResolutionPolicy policy);
00115   bool getParamsRelative() { return getParams(RELATIVE); }
00116   bool getParamsAbsolute() { return getParams(ABSOLUTE); }
00117   bool getParamsPrivate() { return getParams(PRIVATE); }
00118   bool getParamsComponentPrivate() { return getParams(COMPONENT); }
00119 
00120   bool get(
00121     const std::string &param_name, 
00122     const unsigned int policy = (unsigned int)ROSParamService::COMPONENT);
00123   bool getParam(
00124     const std::string &ros_name, 
00125     const std::string &rtt_name);
00126   bool getParamRelative(const std::string &name) { return get(name, RELATIVE); }
00127   bool getParamAbsolute(const std::string &name) { return get(name, ABSOLUTE); }
00128   bool getParamPrivate(const std::string &name) { return get(name, PRIVATE); }
00129   bool getParamComponentPrivate(const std::string &name) { return get(name, COMPONENT); }
00130 
00131   bool setParams(RTT::Service::shared_ptr service, const std::string& ns);
00132   bool setParams(const ROSParamService::ResolutionPolicy policy);
00133   bool setParamsRelative() { return setParams(RELATIVE); }
00134   bool setParamsAbsolute() { return setParams(ABSOLUTE); }
00135   bool setParamsPrivate() { return setParams(PRIVATE); }
00136   bool setParamsComponentPrivate() { return setParams(COMPONENT); }
00137 
00138   bool set(
00139     const std::string &param_name, 
00140     const unsigned int policy = (unsigned int)ROSParamService::COMPONENT);
00141   bool setParam(
00142     const std::string &ros_name, 
00143     const std::string &rtt_name);
00144   bool setParamRelative(const std::string &name) { return set(name, RELATIVE); }
00145   bool setParamAbsolute(const std::string &name) { return set(name, ABSOLUTE); }
00146   bool setParamPrivate(const std::string &name) { return set(name, PRIVATE); }
00147   bool setParamComponentPrivate(const std::string &name) { return set(name, COMPONENT); }
00148 };
00149 
00150 const std::string ROSParamService::resolvedName(
00151     const std::string &param_name, 
00152     const ROSParamService::ResolutionPolicy policy)
00153 {
00154   std::string leader = "";
00155   std::string resolved_name = "";
00156 
00157   if(param_name.length() > 0) {
00158     leader = param_name[0];
00159   }
00160 
00161   switch(policy) {
00162     case ROSParamService::RELATIVE:
00163       resolved_name = param_name;
00164       break;
00165     case ROSParamService::ABSOLUTE:
00166       resolved_name = (leader == "/") ? param_name : std::string("/") + param_name;
00167       break;
00168     case ROSParamService::PRIVATE:
00169       resolved_name = (leader == "~") ? param_name : std::string("~") + param_name;
00170       break;
00171     case ROSParamService::COMPONENT:
00172       resolved_name = std::string("~") + ros::names::append(this->getOwner()->getName(),param_name);
00173       break;
00174   };
00175 
00176   RTT::log(RTT::Debug) << "["<<this->getOwner()->getName()<<"] Resolving ROS param \""<<param_name<<"\" to \""<<resolved_name<<"\"" << RTT::endlog();
00177 
00178   return resolved_name;
00179 }
00180 
00181 
00182 
00184 template<class T>
00185 bool castable(const RTT::base::PropertyBase *prop);
00187 template<class T>
00188 XmlRpc::XmlRpcValue rttPropertyToXmlParam(const T &prop);
00190 template<>
00191 XmlRpc::XmlRpcValue rttPropertyToXmlParam<RTT::PropertyBag>(const RTT::PropertyBag &bag);
00193 template<class T>
00194 XmlRpc::XmlRpcValue rttPropertyToXmlParam(const std::vector<T> &vec);
00196 XmlRpc::XmlRpcValue rttPropertyBaseToXmlParam(RTT::base::PropertyBase *prop);
00197 
00198 template<class T>
00199 bool castable(const RTT::base::PropertyBase *prop) 
00200 {
00201   return dynamic_cast<const Property<T>*>(prop);
00202 }
00203 
00204 template<class T>
00205 XmlRpc::XmlRpcValue rttPropertyToXmlParam(const T &prop)
00206 {
00207   return XmlRpc::XmlRpcValue(prop);
00208 }
00209 
00210 template<>
00211 XmlRpc::XmlRpcValue rttPropertyToXmlParam<float>(const float &prop)
00212 {
00213   return XmlRpc::XmlRpcValue(static_cast<double>(prop));
00214 }
00215 
00216 template<>
00217 XmlRpc::XmlRpcValue rttPropertyToXmlParam<unsigned int>(const unsigned int &prop)
00218 {
00219   return XmlRpc::XmlRpcValue(static_cast<int>(prop));
00220 }
00221 
00222 template<>
00223 XmlRpc::XmlRpcValue rttPropertyToXmlParam<char>(const char &prop)
00224 {
00225   return XmlRpc::XmlRpcValue(static_cast<int>(prop));
00226 }
00227 
00228 template<>
00229 XmlRpc::XmlRpcValue rttPropertyToXmlParam<unsigned char>(const unsigned char &prop)
00230 {
00231   return XmlRpc::XmlRpcValue(static_cast<int>(prop));
00232 }
00233 
00234 template<>
00235 XmlRpc::XmlRpcValue rttPropertyToXmlParam<RTT::PropertyBag>(const RTT::PropertyBag &bag)
00236 {
00237   // Make the xml value a struct
00238   XmlRpc::XmlRpcValue xml_struct;
00239   const XmlRpc::XmlRpcValue::ValueStruct &xml_map = (const XmlRpc::XmlRpcValue::ValueStruct &)(xml_struct);
00240 
00241   // Get the properties
00242   const RTT::PropertyBag::Properties &properties = bag.getProperties();
00243 
00244   for(RTT::PropertyBag::Properties::const_iterator it = properties.begin();
00245       it != properties.end();
00246       ++it)
00247   {
00248     xml_struct[(*it)->getName()] = rttPropertyBaseToXmlParam(*it);
00249   }
00250 
00251   return xml_struct;
00252 }
00253 
00254 template<class T>
00255 XmlRpc::XmlRpcValue rttPropertyToXmlParam(const std::vector<T> &vec)
00256 {
00257   XmlRpc::XmlRpcValue xml_array;
00258   xml_array.setSize(vec.size());
00259 
00260   for(unsigned i=0; i<vec.size(); i++) {
00261     xml_array[i] = rttPropertyToXmlParam<T>(vec.at(i));
00262   }
00263 
00264   return xml_array;
00265 }
00266 
00267 template<>
00268 XmlRpc::XmlRpcValue rttPropertyToXmlParam(const Eigen::VectorXd &vec)
00269 {
00270   XmlRpc::XmlRpcValue xml_array;
00271   xml_array.setSize(vec.size());
00272 
00273   for(unsigned i=0; i<vec.size(); i++) {
00274     xml_array[i] = rttPropertyToXmlParam<double>(vec(i));
00275   }
00276 
00277   return xml_array;
00278 }
00279 
00280 template<>
00281 XmlRpc::XmlRpcValue rttPropertyToXmlParam(const Eigen::VectorXf &vec)
00282 {
00283   XmlRpc::XmlRpcValue xml_array;
00284   xml_array.setSize(vec.size());
00285 
00286   for(unsigned i=0; i<vec.size(); i++) {
00287     xml_array[i] = rttPropertyToXmlParam<double>(vec(i));
00288   }
00289 
00290   return xml_array;
00291 }
00292 
00293 
00294 // These just save typing
00295 #define RETURN_RTT_PROPERTY_TO_XML_PARAM(type,prop)\
00296   if(castable< type >(prop)) { return rttPropertyToXmlParam< type >(static_cast<const RTT::Property< type >*>(prop)->rvalue()); }
00297 
00298 #define RETURN_RTT_PROPERTY_CONTAINER_TO_XML_PARAM(type,elem_type,prop)\
00299   if(castable< type >(prop)) { return rttPropertyToXmlParam< elem_type >(static_cast<const RTT::Property< type >*>(prop)->rvalue()); }
00300 
00301 XmlRpc::XmlRpcValue rttPropertyBaseToXmlParam(RTT::base::PropertyBase *prop)
00302 {
00303   // Primitive parameters
00304   RETURN_RTT_PROPERTY_TO_XML_PARAM(std::string,prop);
00305   RETURN_RTT_PROPERTY_TO_XML_PARAM(double,prop);
00306   RETURN_RTT_PROPERTY_TO_XML_PARAM(float,prop);
00307   RETURN_RTT_PROPERTY_TO_XML_PARAM(int,prop);
00308   RETURN_RTT_PROPERTY_TO_XML_PARAM(unsigned int,prop);
00309   RETURN_RTT_PROPERTY_TO_XML_PARAM(char,prop);
00310   RETURN_RTT_PROPERTY_TO_XML_PARAM(unsigned char,prop);
00311   RETURN_RTT_PROPERTY_TO_XML_PARAM(bool,prop);
00312 
00313   // Vector parameters
00314   RETURN_RTT_PROPERTY_CONTAINER_TO_XML_PARAM(std::vector<std::string>, std::string, prop);
00315   RETURN_RTT_PROPERTY_CONTAINER_TO_XML_PARAM(std::vector<double>, double, prop);
00316   RETURN_RTT_PROPERTY_CONTAINER_TO_XML_PARAM(std::vector<float>, float, prop);
00317   RETURN_RTT_PROPERTY_CONTAINER_TO_XML_PARAM(std::vector<int>, int, prop);
00318   RETURN_RTT_PROPERTY_CONTAINER_TO_XML_PARAM(std::vector<unsigned int>, unsigned int, prop);
00319   RETURN_RTT_PROPERTY_CONTAINER_TO_XML_PARAM(std::vector<char>, char, prop);
00320   RETURN_RTT_PROPERTY_CONTAINER_TO_XML_PARAM(std::vector<unsigned char>, unsigned char, prop);
00321   RETURN_RTT_PROPERTY_CONTAINER_TO_XML_PARAM(std::vector<bool>, bool, prop);
00322 
00323   RETURN_RTT_PROPERTY_TO_XML_PARAM(Eigen::VectorXd, prop);
00324   RETURN_RTT_PROPERTY_TO_XML_PARAM(Eigen::VectorXf, prop);
00325 
00326   // Struct parameters
00327   RETURN_RTT_PROPERTY_TO_XML_PARAM(RTT::PropertyBag,prop);
00328 
00329   // Try to decompose property into a property bag
00330   RTT::PropertyBag bag;
00331   if (RTT::types::propertyDecomposition(prop, bag)) {
00332     return rttPropertyToXmlParam(bag);
00333   }
00334 
00335   return XmlRpc::XmlRpcValue();
00336 }
00337 
00338 
00339 bool ROSParamService::set(
00340     const std::string &param_name, 
00341     const unsigned int policy)
00342 {
00343   RTT::Logger::In in("ROSParamService::set");
00344 
00345   const std::string resolved_name = resolvedName(param_name,ResolutionPolicy(policy));
00346 
00347   return this->setParam(resolved_name, param_name);
00348 }
00349 
00350 bool ROSParamService::setParam(
00351     const std::string &ros_name,
00352     const std::string &rtt_name)
00353 {
00354   RTT::Logger::In in("ROSParamService::setParam");
00355 
00356   XmlRpc::XmlRpcValue xml_value;
00357 
00358   // Try to find a property named rtt_name
00359   RTT::base::PropertyBase *property = this->getOwner()->getProperty(rtt_name);
00360   if (property) {
00361     xml_value = rttPropertyBaseToXmlParam(this->getOwner()->getProperty(rtt_name));
00362     ros::param::set(ros_name, xml_value);
00363     return true;
00364   }
00365 
00366   // Try to find a sub-service named rtt_name 
00367   RTT::Service::shared_ptr service = this->getOwner()->provides()->getService(rtt_name);
00368   if (service) {
00369     // Set all parameters of the sub-service
00370     return setParams(service, ros_name);
00371   }
00372 
00373   RTT::log(RTT::Debug) << "RTT component does not have a property or service named \"" << rtt_name << "\"" << RTT::endlog();
00374   return false;
00375 }
00376 
00377 bool ROSParamService::setParams(const ROSParamService::ResolutionPolicy policy)
00378 {
00379   return setParams(this->getOwner()->provides(), resolvedName(std::string(), policy));
00380 }
00381 
00382 bool ROSParamService::setParams(
00383     RTT::Service::shared_ptr service, 
00384     const std::string& ns) {
00385   XmlRpc::XmlRpcValue xml_value;
00386   xml_value = rttPropertyToXmlParam(*(service->properties()));
00387   ros::param::set(ns, xml_value);
00388 
00389   // Recurse into sub-services
00390   RTT::Service::ProviderNames names = service->getProviderNames();
00391   for (RTT::Service::ProviderNames::const_iterator it = names.begin(); it != names.end(); ++it)
00392   {
00393     RTT::Service::shared_ptr sub(service->getService(*it));
00394     if (sub) {
00395       if (!setParams(sub, ros::names::append(ns,sub->getName()))) return false;
00396     }
00397   }
00398 
00399   return true;
00400 }
00401 
00402 // Declarations
00404 template <class T>
00405 bool xmlParamToProp(const XmlRpc::XmlRpcValue &xml_value, RTT::Property<T>* prop);
00407 template <class T>
00408 bool xmlParamToProp(const XmlRpc::XmlRpcValue &xml_value, RTT::Property<std::vector<T> >* prop);
00410 template <>
00411 bool xmlParamToProp(const XmlRpc::XmlRpcValue &xml_value, RTT::Property<Eigen::VectorXd>* prop);
00412 template <>
00413 bool xmlParamToProp(const XmlRpc::XmlRpcValue &xml_value, RTT::Property<Eigen::VectorXf>* prop);
00415 template <> 
00416 bool xmlParamToProp<RTT::PropertyBag>(const XmlRpc::XmlRpcValue &xml_value, RTT::Property<RTT::PropertyBag>* prop);
00418 bool xmlParamToProp( const XmlRpc::XmlRpcValue &xml_value, RTT::base::PropertyBase* prop_base);
00419 
00420 template <class XMLRPCType,class PropertyType, class enabled=void> struct XmlParamToValue{
00421     static bool assign(const XMLRPCType& xml_value, PropertyType &prop_value)
00422     {
00423         return false;
00424     }
00425 };
00426 
00427 template <class XMLRPCType, class PropertyType >
00428 struct XmlParamToValue <XMLRPCType, PropertyType, typename boost::enable_if<boost::is_convertible<XMLRPCType,PropertyType> >::type >{
00429     static bool assign(const XMLRPCType& xml_value, PropertyType &prop_value)
00430     {
00431         prop_value = xml_value;
00432         return true;
00433     }
00434 };
00435 
00437 template <class PropertyType> bool xmlParamToValue(const XmlRpc::XmlRpcValue &xml_value, PropertyType &value) {
00438     switch(xml_value.getType()) {
00439     case XmlRpc::XmlRpcValue::TypeString:
00440         return XmlParamToValue<std::string, PropertyType>::assign(static_cast<const std::string&>(const_cast<XmlRpc::XmlRpcValue &>(xml_value)),value);
00441     case XmlRpc::XmlRpcValue::TypeDouble:
00442         return XmlParamToValue<double, PropertyType>::assign(static_cast<double>(const_cast<XmlRpc::XmlRpcValue &>(xml_value)),value);
00443     case XmlRpc::XmlRpcValue::TypeInt:
00444         return XmlParamToValue<int, PropertyType>::assign(static_cast<int>(const_cast<XmlRpc::XmlRpcValue &>(xml_value)),value);
00445     case XmlRpc::XmlRpcValue::TypeBoolean:
00446         return XmlParamToValue<bool, PropertyType>::assign(static_cast<bool>(const_cast<XmlRpc::XmlRpcValue &>(xml_value)),value);
00447     }
00448     return false;
00449 }
00450 
00451 template <class T>
00452 bool xmlParamToProp(
00453     const XmlRpc::XmlRpcValue &xml_value,
00454     RTT::Property<T>* prop)
00455 {
00456   // Check if the property value is the requested type T
00457   if(!prop) {
00458     return false;
00459   }
00460 
00461   // Set the value
00462   return xmlParamToValue(xml_value, prop->set());
00463 
00464 }
00465 
00466 template <class T>
00467 bool xmlParamToProp(
00468     const XmlRpc::XmlRpcValue &xml_value,
00469     RTT::Property<std::vector<T> >* prop)
00470 {
00471   // Check if the property value is the requested type T
00472   if(!prop) {
00473     return false;
00474   }
00475 
00476   // Make sure it's an array 
00477   if(xml_value.getType() != XmlRpc::XmlRpcValue::TypeArray) {
00478     return false;
00479   }
00480 
00481   // Copy the data into the vector property
00482   std::vector<T> &vec = prop->value();
00483   vec.resize(xml_value.size());
00484   bool result = true;
00485   for(size_t i=0; i<vec.size(); i++) {
00486     result &= xmlParamToValue(xml_value[i], vec[i]);
00487   }
00488 
00489   return result;
00490 }
00491 
00492 template <>
00493 bool xmlParamToProp<bool>(
00494     const XmlRpc::XmlRpcValue &xml_value,
00495     RTT::Property<std::vector<bool> >* prop)
00496 {
00497   // Check if the property value is the requested type T
00498   if(!prop) {
00499     return false;
00500   }
00501 
00502   // Make sure it's an array
00503   if(xml_value.getType() != XmlRpc::XmlRpcValue::TypeArray) {
00504     return false;
00505   }
00506 
00507   // Copy the data into the vector property
00508   std::vector<bool> &vec = prop->value();
00509   vec.resize(xml_value.size());
00510   bool result = true;
00511   for(size_t i=0; i<vec.size(); i++) {
00512     bool temp;
00513     result &= xmlParamToValue(xml_value[i], temp);
00514     vec[i] = temp;
00515   }
00516 
00517   return result;
00518 }
00519 
00520 template <>
00521 bool xmlParamToProp(
00522     const XmlRpc::XmlRpcValue &xml_value,
00523     RTT::Property<Eigen::VectorXd >* prop)
00524 {
00525   // Check if the property value is the requested type T
00526   if(!prop) {
00527     return false;
00528   }
00529 
00530   // Make sure it's an array
00531   if(xml_value.getType() != XmlRpc::XmlRpcValue::TypeArray) {
00532     return false;
00533   }
00534 
00535   // Copy the data into the vector property
00536   Eigen::VectorXd &vec = prop->value();
00537   vec.resize(xml_value.size());
00538   bool result = true;
00539   for(size_t i=0; i<vec.size(); i++) {
00540     double temp;
00541     result &= xmlParamToValue(xml_value[i], temp);
00542     vec[i] = temp;
00543   }
00544 
00545   return result;
00546 }
00547 
00548 template <>
00549 bool xmlParamToProp(
00550     const XmlRpc::XmlRpcValue &xml_value,
00551     RTT::Property<Eigen::VectorXf >* prop)
00552 {
00553   // Check if the property value is the requested type T
00554   if(!prop) {
00555     return false;
00556   }
00557 
00558   // Make sure it's an array
00559   if(xml_value.getType() != XmlRpc::XmlRpcValue::TypeArray) {
00560     return false;
00561   }
00562 
00563   // Copy the data into the vector property
00564   Eigen::VectorXf &vec = prop->value();
00565   vec.resize(xml_value.size());
00566   bool result = true;
00567   for(size_t i=0; i<vec.size(); i++) {
00568     double temp;
00569     result &= xmlParamToValue(xml_value[i], temp);
00570     vec[i] = temp;
00571   }
00572 
00573   return result;
00574 }
00575 
00576 template <>
00577 bool xmlParamToProp<RTT::PropertyBag>(
00578     const XmlRpc::XmlRpcValue &xml_value,
00579     RTT::Property<RTT::PropertyBag>* prop)
00580 {
00581   // Check if the property value is the requested type T
00582   if(!prop) {
00583     return false;
00584   }
00585 
00586   // Make sure it's a struct
00587   if(xml_value.getType() != XmlRpc::XmlRpcValue::TypeStruct) {
00588     return false;
00589   }
00590 
00591   // Copy the properties
00592   bool success = true;
00593   // We need to copy the struct because XmlRpc++ doesn't have const operations for this
00594   XmlRpc::XmlRpcValue xml_value_struct(xml_value);
00595   for(XmlRpc::XmlRpcValue::ValueStruct::iterator it = xml_value_struct.begin();
00596       it != xml_value_struct.end();
00597       ++it)
00598   {
00599     RTT::base::PropertyBase *sub_prop_base = prop->value().getProperty(it->first);
00600     if(sub_prop_base) {
00601       success &= xmlParamToProp(it->second, sub_prop_base);
00602     }
00603   }
00604 
00605   return success;
00606 }
00607 
00608 bool xmlParamToProp(
00609     const XmlRpc::XmlRpcValue &xml_value,
00610     RTT::base::PropertyBase* prop_base)
00611 {
00612   bool array_ret = false;
00613 
00614   // Switch based on the type of XmlRpcValue 
00615   switch(xml_value.getType()) {
00616     case XmlRpc::XmlRpcValue::TypeString:
00617       return 
00618         xmlParamToProp(xml_value, dynamic_cast<RTT::Property<std::string>*>(prop_base)); 
00619     case XmlRpc::XmlRpcValue::TypeDouble:
00620       return 
00621         xmlParamToProp(xml_value, dynamic_cast<RTT::Property<double>*>(prop_base)) ||
00622         xmlParamToProp(xml_value, dynamic_cast<RTT::Property<float>*>(prop_base));
00623     case XmlRpc::XmlRpcValue::TypeInt:
00624       return 
00625         xmlParamToProp(xml_value, dynamic_cast<RTT::Property<double>*>(prop_base)) ||
00626         xmlParamToProp(xml_value, dynamic_cast<RTT::Property<float>*>(prop_base)) ||
00627         xmlParamToProp(xml_value, dynamic_cast<RTT::Property<int>*>(prop_base)) ||
00628         xmlParamToProp(xml_value, dynamic_cast<RTT::Property<unsigned int>*>(prop_base)) ||
00629         xmlParamToProp(xml_value, dynamic_cast<RTT::Property<char>*>(prop_base)) ||
00630         xmlParamToProp(xml_value, dynamic_cast<RTT::Property<unsigned char>*>(prop_base));
00631     case XmlRpc::XmlRpcValue::TypeBoolean:
00632       return 
00633         xmlParamToProp(xml_value, dynamic_cast<RTT::Property<bool>*>(prop_base)); 
00634     case XmlRpc::XmlRpcValue::TypeArray:
00635       array_ret =
00636         xmlParamToProp(xml_value, dynamic_cast<RTT::Property<std::vector<std::string> >*>(prop_base)) ||
00637         xmlParamToProp(xml_value, dynamic_cast<RTT::Property<std::vector<double> >*>(prop_base)) ||
00638         xmlParamToProp(xml_value, dynamic_cast<RTT::Property<std::vector<float> >*>(prop_base)) ||
00639         xmlParamToProp(xml_value, dynamic_cast<RTT::Property<std::vector<int> >*>(prop_base)) ||
00640         xmlParamToProp(xml_value, dynamic_cast<RTT::Property<std::vector<unsigned int> >*>(prop_base)) ||
00641         xmlParamToProp(xml_value, dynamic_cast<RTT::Property<std::vector<char> >*>(prop_base)) ||
00642         xmlParamToProp(xml_value, dynamic_cast<RTT::Property<std::vector<unsigned char> >*>(prop_base)) ||
00643         xmlParamToProp(xml_value, dynamic_cast<RTT::Property<std::vector<bool> >*>(prop_base)) ||
00644         xmlParamToProp(xml_value, dynamic_cast<RTT::Property<Eigen::VectorXd >*>(prop_base));
00645       // Return true if it gets parsed into an array structure, otherwise, try property bag decomposition
00646       if(array_ret) {
00647         return true;
00648       }
00649     case XmlRpc::XmlRpcValue::TypeStruct:
00650       // Try to decompose the property in a property bag:
00651       if(xmlParamToProp(xml_value, dynamic_cast<RTT::Property<RTT::PropertyBag>*>(prop_base))) {
00652         return true;
00653       }
00654       {
00655         RTT::Property<RTT::PropertyBag> bag(prop_base->getName());
00656         if(RTT::types::propertyDecomposition(prop_base, bag.set()) && xmlParamToProp(xml_value, &bag)) {
00657           return true;
00658         } else {
00659           RTT::log(RTT::Debug) << "Could not decompose property bag for property type \"" << prop_base->getName() << "\"" << RTT::endlog();
00660           return false;
00661         }
00662 
00663       }
00664   };
00665 
00666   RTT::log(RTT::Debug) << "No appropriate conversion for property \"" << prop_base->getName() << "\"" << RTT::endlog();
00667 
00668   return false;
00669 }
00670 
00671 bool ROSParamService::get(
00672     const std::string &param_name, 
00673     const unsigned int policy)
00674 {
00675   RTT::Logger::In in("ROSParamService::get");
00676 
00677   const std::string resolved_name = resolvedName(
00678       param_name,ResolutionPolicy(policy));
00679 
00680   return this->getParam(resolved_name, param_name);
00681 }
00682 
00683 bool ROSParamService::getParam(
00684     const std::string &ros_name,
00685     const std::string &rtt_name)
00686 {
00687   RTT::Logger::In in("ROSParamService::getParam");
00688 
00689   try {
00690     // Get the parameter
00691     XmlRpc::XmlRpcValue xml_value;
00692 
00693     if(!ros::param::get(ros_name, xml_value)) {
00694       RTT::log(RTT::Debug) << "ROS Parameter \"" << ros_name << "\" not found on the parameter server!" << RTT::endlog();
00695       return false;
00696     }
00697 
00698     // Try to get the property if it exists
00699     RTT::base::PropertyBase *prop_base = this->getOwner()->getProperty(rtt_name);
00700     if(prop_base) {
00701       // Deal with the xml value
00702       bool ret = xmlParamToProp(xml_value, prop_base);
00703       if(!ret) {
00704         RTT::log(RTT::Warning) << "Could not convert \"" << ros_name << "\" from an XMLRPC value to an RTT property." << RTT::endlog();
00705       }
00706       return ret;
00707     }
00708 
00709     // Try to get the properties of a sub-service if it exists
00710     RTT::Service::shared_ptr service = this->getOwner()->provides()->getService(rtt_name);
00711     if(service) {
00712       // Get all parameters of the sub-service
00713       return getParams(service, ros_name);
00714     }
00715 
00716     RTT::log(RTT::Debug) << "RTT component does not have a property or service named \"" << rtt_name << "\"" << RTT::endlog();
00717 
00718   } catch(XmlRpc::XmlRpcException &err) {
00719     RTT::log(RTT::Error) << "XmlRpcException when getting ROS parameter \""<<ros_name<<"\": " << err.getMessage() << RTT::endlog();
00720     RTT::log(RTT::Debug) << " -- Make sure your parameters are the right primitive type." << RTT::endlog();
00721   }
00722 
00723   return false;
00724 }
00725 
00726 
00727 bool ROSParamService::getParams(const ROSParamService::ResolutionPolicy policy)
00728 {
00729   return getParams(this->getOwner()->provides(), resolvedName(std::string(), policy));
00730 }
00731 
00732 bool ROSParamService::getParams(
00733     RTT::Service::shared_ptr service, 
00734     const std::string& ns)
00735 {
00736   RTT::Logger::In in("ROSParamService::getParams");
00737 
00738   // Get the parameter
00739   XmlRpc::XmlRpcValue xml_value;
00740 
00741   if(!ros::param::get(ns, xml_value)) {
00742     RTT::log(RTT::Debug) << "ROS Parameter namespace \"" << ns << "\" not found on the parameter server!" << RTT::endlog();
00743     return false;
00744   }
00745 
00746   // Create a Property<> wrapper around the propertybag
00747   RTT::PropertyBag *properties = service->properties();
00748   RTT::internal::AssignableDataSource<RTT::PropertyBag>::shared_ptr datasource(new RTT::internal::ReferenceDataSource<RTT::PropertyBag>(*properties));
00749   RTT::Property<RTT::PropertyBag> prop(this->getOwner()->getName(),"",datasource);
00750 
00751   // Deal with the xml value
00752   bool ret = xmlParamToProp(xml_value, &prop);
00753   if (!ret) {
00754     RTT::log(RTT::Warning) << "Could not convert \"" << ns << "\" from an XMLRPC value to an RTT property." << RTT::endlog();
00755     return false;
00756   }
00757 
00758   // Recurse into sub-services
00759   RTT::Service::ProviderNames names = service->getProviderNames();
00760   for (RTT::Service::ProviderNames::const_iterator it = names.begin(); it != names.end(); ++it)
00761   {
00762     RTT::Service::shared_ptr sub(service->getService(*it));
00763     if (sub) {
00764       std::string sub_ns = sub->getName();
00765       if (!ns.empty()) sub_ns = ros::names::append(ns,sub_ns);
00766       getParams(sub, sub_ns);
00767     }
00768   }
00769 
00770   return true;
00771 }
00772 
00773 ORO_SERVICE_NAMED_PLUGIN(ROSParamService, "rosparam")


rtt_rosparam
Author(s): Ruben Smits
autogenerated on Mon Apr 3 2017 03:34:53