Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029 #include <swri_math_util/interpolation_1d.h>
00030
00031 namespace swri_math_util
00032 {
00033 Interpolation1D::Interpolation1D()
00034 :
00035 interp_type_(ZERO_ORDER_HOLD)
00036 {
00037 }
00038
00039 bool Interpolation1D::appendPoint(double x, double y)
00040 {
00041 if (x_.size() == 0 || x > x_.back()) {
00042
00043
00044 x_.push_back(x);
00045 y_.push_back(y);
00046 return true;
00047 } else {
00048 ROS_ERROR("Error appending new point. "
00049 "X values must be increasing. (%f <= %f)",
00050 x, x_.back());
00051 return false;
00052 }
00053 }
00054
00055 size_t Interpolation1D::numPoints() const
00056 {
00057 return x_.size();
00058 }
00059
00060 std::pair<double, double> Interpolation1D::getPoint(size_t index) const
00061 {
00062 if (index < x_.size()) {
00063 return std::pair<double, double>(x_[index], y_[index]);
00064 } else {
00065 ROS_ERROR("Invalid index in getPoint (index=%zu, numPoints=%zu)",
00066 index, x_.size());
00067 return std::pair<double, double>(0.0, 0.0);
00068 }
00069 }
00070
00071 void Interpolation1D::removePoint(size_t index)
00072 {
00073 if (index < x_.size()) {
00074 x_.erase(x_.begin()+index);
00075 y_.erase(y_.begin()+index);
00076 } else {
00077 ROS_ERROR("Invalid index in removePoint (index=%zu, numPoints=%zu)",
00078 index, x_.size());
00079 }
00080 }
00081
00082 void Interpolation1D::clear()
00083 {
00084 x_.clear();
00085 y_.clear();
00086 }
00087
00088 Interpolation1D::InterpolationType Interpolation1D::interpolationType()
00089 {
00090 return interp_type_;
00091 }
00092
00093 std::string Interpolation1D::interpolationTypeString() const
00094 {
00095 if (interp_type_ == ZERO_ORDER_HOLD) {
00096 return "zero_order_hold";
00097 } else if (interp_type_ == LINEAR) {
00098 return "linear";
00099 } else {
00100 return "<unknown>";
00101 }
00102 }
00103
00104 void Interpolation1D::setInterpolationType(InterpolationType type)
00105 {
00106 interp_type_ = type;
00107 }
00108
00109 bool Interpolation1D::readFromParameter(
00110 const ros::NodeHandle &node_handle,
00111 const std::string ¶m_name,
00112 bool error_if_missing)
00113 {
00114 std::string resolved_name = node_handle.resolveName(param_name);
00115
00116 XmlRpc::XmlRpcValue curve_param;
00117 if (node_handle.getParam(param_name, curve_param)) {
00118 return readFromParameter(curve_param, param_name);
00119 } else {
00120 if (error_if_missing) {
00121 ROS_ERROR("Missing required parameter at '%s'.", resolved_name.c_str());
00122 return false;
00123 } else {
00124 return true;
00125 }
00126 }
00127 }
00128
00129 bool Interpolation1D::readFromParameter(
00130 XmlRpc::XmlRpcValue &curve_param,
00131 const std::string ¶m_name)
00132 {
00133 if (curve_param.getType() != XmlRpc::XmlRpcValue::TypeStruct) {
00134 ROS_ERROR("Parameter '%s' must be a structure.", param_name.c_str());
00135 return false;
00136 }
00137
00138 bool error_occurred = false;
00139
00140 if (curve_param.hasMember("interpolation_type")) {
00141 std::string interp_type = static_cast<std::string>(curve_param["interpolation_type"]);
00142
00143 if (interp_type == "zero_order_hold") {
00144 setInterpolationType(ZERO_ORDER_HOLD);
00145 } else if (interp_type == "linear") {
00146 setInterpolationType(LINEAR);
00147 } else {
00148 error_occurred = true;
00149 ROS_ERROR("Invalid interpolation type '%s' at '%s'.",
00150 interp_type.c_str(), param_name.c_str());
00151 }
00152 } else {
00153 ROS_INFO("No 'interpolation_type' found in %s. "
00154 "Defaulting to zero_order_hold.",
00155 param_name.c_str());
00156 setInterpolationType(ZERO_ORDER_HOLD);
00157 }
00158
00159 if (!curve_param.hasMember("values")) {
00160 ROS_INFO("Missing required parameter '%s/values'.",
00161 param_name.c_str());
00162 return false;
00163 }
00164
00165 XmlRpc::XmlRpcValue param_value = curve_param["values"];
00166 if (param_value.getType() != XmlRpc::XmlRpcValue::TypeArray)
00167 {
00168 ROS_ERROR("Parameter '%s/values' must be an array.", param_name.c_str());
00169 return false;
00170 }
00171
00172 for (int i = 0; i < param_value.size(); i++)
00173 {
00174 XmlRpc::XmlRpcValue point_value = param_value[i];
00175
00176 if (point_value.getType() != XmlRpc::XmlRpcValue::TypeArray)
00177 {
00178 ROS_ERROR("Parameter '%s/values[%d]' must be an array.",
00179 param_name.c_str(), i);
00180 error_occurred = true;
00181 continue;
00182 }
00183
00184 if (point_value.size() != 2) {
00185 ROS_ERROR("Parameter '%s/values[%d]' must be 2 elements.",
00186 param_name.c_str(), i);
00187 error_occurred = true;
00188 continue;
00189 }
00190
00191 XmlRpc::XmlRpcValue x_param = point_value[0];
00192 XmlRpc::XmlRpcValue y_param = point_value[1];
00193 double x_value = std::numeric_limits<double>::quiet_NaN();
00194 double y_value = std::numeric_limits<double>::quiet_NaN();
00195
00196 if (x_param.getType() != XmlRpc::XmlRpcValue::TypeDouble &&
00197 x_param.getType() != XmlRpc::XmlRpcValue::TypeInt)
00198 {
00199 ROS_ERROR("Parameters '%s/values[%d][0] must be a double or an integer.",
00200 param_name.c_str(), i);
00201 error_occurred = true;
00202 } else {
00203 x_value = static_cast<double>(x_param);
00204 }
00205
00206 if (y_param.getType() != XmlRpc::XmlRpcValue::TypeDouble &&
00207 y_param.getType() != XmlRpc::XmlRpcValue::TypeInt)
00208 {
00209 ROS_ERROR("Parameters '%s/values[%d][1] must be a double or an integer.",
00210 param_name.c_str(), i);
00211 error_occurred = true;
00212 } else {
00213 y_value = static_cast<double>(y_param);
00214 }
00215
00216 if (!error_occurred) {
00217 if (!appendPoint(x_value, y_value)) {
00218 ROS_ERROR("Failed to add point %s/values[%d].",
00219 param_name.c_str(), i);
00220 error_occurred = true;
00221 }
00222 }
00223 }
00224
00225 if (error_occurred) {
00226 clear();
00227 return false;
00228 } else {
00229 return true;
00230 }
00231 }
00232
00233 double Interpolation1D::minX() const
00234 {
00235 if (x_.size() == 0) {
00236 return 0.0;
00237 } else {
00238 return x_.front();
00239 }
00240 }
00241
00242 double Interpolation1D::maxX() const
00243 {
00244 if (x_.size() == 0) {
00245 return 0.0;
00246 } else {
00247 return x_.back();
00248 }
00249 }
00250 }