utils.cpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  * Copyright (c) 2012, Southwest Research Institute
00005  * All rights reserved.
00006  *
00007  * Redistribution and use in source and binary forms, with or without
00008  * modification, are permitted provided that the following conditions are met:
00009  *
00010  *       * Redistributions of source code must retain the above copyright
00011  *       notice, this list of conditions and the following disclaimer.
00012  *       * Redistributions in binary form must reproduce the above copyright
00013  *       notice, this list of conditions and the following disclaimer in the
00014  *       documentation and/or other materials provided with the distribution.
00015  *       * Neither the name of the Southwest Research Institute, nor the names
00016  *       of its contributors may be used to endorse or promote products derived
00017  *       from this software without specific prior written permission.
00018  *
00019  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00020  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00021  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00022  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00023  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00024  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00025  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00026  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00027  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00028  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00029  * POSSIBILITY OF SUCH DAMAGE.
00030  */
00031 
00032 #include "ros/console.h"
00033 
00034 #include "industrial_utils/utils.h"
00035 #include "industrial_robot_client/utils.h"
00036 
00037 #include <cmath>
00038 #include <iostream>
00039 
00040 namespace industrial_robot_client
00041 {
00042 namespace utils
00043 {
00044 
00045 bool isWithinRange(const std::vector<double> & lhs, const std::vector<double> & rhs, double full_range)
00046 {
00047   bool rtn = false;
00048 
00049   if (lhs.size() != rhs.size())
00050   {
00051     ROS_ERROR_STREAM(__FUNCTION__ << "::lhs size: " << lhs.size() << " does not match rhs size: " << rhs.size());
00052     rtn = false;
00053   }
00054   else
00055   {
00056     // Calculating the half range causes some precision loss, but it's good enough
00057     double half_range = full_range / 2.0;
00058     rtn = true; // Assume within range, catch exception in loop below
00059 
00060     // This loop will not run for empty vectors, results in return of true
00061     for (size_t i = 0; i < lhs.size(); ++i)
00062     {
00063       if (fabs(lhs[i] - rhs[i]) > fabs(half_range))
00064       {
00065         rtn = false;
00066         break;
00067       }
00068     }
00069 
00070   }
00071 
00072   return rtn;
00073 }
00074 
00075 bool mapInsert(const std::string & key, double value, std::map<std::string, double> & mappings)
00076 {
00077   bool rtn = false;
00078 
00079   std::pair<std::map<std::string, double>::iterator, bool> insert_rtn;
00080 
00081   insert_rtn = mappings.insert(std::make_pair(key, value));
00082 
00083   // The second value returned form insert is a boolean (true for success)
00084   if (!insert_rtn.second)
00085   {
00086     ROS_ERROR_STREAM(__FUNCTION__ << "::Failed to insert item into map with key: " << key);
00087     rtn = false;
00088   }
00089   else
00090   {
00091     rtn = true;
00092   }
00093   return rtn;
00094 
00095 }
00096 
00097 bool toMap(const std::vector<std::string> & keys, const std::vector<double> & values,
00098            std::map<std::string, double> & mappings)
00099 {
00100   bool rtn;
00101 
00102   mappings.clear();
00103 
00104   if (keys.size() == values.size())
00105   {
00106     rtn = true;
00107 
00108     for (size_t i = 0; i < keys.size(); ++i)
00109     {
00110       rtn = mapInsert(keys[i], values[i], mappings);
00111       if (!rtn)
00112       {
00113         break;
00114       }
00115     }
00116 
00117   }
00118   else
00119   {
00120     ROS_ERROR_STREAM(__FUNCTION__ << "::keys size: " << keys.size()
00121                      << " does not match values size: " << values.size());
00122 
00123     rtn = false;
00124   }
00125 
00126   return rtn;
00127 }
00128 
00129 bool isWithinRange(const std::vector<std::string> & keys, const std::map<std::string, double> & lhs,
00130                    const std::map<std::string, double> & rhs, double full_range)
00131 {
00132   bool rtn = false;
00133 
00134   if ((keys.size() != rhs.size()) || (keys.size() != lhs.size()))
00135   {
00136     ROS_ERROR_STREAM(__FUNCTION__ << "::Size mistmatch ::lhs size: " << lhs.size() <<
00137                      " rhs size: " << rhs.size() << " key size: " << keys.size());
00138 
00139     rtn = false;
00140   }
00141   else
00142   {
00143     // Calculating the half range causes some precision loss, but it's good enough
00144     double half_range = full_range / 2.0;
00145     rtn = true; // Assume within range, catch exception in loop below
00146 
00147     // This loop will not run for empty vectors, results in return of true
00148     for (size_t i = 0; i < keys.size(); ++i)
00149     {
00150       if (fabs(lhs.at(keys[i]) - rhs.at(keys[i])) > fabs(half_range))
00151       {
00152         rtn = false;
00153         break;
00154       }
00155     }
00156 
00157   }
00158 
00159   return rtn;
00160 }
00161 
00162 bool isWithinRange(const std::vector<std::string> & lhs_keys, const std::vector<double> & lhs_values,
00163                    const std::vector<std::string> & rhs_keys, const std::vector<double> & rhs_values, double full_range)
00164 {
00165   bool rtn = false;
00166   std::map<std::string, double> lhs_map;
00167   std::map<std::string, double> rhs_map;
00168   if (industrial_utils::isSimilar(lhs_keys, rhs_keys))
00169   {
00170     if (toMap(lhs_keys, lhs_values, lhs_map) && toMap(rhs_keys, rhs_values, rhs_map))
00171     {
00172       rtn = isWithinRange(lhs_keys, lhs_map, rhs_map, full_range);
00173     }
00174   }
00175   else
00176   {
00177     ROS_ERROR_STREAM(__FUNCTION__ << "::Key vectors are not similar");
00178     rtn = false;
00179   }
00180   return rtn;
00181 }
00182 
00183 } //utils
00184 } //industrial_robot_client


industrial_robot_client
Author(s): Jeremy Zoss
autogenerated on Mon Oct 6 2014 00:55:19