utils.cpp
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2012, Southwest Research Institute
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 are met:
9  *
10  * * Redistributions of source code must retain the above copyright
11  * notice, this list of conditions and the following disclaimer.
12  * * Redistributions in binary form must reproduce the above copyright
13  * notice, this list of conditions and the following disclaimer in the
14  * documentation and/or other materials provided with the distribution.
15  * * Neither the name of the Southwest Research Institute, nor the names
16  * of its contributors may be used to endorse or promote products derived
17  * from this software without specific prior written permission.
18  *
19  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
20  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
21  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
22  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
23  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
24  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
25  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
26  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
27  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
28  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
29  * POSSIBILITY OF SUCH DAMAGE.
30  */
31 
32 #include "ros/console.h"
33 
34 #include "industrial_utils/utils.h"
36 
37 #include <cmath>
38 #include <iostream>
39 
41 {
42 namespace utils
43 {
44 
45 bool isWithinRange(const std::vector<double> & lhs, const std::vector<double> & rhs, double full_range)
46 {
47  bool rtn = false;
48 
49  if (lhs.size() != rhs.size())
50  {
51  ROS_ERROR_STREAM(__FUNCTION__ << "::lhs size: " << lhs.size() << " does not match rhs size: " << rhs.size());
52  rtn = false;
53  }
54  else
55  {
56  // Calculating the half range causes some precision loss, but it's good enough
57  double half_range = full_range / 2.0;
58  rtn = true; // Assume within range, catch exception in loop below
59 
60  // This loop will not run for empty vectors, results in return of true
61  for (size_t i = 0; i < lhs.size(); ++i)
62  {
63  if (fabs(lhs[i] - rhs[i]) > fabs(half_range))
64  {
65  rtn = false;
66  break;
67  }
68  }
69 
70  }
71 
72  return rtn;
73 }
74 
75 bool mapInsert(const std::string & key, double value, std::map<std::string, double> & mappings)
76 {
77  bool rtn = false;
78 
79  std::pair<std::map<std::string, double>::iterator, bool> insert_rtn;
80 
81  insert_rtn = mappings.insert(std::make_pair(key, value));
82 
83  // The second value returned form insert is a boolean (true for success)
84  if (!insert_rtn.second)
85  {
86  ROS_ERROR_STREAM(__FUNCTION__ << "::Failed to insert item into map with key: " << key);
87  rtn = false;
88  }
89  else
90  {
91  rtn = true;
92  }
93  return rtn;
94 
95 }
96 
97 bool toMap(const std::vector<std::string> & keys, const std::vector<double> & values,
98  std::map<std::string, double> & mappings)
99 {
100  bool rtn;
101 
102  mappings.clear();
103 
104  if (keys.size() == values.size())
105  {
106  rtn = true;
107 
108  for (size_t i = 0; i < keys.size(); ++i)
109  {
110  rtn = mapInsert(keys[i], values[i], mappings);
111  if (!rtn)
112  {
113  break;
114  }
115  }
116 
117  }
118  else
119  {
120  ROS_ERROR_STREAM(__FUNCTION__ << "::keys size: " << keys.size()
121  << " does not match values size: " << values.size());
122 
123  rtn = false;
124  }
125 
126  return rtn;
127 }
128 
129 bool isWithinRange(const std::vector<std::string> & keys, const std::map<std::string, double> & lhs,
130  const std::map<std::string, double> & rhs, double full_range)
131 {
132  bool rtn = false;
133 
134  if ((keys.size() != rhs.size()) || (keys.size() != lhs.size()))
135  {
136  ROS_ERROR_STREAM(__FUNCTION__ << "::Size mistmatch ::lhs size: " << lhs.size() <<
137  " rhs size: " << rhs.size() << " key size: " << keys.size());
138 
139  rtn = false;
140  }
141  else
142  {
143  // Calculating the half range causes some precision loss, but it's good enough
144  double half_range = full_range / 2.0;
145  rtn = true; // Assume within range, catch exception in loop below
146 
147  // This loop will not run for empty vectors, results in return of true
148  for (size_t i = 0; i < keys.size(); ++i)
149  {
150  if (fabs(lhs.at(keys[i]) - rhs.at(keys[i])) > fabs(half_range))
151  {
152  rtn = false;
153  break;
154  }
155  }
156 
157  }
158 
159  return rtn;
160 }
161 
162 bool isWithinRange(const std::vector<std::string> & lhs_keys, const std::vector<double> & lhs_values,
163  const std::vector<std::string> & rhs_keys, const std::vector<double> & rhs_values, double full_range)
164 {
165  bool rtn = false;
166  std::map<std::string, double> lhs_map;
167  std::map<std::string, double> rhs_map;
168  if (industrial_utils::isSimilar(lhs_keys, rhs_keys))
169  {
170  if (toMap(lhs_keys, lhs_values, lhs_map) && toMap(rhs_keys, rhs_values, rhs_map))
171  {
172  rtn = isWithinRange(lhs_keys, lhs_map, rhs_map, full_range);
173  }
174  }
175  else
176  {
177  ROS_ERROR_STREAM(__FUNCTION__ << "::Key vectors are not similar");
178  rtn = false;
179  }
180  return rtn;
181 }
182 
183 } //utils
184 } //industrial_robot_client
ROS_ERROR_STREAM
#define ROS_ERROR_STREAM(args)
utils.h
industrial_robot_client::utils::mapInsert
bool mapInsert(const std::string &key, double value, std::map< std::string, double > &mappings)
Convenience function for inserting a value into an std::map, assuming a string key and a double value...
Definition: utils.cpp:75
industrial_utils::isSimilar
bool isSimilar(std::vector< std::string > lhs, std::vector< std::string > rhs)
console.h
utils.h
industrial_robot_client::utils::toMap
bool toMap(const std::vector< std::string > &keys, const std::vector< double > &values, std::map< std::string, double > &mappings)
Convenience function for taking two separate key,value vectors and creating the more convenient map....
Definition: utils.cpp:97
industrial_robot_client::utils::isWithinRange
bool isWithinRange(const std::vector< double > &lhs, const std::vector< double > &rhs, double full_range)
Checks to see if members are within the same range. Specifically, Each item in abs(lhs[i] - rhs[i]) <...
Definition: utils.cpp:45
industrial_robot_client
Definition: joint_relay_handler.h:46
values
std::vector< double > values


industrial_robot_client
Author(s): Jeremy Zoss
autogenerated on Wed Mar 2 2022 00:24:59