GenericParam.cpp
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2011, Willow Garage
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of the Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Ioan Sucan */
36 
37 #include "ompl/base/GenericParam.h"
38 #include "ompl/util/Exception.h"
39 #include <limits>
40 
41 namespace ompl
42 {
43  namespace base
44  {
45  template <>
46  bool SpecificParam<bool>::lexical_cast(const std::string &value) const
47  {
48  return !(value.empty() || value == "0" || value == "false" || value == "FALSE" || value == "False" ||
49  value == "f" || value == "F");
50  }
51  template <>
52  int SpecificParam<int>::lexical_cast(const std::string &value) const
53  {
54  return std::stoi(value);
55  }
56  template <>
57  long SpecificParam<long>::lexical_cast(const std::string &value) const
58  {
59  return std::stol(value);
60  }
61  template <>
62  long long SpecificParam<long long>::lexical_cast(const std::string &value) const
63  {
64  return std::stoll(value);
65  }
66  template <>
67  unsigned int SpecificParam<unsigned int>::lexical_cast(const std::string &value) const
68  {
69  return std::stoul(value);
70  }
71  template <>
72  unsigned long SpecificParam<unsigned long>::lexical_cast(const std::string &value) const
73  {
74  return std::stoul(value);
75  }
76  template <>
77  unsigned long long SpecificParam<unsigned long long>::lexical_cast(const std::string &value) const
78  {
79  return std::stoull(value);
80  }
81  template <>
82  float SpecificParam<float>::lexical_cast(const std::string &value) const
83  {
84  return ompl::stof(value);
85  }
86  template <>
87  double SpecificParam<double>::lexical_cast(const std::string &value) const
88  {
89  return ompl::stod(value);
90  }
91  template <>
92  long double SpecificParam<long double>::lexical_cast(const std::string &value) const
93  {
94  return ompl::stold(value);
95  }
96  template <>
97  char SpecificParam<char>::lexical_cast(const std::string &value) const
98  {
99  static const int minChar = std::numeric_limits<char>::min(), maxChar = std::numeric_limits<char>::max();
100  int val = std::stoi(value);
101  if (val < minChar || val > maxChar)
102  throw std::invalid_argument("character value out of range");
103  return val;
104  }
105  template <>
106  std::string SpecificParam<std::string>::lexical_cast(const std::string &value) const
107  {
108  return value;
109  }
110 
111  } // namespace base
112 } // namespace ompl
113 
114 bool ompl::base::ParamSet::setParam(const std::string &key, const std::string &value)
115 {
116  std::map<std::string, GenericParamPtr>::const_iterator it = params_.find(key);
117  if (it != params_.end())
118  return it->second->setValue(value);
119 
120  OMPL_ERROR("Parameter '%s' was not found", key.c_str());
121  return false;
122 }
123 
124 bool ompl::base::ParamSet::setParams(const std::map<std::string, std::string> &kv, bool ignoreUnknown)
125 {
126  bool result = true;
127  for (const auto &it : kv)
128  {
129  if (ignoreUnknown)
130  if (!hasParam(it.first))
131  continue;
132  bool r = setParam(it.first, it.second);
133  result = result && r;
134  }
135  return result;
136 }
137 
138 bool ompl::base::ParamSet::getParam(const std::string &key, std::string &value) const
139 {
140  auto it = params_.find(key);
141  if (it != params_.end())
142  {
143  value = it->second->getValue();
144  return true;
145  }
146  return false;
147 }
148 
149 void ompl::base::ParamSet::getParamNames(std::vector<std::string> &params) const
150 {
151  params.clear();
152  params.reserve(params_.size());
153  for (const auto &param : params_)
154  params.push_back(param.first);
155 }
156 
157 void ompl::base::ParamSet::getParamValues(std::vector<std::string> &vals) const
158 {
159  std::vector<std::string> names;
160  getParamNames(names);
161  vals.resize(names.size());
162  for (std::size_t i = 0; i < names.size(); ++i)
163  vals[i] = params_.find(names[i])->second->getValue();
164 }
165 
166 const std::map<std::string, ompl::base::GenericParamPtr> &ompl::base::ParamSet::getParams() const
167 {
168  return params_;
169 }
170 
171 const ompl::base::GenericParamPtr &ompl::base::ParamSet::getParam(const std::string &key) const
172 {
173  static GenericParamPtr empty;
174  auto it = params_.find(key);
175  if (it != params_.end())
176  return it->second;
177  return empty;
178 }
179 
180 void ompl::base::ParamSet::getParams(std::map<std::string, std::string> &params) const
181 {
182  for (const auto &param : params_)
183  params[param.first] = param.second->getValue();
184 }
185 
186 bool ompl::base::ParamSet::hasParam(const std::string &key) const
187 {
188  return params_.find(key) != params_.end();
189 }
190 
192 {
193  if (!hasParam(key))
194  throw Exception("Parameter '%s' is not defined", key);
195  return *getParam(key);
196 }
197 
198 void ompl::base::ParamSet::include(const ParamSet &other, const std::string &prefix)
199 {
200  const std::map<std::string, GenericParamPtr> &p = other.getParams();
201  if (prefix.empty())
202  for (const auto &it : p)
203  params_[it.first] = it.second;
204  else
205  for (const auto &it : p)
206  params_[prefix + "." + it.first] = it.second;
207 }
208 
209 void ompl::base::ParamSet::add(const GenericParamPtr &param)
210 {
211  params_[param->getName()] = param;
212 }
213 
214 void ompl::base::ParamSet::remove(const std::string &name)
215 {
216  params_.erase(name);
217 }
218 
220 {
221  params_.clear();
222 }
223 
224 void ompl::base::ParamSet::print(std::ostream &out) const
225 {
226  for (const auto &param : params_)
227  out << param.first << " = " << param.second->getValue() << std::endl;
228 }
void include(const ParamSet &other, const std::string &prefix="")
Include the params of a different ParamSet into this one. Optionally include a prefix for each of the...
Motion planning algorithms often employ parameters to guide their exploration process. (e.g., goal biasing). Motion planners (and some of their components) use this class to declare what the parameters are, in a generic way, so that they can be set externally.
Definition: GenericParam.h:65
bool getParam(const std::string &key, std::string &value) const
Get the value of the parameter named key. Store the value as string in value and return true if the p...
bool setParams(const std::map< std::string, std::string > &kv, bool ignoreUnknown=false)
Set the values for a set of parameters. The parameter names are the keys in the map kv...
T lexical_cast(const std::string &value) const
Helper function to convert strings into objects of type T.
double stod(const std::string &str)
convert string to double using classic "C" locale semantics
Definition: String.cpp:72
void getParamNames(std::vector< std::string > &params) const
List the names of the known parameters.
Maintain a set of parameters.
Definition: GenericParam.h:225
Main namespace. Contains everything in this library.
Definition: AppBase.h:21
void print(std::ostream &out) const
Print the parameters to a stream.
bool setParam(const std::string &key, const std::string &value)
Algorithms in OMPL often have parameters that can be set externally. While each algorithm will have t...
void remove(const std::string &name)
Remove a parameter from the set.
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
GenericParam & operator[](const std::string &key)
Access operator for parameters, by name. If the parameter is not defined, an exception is thrown...
void getParams(std::map< std::string, std::string > &params) const
Get the known parameter as a map from names to their values cast as string.
bool hasParam(const std::string &key) const
Check whether this set of parameters includes the parameter named key.
The exception type for ompl.
Definition: Exception.h:46
const std::map< std::string, GenericParamPtr > & getParams() const
Get the map from parameter names to parameter descriptions.
void add(const GenericParamPtr &param)
Add a parameter to the set.
long double stold(const std::string &str)
convert string to long double using classic "C" locale semantics
Definition: String.cpp:77
float stof(const std::string &str)
convert string to float using classic "C" locale semantics
Definition: String.cpp:67
void clear()
Clear all the set parameters.
void getParamValues(std::vector< std::string > &vals) const
List the values of the known parameters, in the same order as getParamNames()