interpolation_1d.cpp
Go to the documentation of this file.
00001 // *****************************************************************************
00002 //
00003 // Copyright (c) 2014, Southwest Research Institute® (SwRI®)
00004 // All rights reserved.
00005 //
00006 // Redistribution and use in source and binary forms, with or without
00007 // modification, are permitted provided that the following conditions are met:
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 Southwest Research Institute® (SwRI®) nor the
00014 //       names of its contributors may be used to endorse or promote products
00015 //       derived from 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 <COPYRIGHT HOLDER> BE LIABLE FOR ANY
00021 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00022 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00023 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00024 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00025 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00026 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
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     // We can accept new points if it is the first point or greater
00043     // than the last point.
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 &param_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 &param_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 }  // namespace swri_math_util


swri_math_util
Author(s): Marc Alban
autogenerated on Thu Jun 6 2019 20:34:41