$search
00001 /* 00002 * Copyright (c) 2008, Willow Garage, Inc. 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 * 00008 * * Redistributions of source code must retain the above copyright 00009 * notice, this list of conditions and the following disclaimer. 00010 * * Redistributions in binary form must reproduce the above copyright 00011 * notice, this list of conditions and the following disclaimer in the 00012 * documentation and/or other materials provided with the distribution. 00013 * * Neither the name of the Willow Garage, Inc. nor the names of its 00014 * contributors may be used to endorse or promote products derived from 00015 * this software without specific prior written permission. 00016 * 00017 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 00018 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 00019 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 00020 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 00021 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 00022 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 00023 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 00024 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 00025 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 00026 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00027 * POSSIBILITY OF SUCH DAMAGE. 00028 */ 00029 00030 #ifndef FILTERS_FILTER_BASE_H_ 00031 #define FILTERS_FILTER_BASE_H_ 00032 00033 #include <typeinfo> 00034 #include "ros/assert.h" 00035 #include "ros/console.h" 00036 #include "ros/ros.h" 00037 00038 #include "boost/scoped_ptr.hpp" 00039 #include <boost/algorithm/string.hpp> 00040 00041 namespace filters 00042 { 00043 00044 typedef std::map<std::string, XmlRpc::XmlRpcValue> string_map_t; 00045 00049 template<typename T> 00050 class FilterBase 00051 { 00052 public: 00055 FilterBase():configured_(false){}; 00056 00059 virtual ~FilterBase(){}; 00060 00065 bool configure(const std::string& param_name, ros::NodeHandle node_handle = ros::NodeHandle()) 00066 { 00067 XmlRpc::XmlRpcValue config; 00068 if (!node_handle.getParam(param_name, config)) 00069 { 00070 ROS_ERROR("Could not find parameter %s on the server, are you sure that it was pushed up correctly?", param_name.c_str()); 00071 return false; 00072 } 00073 return configure(config); 00074 00075 } 00076 00080 bool configure(XmlRpc::XmlRpcValue& config) 00081 { 00082 if (configured_) 00083 { 00084 ROS_WARN("Filter %s of type %s already being reconfigured", filter_name_.c_str(), filter_type_.c_str()); 00085 }; 00086 configured_ = false; 00087 bool retval = true; 00088 00089 retval = retval && loadConfiguration(config); 00090 retval = retval && configure(); 00091 configured_ = retval; 00092 return retval; 00093 } 00094 00100 virtual bool update(const T& data_in, T& data_out)=0; 00101 00103 std::string getType() {return filter_type_;}; 00104 00106 inline const std::string& getName(){return filter_name_;}; 00107 00108 00109 protected: 00110 00114 virtual bool configure()=0; 00115 00116 00121 bool getParam(const std::string& name, std::string& value) 00122 { 00123 string_map_t::iterator it = params_.find(name); 00124 if (it == params_.end()) 00125 { 00126 return false; 00127 } 00128 00129 if(it->second.getType() != XmlRpc::XmlRpcValue::TypeString) 00130 { 00131 return false; 00132 } 00133 00134 value = std::string(it->second); 00135 return true; 00136 } 00137 00142 bool getParam(const std::string&name, double& value) 00143 { 00144 string_map_t::iterator it = params_.find(name); 00145 if (it == params_.end()) 00146 { 00147 return false; 00148 } 00149 00150 if(it->second.getType() != XmlRpc::XmlRpcValue::TypeDouble && it->second.getType() != XmlRpc::XmlRpcValue::TypeInt) 00151 { 00152 return false; 00153 } 00154 00155 value = it->second.getType() == XmlRpc::XmlRpcValue::TypeInt ? (int)(it->second) : (double)(it->second); 00156 return true; 00157 } 00158 00163 bool getParam(const std::string&name, int& value) 00164 { 00165 string_map_t::iterator it = params_.find(name); 00166 if (it == params_.end()) 00167 { 00168 return false; 00169 } 00170 00171 if(it->second.getType() != XmlRpc::XmlRpcValue::TypeInt) 00172 { 00173 return false; 00174 } 00175 00176 value = it->second; 00177 return true; 00178 } 00179 00184 bool getParam(const std::string&name, unsigned int& value) 00185 { 00186 int signed_value; 00187 if (!getParam(name, signed_value)) 00188 return false; 00189 if (signed_value < 0) 00190 return false; 00191 value = signed_value; 00192 return true; 00193 }; 00194 00199 bool getParam(const std::string&name, std::vector<double>& value) 00200 { 00201 string_map_t::iterator it = params_.find(name); 00202 if (it == params_.end()) 00203 { 00204 return false; 00205 } 00206 00207 value.clear(); 00208 00209 if(it->second.getType() != XmlRpc::XmlRpcValue::TypeArray) 00210 { 00211 return false; 00212 } 00213 00214 XmlRpc::XmlRpcValue double_array = it->second; 00215 00216 for (int i = 0; i < double_array.size(); ++i){ 00217 if(double_array[i].getType() != XmlRpc::XmlRpcValue::TypeDouble && double_array[i].getType() != XmlRpc::XmlRpcValue::TypeInt) 00218 { 00219 return false; 00220 } 00221 00222 double double_value = double_array[i].getType() == XmlRpc::XmlRpcValue::TypeInt ? (int)(double_array[i]) : (double)(double_array[i]); 00223 value.push_back(double_value); 00224 } 00225 00226 return true; 00227 } 00228 00233 bool getParam(const std::string&name, std::vector<std::string>& value) 00234 { 00235 string_map_t::iterator it = params_.find(name); 00236 if (it == params_.end()) 00237 { 00238 return false; 00239 } 00240 00241 value.clear(); 00242 00243 if(it->second.getType() != XmlRpc::XmlRpcValue::TypeArray) 00244 { 00245 return false; 00246 } 00247 00248 XmlRpc::XmlRpcValue string_array = it->second; 00249 00250 for (unsigned int i = 0; i < string_array.size(); ++i){ 00251 if(string_array[i].getType() != XmlRpc::XmlRpcValue::TypeString) 00252 { 00253 return false; 00254 } 00255 00256 value.push_back(string_array[i]); 00257 } 00258 00259 return true; 00260 } 00261 00266 bool getParam(const std::string& name, XmlRpc::XmlRpcValue& value) 00267 { 00268 string_map_t::iterator it = params_.find(name); 00269 if (it == params_.end()) 00270 { 00271 return false; 00272 } 00273 00274 value = it->second; 00275 return true; 00276 } 00277 00279 std::string filter_name_; 00281 std::string filter_type_; 00283 bool configured_; 00284 00286 string_map_t params_; 00287 00288 private: 00292 bool setNameAndType(XmlRpc::XmlRpcValue& config) 00293 { 00294 if(!config.hasMember("name")) 00295 { 00296 ROS_ERROR("Filter didn't have name defined, other strings are not allowed"); 00297 return false; 00298 } 00299 00300 std::string name = config["name"]; 00301 00302 if(!config.hasMember("type")) 00303 { 00304 ROS_ERROR("Filter %s didn't have type defined, other strings are not allowed", name.c_str()); 00305 return false; 00306 } 00307 00308 std::string type = config["type"]; 00309 00310 filter_name_ = name; 00311 filter_type_ = type; 00312 ROS_DEBUG("Configuring Filter of Type: %s with name %s", type.c_str(), name.c_str()); 00313 return true; 00314 } 00315 00316 protected: 00317 bool loadConfiguration(XmlRpc::XmlRpcValue& config) 00318 { 00319 if(config.getType() != XmlRpc::XmlRpcValue::TypeStruct) 00320 { 00321 ROS_ERROR("A filter configuration must be a map with fields name, type, and params"); 00322 return false; 00323 } 00324 00325 if (!setNameAndType(config)) 00326 { 00327 return false; 00328 } 00329 00330 //check to see if we have parameters in our list 00331 if(config.hasMember("params")) 00332 { 00333 //get the params map 00334 XmlRpc::XmlRpcValue params = config["params"]; 00335 00336 if(params.getType() != XmlRpc::XmlRpcValue::TypeStruct) 00337 { 00338 ROS_ERROR("params must be a map"); 00339 return false; 00340 } 00341 else{ 00342 //Load params into map 00343 for(XmlRpc::XmlRpcValue::iterator it = params.begin(); it != params.end(); ++it) 00344 { 00345 ROS_DEBUG("Loading param %s\n", it->first.c_str()); 00346 params_[it->first] = it->second; 00347 } 00348 } 00349 } 00350 00351 return true; 00352 } 00353 }; 00354 00355 00356 template <typename T> 00357 class MultiChannelFilterBase : public FilterBase<T> 00358 { 00359 public: 00360 MultiChannelFilterBase():number_of_channels_(0){}; 00361 00367 bool configure(unsigned int number_of_channels, const std::string& param_name, ros::NodeHandle node_handle = ros::NodeHandle()) 00368 { 00369 XmlRpc::XmlRpcValue config; 00370 if (!node_handle.getParam(param_name, config)) 00371 { 00372 ROS_ERROR("Could not find parameter %s on the server, are you sure that it was pushed up correctly?", param_name.c_str()); 00373 return false; 00374 } 00375 return configure(number_of_channels, config); 00376 00377 } 00378 00379 00384 bool configure(unsigned int number_of_channels, XmlRpc::XmlRpcValue& config) 00385 { 00386 ROS_DEBUG("FilterBase being configured with XmlRpc xml: %s type: %d", config.toXml().c_str(), config.getType()); 00387 if (configured_) 00388 { 00389 ROS_WARN("Filter %s of type %s already being reconfigured", filter_name_.c_str(), filter_type_.c_str()); 00390 }; 00391 configured_ = false; 00392 number_of_channels_ = number_of_channels; 00393 ROS_DEBUG("MultiChannelFilterBase configured with %d channels", number_of_channels_); 00394 bool retval = true; 00395 00396 retval = retval && FilterBase<T>::loadConfiguration(config); 00397 retval = retval && configure(); 00398 configured_ = retval; 00399 return retval; 00400 }; 00401 00402 00404 bool configure(XmlRpc::XmlRpcValue& config) 00405 { 00406 ROS_ERROR("MultiChannelFilterBase configure should be called with a number of channels argument, assuming 1"); 00407 return configure(1, config); 00408 } 00409 00410 virtual bool configure()=0; 00411 00412 00418 virtual bool update(const std::vector<T>& data_in, std::vector<T>& data_out)=0; 00419 00420 virtual bool update(const T& data_in, T& data_out) 00421 { 00422 ROS_ERROR("THIS IS A MULTI FILTER DON'T CALL SINGLE FORM OF UPDATE"); 00423 return false; 00424 }; 00425 00426 00427 protected: 00428 using FilterBase<T>::configured_; 00429 using FilterBase<T>::filter_type_; 00430 using FilterBase<T>::filter_name_; 00431 00433 unsigned int number_of_channels_; 00434 00435 00436 }; 00437 00438 } 00439 #endif //#ifndef FILTERS_FILTER_BASE_H_