param.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (C) 2009, Willow Garage, Inc.
00003  *
00004  * Redistribution and use in source and binary forms, with or without
00005  * modification, are permitted provided that the following conditions are met:
00006  *   * Redistributions of source code must retain the above copyright notice,
00007  *     this list of conditions and the following disclaimer.
00008  *   * Redistributions in binary form must reproduce the above copyright
00009  *     notice, this list of conditions and the following disclaimer in the
00010  *     documentation and/or other materials provided with the distribution.
00011  *   * Neither the names of Willow Garage, Inc. nor the names of its
00012  *     contributors may be used to endorse or promote products derived from
00013  *     this software without specific prior written permission.
00014  *
00015  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00016  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00017  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00018  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00019  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00020  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00021  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00022  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00023  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00024  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00025  * POSSIBILITY OF SUCH DAMAGE.
00026  */
00027 
00028 #include "ros/param.h"
00029 #include "ros/master.h"
00030 #include "ros/xmlrpc_manager.h"
00031 #include "ros/this_node.h"
00032 #include "ros/names.h"
00033 
00034 #include <ros/console.h>
00035 
00036 #include <boost/thread/mutex.hpp>
00037 #include <boost/lexical_cast.hpp>
00038 
00039 #include <vector>
00040 #include <map>
00041 
00042 namespace ros
00043 {
00044 
00045 namespace param
00046 {
00047 
00048 typedef std::map<std::string, XmlRpc::XmlRpcValue> M_Param;
00049 M_Param g_params;
00050 boost::mutex g_params_mutex;
00051 S_string g_subscribed_params;
00052 
00053 void set(const std::string& key, const XmlRpc::XmlRpcValue& v)
00054 {
00055   std::string mapped_key = ros::names::resolve(key);
00056 
00057   XmlRpc::XmlRpcValue params, result, payload;
00058   params[0] = this_node::getName();
00059   params[1] = mapped_key;
00060   params[2] = v;
00061 
00062   {
00063     // Lock around the execute to the master in case we get a parameter update on this value between
00064     // executing on the master and setting the parameter in the g_params list.
00065     boost::mutex::scoped_lock lock(g_params_mutex);
00066 
00067     if (master::execute("setParam", params, result, payload, true))
00068     {
00069       // Update our cached params list now so that if get() is called immediately after param::set()
00070       // we already have the cached state and our value will be correct
00071       if (g_subscribed_params.find(mapped_key) != g_subscribed_params.end())
00072       {
00073         g_params[mapped_key] = v;
00074       }
00075     }
00076   }
00077 }
00078 
00079 void set(const std::string& key, const std::string& s)
00080 {
00081   // construct xmlrpc_c::value object of the std::string and
00082   // call param::set(key, xmlvalue);
00083   XmlRpc::XmlRpcValue v(s);
00084   ros::param::set(key, v);
00085 }
00086 
00087 void set(const std::string& key, const char* s)
00088 {
00089   // construct xmlrpc_c::value object of the std::string and
00090   // call param::set(key, xmlvalue);
00091   std::string sxx = std::string(s);
00092   XmlRpc::XmlRpcValue v(sxx);
00093   ros::param::set(key, v);
00094 }
00095 
00096 void set(const std::string& key, double d)
00097 {
00098   XmlRpc::XmlRpcValue v(d);
00099   ros::param::set(key, v);
00100 }
00101 
00102 void set(const std::string& key, int i)
00103 {
00104   XmlRpc::XmlRpcValue v(i);
00105   ros::param::set(key, v);
00106 }
00107 
00108 void set(const std::string& key, bool b)
00109 {
00110   XmlRpc::XmlRpcValue v(b);
00111   ros::param::set(key, v);
00112 }
00113 
00114 template <class T>
00115   void setImpl(const std::string& key, const std::vector<T>& vec)
00116 {
00117   // Note: the XmlRpcValue starts off as "invalid" and assertArray turns it
00118   // into an array type with the given size
00119   XmlRpc::XmlRpcValue xml_vec;
00120   xml_vec.setSize(vec.size());
00121 
00122   // Copy the contents into the XmlRpcValue
00123   for(size_t i=0; i < vec.size(); i++) {
00124     xml_vec[i] = vec.at(i);
00125   }
00126 
00127   ros::param::set(key, xml_vec);
00128 }
00129 
00130 void set(const std::string& key, const std::vector<std::string>& vec)
00131 {
00132   setImpl(key, vec);
00133 }
00134 
00135 void set(const std::string& key, const std::vector<double>& vec)
00136 {
00137   setImpl(key, vec);
00138 }
00139 
00140 void set(const std::string& key, const std::vector<float>& vec)
00141 {
00142   setImpl(key, vec);
00143 }
00144 
00145 void set(const std::string& key, const std::vector<int>& vec)
00146 {
00147   setImpl(key, vec);
00148 }
00149 
00150 void set(const std::string& key, const std::vector<bool>& vec)
00151 {
00152   setImpl(key, vec);
00153 }
00154 
00155 template <class T>
00156   void setImpl(const std::string& key, const std::map<std::string, T>& map)
00157 {
00158   // Note: the XmlRpcValue starts off as "invalid" and assertArray turns it
00159   // into an array type with the given size
00160   XmlRpc::XmlRpcValue xml_value;
00161   const XmlRpc::XmlRpcValue::ValueStruct& xml_map = (const XmlRpc::XmlRpcValue::ValueStruct &)(xml_value);
00162 
00163   // Copy the contents into the XmlRpcValue
00164   for(typename std::map<std::string, T>::const_iterator it = map.begin(); it != map.end(); ++it) {
00165     xml_value[it->first] = it->second;
00166   }
00167 
00168   ros::param::set(key, xml_value);
00169 }
00170 
00171 void set(const std::string& key, const std::map<std::string, std::string>& map)
00172 {
00173   setImpl(key, map);
00174 }
00175 
00176 void set(const std::string& key, const std::map<std::string, double>& map)
00177 {
00178   setImpl(key, map);
00179 }
00180 
00181 void set(const std::string& key, const std::map<std::string, float>& map)
00182 {
00183   setImpl(key, map);
00184 }
00185 
00186 void set(const std::string& key, const std::map<std::string, int>& map)
00187 {
00188   setImpl(key, map);
00189 }
00190 
00191 void set(const std::string& key, const std::map<std::string, bool>& map)
00192 {
00193   setImpl(key, map);
00194 }
00195 
00196 bool has(const std::string& key)
00197 {
00198   XmlRpc::XmlRpcValue params, result, payload;
00199   params[0] = this_node::getName();
00200   params[1] = ros::names::resolve(key);
00201   //params[1] = key;
00202   // We don't loop here, because validateXmlrpcResponse() returns false
00203   // both when we can't contact the master and when the master says, "I
00204   // don't have that param."
00205   if (!master::execute("hasParam", params, result, payload, false))
00206   {
00207     return false;
00208   }
00209 
00210   return payload;
00211 }
00212 
00213 bool del(const std::string& key)
00214 {
00215   std::string mapped_key = ros::names::resolve(key);
00216 
00217   {
00218     boost::mutex::scoped_lock lock(g_params_mutex);
00219 
00220     S_string::iterator sub_it = g_subscribed_params.find(mapped_key);
00221     if (sub_it != g_subscribed_params.end())
00222     {
00223       g_subscribed_params.erase(sub_it);
00224 
00225       M_Param::iterator param_it = g_params.find(mapped_key);
00226       if (param_it != g_params.end())
00227       {
00228         g_params.erase(param_it);
00229       }
00230     }
00231   }
00232 
00233   XmlRpc::XmlRpcValue params, result, payload;
00234   params[0] = this_node::getName();
00235   params[1] = mapped_key;
00236   // We don't loop here, because validateXmlrpcResponse() returns false
00237   // both when we can't contact the master and when the master says, "I
00238   // don't have that param."
00239   if (!master::execute("deleteParam", params, result, payload, false))
00240   {
00241     return false;
00242   }
00243 
00244   return true;
00245 }
00246 
00247 bool getImpl(const std::string& key, XmlRpc::XmlRpcValue& v, bool use_cache)
00248 {
00249   std::string mapped_key = ros::names::resolve(key);
00250 
00251   if (use_cache)
00252   {
00253     boost::mutex::scoped_lock lock(g_params_mutex);
00254 
00255     if (g_subscribed_params.find(mapped_key) != g_subscribed_params.end())
00256     {
00257       M_Param::iterator it = g_params.find(mapped_key);
00258       if (it != g_params.end())
00259       {
00260         if (it->second.valid())
00261         {
00262           ROS_DEBUG_NAMED("cached_parameters", "Using cached parameter value for key [%s]", mapped_key.c_str());
00263 
00264           v = it->second;
00265           return true;
00266         }
00267         else
00268         {
00269           ROS_DEBUG_NAMED("cached_parameters", "Cached parameter is invalid for key [%s]", mapped_key.c_str());
00270           return false;
00271         }
00272       }
00273     }
00274     else
00275     {
00276       // parameter we've never seen before, register for update from the master
00277       if (g_subscribed_params.insert(mapped_key).second)
00278       {
00279         XmlRpc::XmlRpcValue params, result, payload;
00280         params[0] = this_node::getName();
00281         params[1] = XMLRPCManager::instance()->getServerURI();
00282         params[2] = mapped_key;
00283 
00284         if (!master::execute("subscribeParam", params, result, payload, false))
00285         {
00286           ROS_DEBUG_NAMED("cached_parameters", "Subscribe to parameter [%s]: call to the master failed", mapped_key.c_str());
00287           g_subscribed_params.erase(mapped_key);
00288           use_cache = false;
00289         }
00290         else
00291         {
00292           ROS_DEBUG_NAMED("cached_parameters", "Subscribed to parameter [%s]", mapped_key.c_str());
00293         }
00294       }
00295     }
00296   }
00297 
00298   XmlRpc::XmlRpcValue params, result;
00299   params[0] = this_node::getName();
00300   params[1] = mapped_key;
00301 
00302   // We don't loop here, because validateXmlrpcResponse() returns false
00303   // both when we can't contact the master and when the master says, "I
00304   // don't have that param."
00305   bool ret = master::execute("getParam", params, result, v, false);
00306 
00307   if (use_cache)
00308   {
00309     boost::mutex::scoped_lock lock(g_params_mutex);
00310 
00311     ROS_DEBUG_NAMED("cached_parameters", "Caching parameter [%s] with value type [%d]", mapped_key.c_str(), v.getType());
00312     g_params[mapped_key] = v;
00313   }
00314 
00315   return ret;
00316 }
00317 
00318 bool getImpl(const std::string& key, std::string& s, bool use_cache)
00319 {
00320   XmlRpc::XmlRpcValue v;
00321   if (!getImpl(key, v, use_cache))
00322     return false;
00323   if (v.getType() != XmlRpc::XmlRpcValue::TypeString)
00324     return false;
00325   s = std::string(v);
00326   return true;
00327 }
00328 
00329 bool getImpl(const std::string& key, double& d, bool use_cache)
00330 {
00331   XmlRpc::XmlRpcValue v;
00332   if (!getImpl(key, v, use_cache))
00333   {
00334     return false;
00335   }
00336 
00337   if (v.getType() == XmlRpc::XmlRpcValue::TypeInt)
00338   {
00339     d = (int)v;
00340   }
00341   else if (v.getType() != XmlRpc::XmlRpcValue::TypeDouble)
00342   {
00343     return false;
00344   }
00345   else
00346   {
00347     d = v;
00348   }
00349 
00350   return true;
00351 }
00352 
00353 bool getImpl(const std::string& key, float& f, bool use_cache)
00354 {
00355   double d = static_cast<double>(f);
00356   bool result = getImpl(key, d, use_cache);
00357   if (result)
00358     f = static_cast<float>(d);
00359   return result;
00360 }
00361 
00362 bool getImpl(const std::string& key, int& i, bool use_cache)
00363 {
00364   XmlRpc::XmlRpcValue v;
00365   if (!getImpl(key, v, use_cache))
00366   {
00367     return false;
00368   }
00369 
00370   if (v.getType() == XmlRpc::XmlRpcValue::TypeDouble)
00371   {
00372     double d = v;
00373 
00374     if (fmod(d, 1.0) < 0.5)
00375     {
00376       d = floor(d);
00377     }
00378     else
00379     {
00380       d = ceil(d);
00381     }
00382 
00383     i = d;
00384   }
00385   else if (v.getType() != XmlRpc::XmlRpcValue::TypeInt)
00386   {
00387     return false;
00388   }
00389   else
00390   {
00391     i = v;
00392   }
00393 
00394   return true;
00395 }
00396 
00397 bool getImpl(const std::string& key, bool& b, bool use_cache)
00398 {
00399   XmlRpc::XmlRpcValue v;
00400   if (!getImpl(key, v, use_cache))
00401     return false;
00402   if (v.getType() != XmlRpc::XmlRpcValue::TypeBoolean)
00403     return false;
00404   b = v;
00405   return true;
00406 }
00407 
00408 bool get(const std::string& key, std::string& s)
00409 {
00410         return getImpl(key, s, false);
00411 }
00412 
00413 bool get(const std::string& key, double& d)
00414 {
00415         return getImpl(key, d, false);
00416 }
00417 
00418 bool get(const std::string& key, float& f)
00419 {
00420         return getImpl(key, f, false);
00421 }
00422 
00423 bool get(const std::string& key, int& i)
00424 {
00425         return getImpl(key, i, false);
00426 }
00427 
00428 bool get(const std::string& key, bool& b)
00429 {
00430         return getImpl(key, b, false);
00431 }
00432 
00433 bool get(const std::string& key, XmlRpc::XmlRpcValue& v)
00434 {
00435         return getImpl(key, v, false);
00436 }
00437 
00438 bool getCached(const std::string& key, std::string& s)
00439 {
00440         return getImpl(key, s, true);
00441 }
00442 
00443 bool getCached(const std::string& key, double& d)
00444 {
00445         return getImpl(key, d, true);
00446 }
00447 
00448 bool getCached(const std::string& key, float& f)
00449 {
00450         return getImpl(key, f, true);
00451 }
00452 
00453 bool getCached(const std::string& key, int& i)
00454 {
00455         return getImpl(key, i, true);
00456 }
00457 
00458 bool getCached(const std::string& key, bool& b)
00459 {
00460         return getImpl(key, b, true);
00461 }
00462 
00463 bool getCached(const std::string& key, XmlRpc::XmlRpcValue& v)
00464 {
00465         return getImpl(key, v, true);
00466 }
00467 
00468 template <class T> T xml_cast(XmlRpc::XmlRpcValue xml_value) 
00469 {
00470   return static_cast<T>(xml_value);
00471 }
00472 
00473 template <class T> bool xml_castable(int XmlType) 
00474 {
00475   return false;
00476 }
00477 
00478 template<> bool xml_castable<std::string>(int XmlType)
00479 {
00480   return XmlType == XmlRpc::XmlRpcValue::TypeString;
00481 }
00482 
00483 template<> bool xml_castable<double>(int XmlType)
00484 {
00485   return ( 
00486       XmlType == XmlRpc::XmlRpcValue::TypeDouble ||
00487       XmlType == XmlRpc::XmlRpcValue::TypeInt ||
00488       XmlType == XmlRpc::XmlRpcValue::TypeBoolean );
00489 }
00490 
00491 template<> bool xml_castable<float>(int XmlType)
00492 {
00493   return ( 
00494       XmlType == XmlRpc::XmlRpcValue::TypeDouble ||
00495       XmlType == XmlRpc::XmlRpcValue::TypeInt ||
00496       XmlType == XmlRpc::XmlRpcValue::TypeBoolean );
00497 }
00498 
00499 template<> bool xml_castable<int>(int XmlType)
00500 {
00501   return ( 
00502       XmlType == XmlRpc::XmlRpcValue::TypeDouble ||
00503       XmlType == XmlRpc::XmlRpcValue::TypeInt ||
00504       XmlType == XmlRpc::XmlRpcValue::TypeBoolean );
00505 }
00506 
00507 template<> bool xml_castable<bool>(int XmlType)
00508 {
00509   return ( 
00510       XmlType == XmlRpc::XmlRpcValue::TypeDouble ||
00511       XmlType == XmlRpc::XmlRpcValue::TypeInt ||
00512       XmlType == XmlRpc::XmlRpcValue::TypeBoolean );
00513 }
00514 
00515 template<> double xml_cast(XmlRpc::XmlRpcValue xml_value)
00516 {
00517   using namespace XmlRpc;
00518   switch(xml_value.getType()) {
00519     case XmlRpcValue::TypeDouble:
00520       return static_cast<double>(xml_value);
00521     case XmlRpcValue::TypeInt:
00522       return static_cast<double>(static_cast<int>(xml_value));
00523     case XmlRpcValue::TypeBoolean:
00524       return static_cast<double>(static_cast<bool>(xml_value));
00525   };
00526 }
00527 
00528 template<> float xml_cast(XmlRpc::XmlRpcValue xml_value)
00529 {
00530   using namespace XmlRpc;
00531   switch(xml_value.getType()) {
00532     case XmlRpcValue::TypeDouble:
00533       return static_cast<float>(static_cast<double>(xml_value));
00534     case XmlRpcValue::TypeInt:
00535       return static_cast<float>(static_cast<int>(xml_value));
00536     case XmlRpcValue::TypeBoolean:
00537       return static_cast<float>(static_cast<bool>(xml_value));
00538   };
00539 }
00540 
00541 template<> int xml_cast(XmlRpc::XmlRpcValue xml_value)
00542 {
00543   using namespace XmlRpc;
00544   switch(xml_value.getType()) {
00545     case XmlRpcValue::TypeDouble:
00546       return static_cast<int>(static_cast<double>(xml_value));
00547     case XmlRpcValue::TypeInt:
00548       return static_cast<int>(xml_value);
00549     case XmlRpcValue::TypeBoolean:
00550       return static_cast<int>(static_cast<bool>(xml_value));
00551   };
00552 }
00553 
00554 template<> bool xml_cast(XmlRpc::XmlRpcValue xml_value)
00555 {
00556   using namespace XmlRpc;
00557   switch(xml_value.getType()) {
00558     case XmlRpcValue::TypeDouble:
00559       return static_cast<bool>(static_cast<double>(xml_value));
00560     case XmlRpcValue::TypeInt:
00561       return static_cast<bool>(static_cast<int>(xml_value));
00562     case XmlRpcValue::TypeBoolean:
00563       return static_cast<bool>(xml_value);
00564   };
00565 }
00566   
00567 template <class T>
00568   bool getImpl(const std::string& key, std::vector<T>& vec, bool cached)
00569 {
00570   XmlRpc::XmlRpcValue xml_array;
00571   if(!getImpl(key, xml_array, cached)) {
00572     return false;
00573   }
00574 
00575   // Make sure it's an array type
00576   if(xml_array.getType() != XmlRpc::XmlRpcValue::TypeArray) {
00577     return false;
00578   }
00579 
00580   // Resize the target vector (destructive)
00581   vec.resize(xml_array.size());
00582 
00583   // Fill the vector with stuff
00584   for (int i = 0; i < xml_array.size(); i++) {
00585     if(!xml_castable<T>(xml_array[i].getType())) {
00586       return false;
00587     }
00588 
00589     vec[i] = xml_cast<T>(xml_array[i]);
00590   }
00591 
00592   return true;
00593 }
00594 
00595 bool get(const std::string& key, std::vector<std::string>& vec)
00596 {
00597   return getImpl(key, vec, false);
00598 }
00599 bool get(const std::string& key, std::vector<double>& vec)
00600 {
00601   return getImpl(key, vec, false);
00602 }
00603 bool get(const std::string& key, std::vector<float>& vec)
00604 {
00605   return getImpl(key, vec, false);
00606 }
00607 bool get(const std::string& key, std::vector<int>& vec)
00608 {
00609   return getImpl(key, vec, false);
00610 }
00611 bool get(const std::string& key, std::vector<bool>& vec)
00612 {
00613   return getImpl(key, vec, false);
00614 }
00615 
00616 bool getCached(const std::string& key, std::vector<std::string>& vec)
00617 {
00618   return getImpl(key, vec, true);
00619 }
00620 bool getCached(const std::string& key, std::vector<double>& vec)
00621 {
00622   return getImpl(key, vec, true);
00623 }
00624 bool getCached(const std::string& key, std::vector<float>& vec)
00625 {
00626   return getImpl(key, vec, true);
00627 }
00628 bool getCached(const std::string& key, std::vector<int>& vec)
00629 {
00630   return getImpl(key, vec, true);
00631 }
00632 bool getCached(const std::string& key, std::vector<bool>& vec)
00633 {
00634   return getImpl(key, vec, true);
00635 }
00636 
00637 template <class T>
00638   bool getImpl(const std::string& key, std::map<std::string, T>& map, bool cached)
00639 {
00640   XmlRpc::XmlRpcValue xml_value;
00641   if(!getImpl(key, xml_value, cached)) {
00642     return false;
00643   }
00644 
00645   // Make sure it's a struct type
00646   if(xml_value.getType() != XmlRpc::XmlRpcValue::TypeStruct) {
00647     return false;
00648   }
00649 
00650   // Fill the map with stuff
00651   for (XmlRpc::XmlRpcValue::ValueStruct::const_iterator it = xml_value.begin();
00652       it != xml_value.end();
00653       ++it)
00654   {
00655     // Make sure this element is the right type
00656     if(!xml_castable<T>(it->second.getType())) {
00657       return false;
00658     }
00659     // Store the element
00660     map[it->first] = xml_cast<T>(it->second);
00661   }
00662 
00663   return true;
00664 }
00665 
00666 bool get(const std::string& key, std::map<std::string, std::string>& map)
00667 {
00668   return getImpl(key, map, false);
00669 }
00670 bool get(const std::string& key, std::map<std::string, double>& map)
00671 {
00672   return getImpl(key, map, false);
00673 }
00674 bool get(const std::string& key, std::map<std::string, float>& map)
00675 {
00676   return getImpl(key, map, false);
00677 }
00678 bool get(const std::string& key, std::map<std::string, int>& map)
00679 {
00680   return getImpl(key, map, false);
00681 }
00682 bool get(const std::string& key, std::map<std::string, bool>& map)
00683 {
00684   return getImpl(key, map, false);
00685 }
00686 
00687 bool getCached(const std::string& key, std::map<std::string, std::string>& map)
00688 {
00689   return getImpl(key, map, true);
00690 }
00691 bool getCached(const std::string& key, std::map<std::string, double>& map)
00692 {
00693   return getImpl(key, map, true);
00694 }
00695 bool getCached(const std::string& key, std::map<std::string, float>& map)
00696 {
00697   return getImpl(key, map, true);
00698 }
00699 bool getCached(const std::string& key, std::map<std::string, int>& map)
00700 {
00701   return getImpl(key, map, true);
00702 }
00703 bool getCached(const std::string& key, std::map<std::string, bool>& map)
00704 {
00705   return getImpl(key, map, true);
00706 }
00707 
00708 
00709 bool search(const std::string& key, std::string& result_out)
00710 {
00711   return search(this_node::getName(), key, result_out);
00712 }
00713 
00714 bool search(const std::string& ns, const std::string& key, std::string& result_out)
00715 {
00716   XmlRpc::XmlRpcValue params, result, payload;
00717   params[0] = ns;
00718 
00719   // searchParam needs a separate form of remapping -- remapping on the unresolved name, rather than the
00720   // resolved one.
00721 
00722   std::string remapped = key;
00723   M_string::const_iterator it = names::getUnresolvedRemappings().find(key);
00724   if (it != names::getUnresolvedRemappings().end())
00725   {
00726     remapped = it->second;
00727   }
00728 
00729   params[1] = remapped;
00730   // We don't loop here, because validateXmlrpcResponse() returns false
00731   // both when we can't contact the master and when the master says, "I
00732   // don't have that param."
00733   if (!master::execute("searchParam", params, result, payload, false))
00734   {
00735     return false;
00736   }
00737 
00738   result_out = (std::string)payload;
00739 
00740   return true;
00741 }
00742 
00743 void update(const std::string& key, const XmlRpc::XmlRpcValue& v)
00744 {
00745   std::string clean_key = names::clean(key);
00746   ROS_DEBUG_NAMED("cached_parameters", "Received parameter update for key [%s]", clean_key.c_str());
00747 
00748   boost::mutex::scoped_lock lock(g_params_mutex);
00749 
00750   g_params[clean_key] = v;
00751 }
00752 
00753 void paramUpdateCallback(XmlRpc::XmlRpcValue& params, XmlRpc::XmlRpcValue& result)
00754 {
00755   result[0] = 1;
00756   result[1] = std::string("");
00757   result[2] = 0;
00758 
00759   ros::param::update((std::string)params[1], params[2]);
00760 }
00761 
00762 void init(const M_string& remappings)
00763 {
00764   M_string::const_iterator it = remappings.begin();
00765   M_string::const_iterator end = remappings.end();
00766   for (; it != end; ++it)
00767   {
00768     const std::string& name = it->first;
00769     const std::string& param = it->second;
00770 
00771     if (name.size() < 2)
00772     {
00773       continue;
00774     }
00775 
00776     if (name[0] == '_' && name[1] != '_')
00777     {
00778       std::string local_name = "~" + name.substr(1);
00779 
00780       bool success = false;
00781 
00782       try
00783       {
00784         int32_t i = boost::lexical_cast<int32_t>(param);
00785         ros::param::set(names::resolve(local_name), i);
00786         success = true;
00787       }
00788       catch (boost::bad_lexical_cast&)
00789       {
00790 
00791       }
00792 
00793       if (success)
00794       {
00795         continue;
00796       }
00797 
00798       try
00799       {
00800         double d = boost::lexical_cast<double>(param);
00801         ros::param::set(names::resolve(local_name), d);
00802         success = true;
00803       }
00804       catch (boost::bad_lexical_cast&)
00805       {
00806 
00807       }
00808 
00809       if (success)
00810       {
00811         continue;
00812       }
00813 
00814       if (param == "true" || param == "True" || param == "TRUE")
00815       {
00816         ros::param::set(names::resolve(local_name), true);
00817       }
00818       else if (param == "false" || param == "False" || param == "FALSE")
00819       {
00820         ros::param::set(names::resolve(local_name), false);
00821       }
00822       else
00823       {
00824         ros::param::set(names::resolve(local_name), param);
00825       }
00826     }
00827   }
00828 
00829   XMLRPCManager::instance()->bind("paramUpdate", paramUpdateCallback);
00830 }
00831 
00832 } // namespace param
00833 
00834 } // namespace ros


roscpp
Author(s): Morgan Quigley, Josh Faust, Brian Gerkey, Troy Straszheim
autogenerated on Mon Oct 6 2014 11:46:44