interpolation_1d.cpp
Go to the documentation of this file.
1 // *****************************************************************************
2 //
3 // Copyright (c) 2014, Southwest Research Institute® (SwRI®)
4 // All rights reserved.
5 //
6 // Redistribution and use in source and binary forms, with or without
7 // modification, are permitted provided that the following conditions are met:
8 // * Redistributions of source code must retain the above copyright
9 // notice, this list of conditions and the following disclaimer.
10 // * Redistributions in binary form must reproduce the above copyright
11 // notice, this list of conditions and the following disclaimer in the
12 // documentation and/or other materials provided with the distribution.
13 // * Neither the name of Southwest Research Institute® (SwRI®) nor the
14 // names of its contributors may be used to endorse or promote products
15 // derived from this software without specific prior written permission.
16 //
17 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20 // ARE DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
21 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
22 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
23 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
24 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
25 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
26 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
27 //
28 // *****************************************************************************
30 
31 namespace swri_math_util
32 {
34  :
35  interp_type_(ZERO_ORDER_HOLD)
36 {
37 }
38 
39 bool Interpolation1D::appendPoint(double x, double y)
40 {
41  if (x_.size() == 0 || x > x_.back()) {
42  // We can accept new points if it is the first point or greater
43  // than the last point.
44  x_.push_back(x);
45  y_.push_back(y);
46  return true;
47  } else {
48  ROS_ERROR("Error appending new point. "
49  "X values must be increasing. (%f <= %f)",
50  x, x_.back());
51  return false;
52  }
53 }
54 
56 {
57  return x_.size();
58 }
59 
60 std::pair<double, double> Interpolation1D::getPoint(size_t index) const
61 {
62  if (index < x_.size()) {
63  return std::pair<double, double>(x_[index], y_[index]);
64  } else {
65  ROS_ERROR("Invalid index in getPoint (index=%zu, numPoints=%zu)",
66  index, x_.size());
67  return std::pair<double, double>(0.0, 0.0);
68  }
69 }
70 
71 void Interpolation1D::removePoint(size_t index)
72 {
73  if (index < x_.size()) {
74  x_.erase(x_.begin()+index);
75  y_.erase(y_.begin()+index);
76  } else {
77  ROS_ERROR("Invalid index in removePoint (index=%zu, numPoints=%zu)",
78  index, x_.size());
79  }
80 }
81 
83 {
84  x_.clear();
85  y_.clear();
86 }
87 
89 {
90  return interp_type_;
91 }
92 
94 {
96  return "zero_order_hold";
97  } else if (interp_type_ == LINEAR) {
98  return "linear";
99  } else {
100  return "<unknown>";
101  }
102 }
103 
105 {
106  interp_type_ = type;
107 }
108 
110  const ros::NodeHandle &node_handle,
111  const std::string &param_name,
112  bool error_if_missing)
113 {
114  std::string resolved_name = node_handle.resolveName(param_name);
115 
116  XmlRpc::XmlRpcValue curve_param;
117  if (node_handle.getParam(param_name, curve_param)) {
118  return readFromParameter(curve_param, param_name);
119  } else {
120  if (error_if_missing) {
121  ROS_ERROR("Missing required parameter at '%s'.", resolved_name.c_str());
122  return false;
123  } else {
124  return true;
125  }
126  }
127 }
128 
130  XmlRpc::XmlRpcValue &curve_param,
131  const std::string &param_name)
132 {
133  if (curve_param.getType() != XmlRpc::XmlRpcValue::TypeStruct) {
134  ROS_ERROR("Parameter '%s' must be a structure.", param_name.c_str());
135  return false;
136  }
137 
138  bool error_occurred = false;
139 
140  if (curve_param.hasMember("interpolation_type")) {
141  std::string interp_type = static_cast<std::string>(curve_param["interpolation_type"]);
142 
143  if (interp_type == "zero_order_hold") {
145  } else if (interp_type == "linear") {
147  } else {
148  error_occurred = true;
149  ROS_ERROR("Invalid interpolation type '%s' at '%s'.",
150  interp_type.c_str(), param_name.c_str());
151  }
152  } else {
153  ROS_INFO("No 'interpolation_type' found in %s. "
154  "Defaulting to zero_order_hold.",
155  param_name.c_str());
157  }
158 
159  if (!curve_param.hasMember("values")) {
160  ROS_INFO("Missing required parameter '%s/values'.",
161  param_name.c_str());
162  return false;
163  }
164 
165  XmlRpc::XmlRpcValue param_value = curve_param["values"];
166  if (param_value.getType() != XmlRpc::XmlRpcValue::TypeArray)
167  {
168  ROS_ERROR("Parameter '%s/values' must be an array.", param_name.c_str());
169  return false;
170  }
171 
172  for (int i = 0; i < param_value.size(); i++)
173  {
174  XmlRpc::XmlRpcValue point_value = param_value[i];
175 
176  if (point_value.getType() != XmlRpc::XmlRpcValue::TypeArray)
177  {
178  ROS_ERROR("Parameter '%s/values[%d]' must be an array.",
179  param_name.c_str(), i);
180  error_occurred = true;
181  continue;
182  }
183 
184  if (point_value.size() != 2) {
185  ROS_ERROR("Parameter '%s/values[%d]' must be 2 elements.",
186  param_name.c_str(), i);
187  error_occurred = true;
188  continue;
189  }
190 
191  XmlRpc::XmlRpcValue x_param = point_value[0];
192  XmlRpc::XmlRpcValue y_param = point_value[1];
193  double x_value = std::numeric_limits<double>::quiet_NaN();
194  double y_value = std::numeric_limits<double>::quiet_NaN();
195 
196  if (x_param.getType() != XmlRpc::XmlRpcValue::TypeDouble &&
198  {
199  ROS_ERROR("Parameters '%s/values[%d][0] must be a double or an integer.",
200  param_name.c_str(), i);
201  error_occurred = true;
202  } else {
203  x_value = static_cast<double>(x_param);
204  }
205 
206  if (y_param.getType() != XmlRpc::XmlRpcValue::TypeDouble &&
208  {
209  ROS_ERROR("Parameters '%s/values[%d][1] must be a double or an integer.",
210  param_name.c_str(), i);
211  error_occurred = true;
212  } else {
213  y_value = static_cast<double>(y_param);
214  }
215 
216  if (!error_occurred) {
217  if (!appendPoint(x_value, y_value)) {
218  ROS_ERROR("Failed to add point %s/values[%d].",
219  param_name.c_str(), i);
220  error_occurred = true;
221  }
222  }
223  }
224 
225  if (error_occurred) {
226  clear();
227  return false;
228  } else {
229  return true;
230  }
231 }
232 
233 double Interpolation1D::minX() const
234 {
235  if (x_.size() == 0) {
236  return 0.0;
237  } else {
238  return x_.front();
239  }
240 }
241 
242 double Interpolation1D::maxX() const
243 {
244  if (x_.size() == 0) {
245  return 0.0;
246  } else {
247  return x_.back();
248  }
249 }
250 } // namespace swri_math_util
bool appendPoint(double x, double y)
std::string interpolationTypeString() const
void setInterpolationType(InterpolationType type)
Type const & getType() const
#define ROS_INFO(...)
bool getParam(const std::string &key, std::string &s) const
std::string resolveName(const std::string &name, bool remap=true) const
std::pair< double, double > getPoint(size_t index) const
#define ROS_ERROR(...)
bool readFromParameter(const ros::NodeHandle &node_handle, const std::string &param_name, bool error_if_missing)
bool hasMember(const std::string &name) const
InterpolationType interpolationType()


swri_math_util
Author(s): Marc Alban
autogenerated on Sat Jan 21 2023 03:13:11