ROSEndEffector
ROS End-Effector package: provides a ROS-based set of standard interfaces to command robotics end-effectors in an agnostic fashion.
Classes | Functions
ROSEE::Utils Namespace Reference

Classes

struct  DifferentKeysException
 
class  SecondOrderFilter
 
class  Timer
 

Functions

static bool create_directory (std::string pathDirectory)
 
static void out2file (std::string pathFile, std::string output)
 
static std::vector< std::string > getFilesInDir (std::string pathFolder)
 
static int binomial_coefficent (int n, int k)
 
static std::string getPackagePath ()
 
template<class KeyType , class ValueType >
static std::vector< KeyType > extract_keys (std::map< KeyType, ValueType > const &input_map)
 
template<class T >
static std::vector< std::string > extract_keys_merged (std::map< std::set< std::string >, T > const &input_map, unsigned int max_string_number=0)
 Extract all the string in the set keys of a map. More...
 
template<class T >
static std::vector< std::string > extract_keys_merged (std::map< std::pair< std::string, std::string >, T > const &input_map, unsigned int max_string_number=0)
 See above, this is the version with pair instead of set. More...
 
template<typename keyType , typename valueType1 , typename valueType2 >
bool keys_equal (std::map< keyType, valueType1 > const &lhs, std::map< keyType, valueType2 > const &rhs)
 Return false if two maps have different keys. More...
 
template<typename RetType , typename... Args>
std::unique_ptr< RetType > loadObject (std::string lib_name, std::string function_name, Args...args)
 Utils to dynamically load an object. More...
 
static std_msgs::Float32MultiArray eigenMatrixToFloat32MultiArray (Eigen::MatrixXd eigenMatrix)
 Utility to fill the Float32MultiArray ROS message from an eigen matrix. More...
 
static std::vector< float > eigenVectorToStdVector (Eigen::VectorXd eigenVector)
 
static Eigen::MatrixXd yamlMatrixToEigen (const YAML::Node &matrixNode)
 given a yaml node with a structure like More...
 
static Eigen::VectorXd yamlVectorToEigen (const YAML::Node &vectorNode)
 

Function Documentation

static int ROSEE::Utils::binomial_coefficent ( int  n,
int  k 
)
inlinestatic

Definition at line 77 of file Utils.h.

77  {
78 
79  if (k == 0 || k == n){
80  return 1;
81  }
82  return Utils::binomial_coefficent(n - 1, k - 1) + Utils::binomial_coefficent(n - 1, k);
83 
84 }
ROSEE::Utils::binomial_coefficent
static int binomial_coefficent(int n, int k)
Definition: Utils.h:77
static bool ROSEE::Utils::create_directory ( std::string  pathDirectory)
static

Definition at line 39 of file Utils.h.

39  {
40  boost::filesystem::path path(pathDirectory);
41  return boost::filesystem::create_directories(path);
42 }
static std_msgs::Float32MultiArray ROSEE::Utils::eigenMatrixToFloat32MultiArray ( Eigen::MatrixXd  eigenMatrix)
static

Utility to fill the Float32MultiArray ROS message from an eigen matrix.

The Float32MultiArray matrix will be stored in column major, independently from the eigen matrix that can be row major (even by default eigen matrix is column major, it can be templatizate specifing the row major)

Definition at line 19 of file UtilsEigen.h.

19  {
20 
21  std_msgs::Float32MultiArray rosMatrix;
22 
23  unsigned int nRow = eigenMatrix.rows();
24  unsigned int nCol = eigenMatrix.cols();
25  unsigned int size = nRow*nCol;
26 
27  if (size == 0){ //no reason to continue
28  std::cerr << "[Utils::eigenMatrixToFloat32MultiArray] eigenMatrix passed has size 0" << std::endl;
29  return rosMatrix;
30  }
31 
32  rosMatrix.layout.dim.push_back(std_msgs::MultiArrayDimension());
33  rosMatrix.layout.dim.push_back(std_msgs::MultiArrayDimension());
34  rosMatrix.layout.dim[0].label = "column";
35  rosMatrix.layout.dim[0].size = nCol;
36  rosMatrix.layout.dim[0].stride = nCol*nRow;
37 
38  rosMatrix.layout.dim[1].label = "row";
39  rosMatrix.layout.dim[1].size = nRow;
40  rosMatrix.layout.dim[1].stride = nRow;
41 
42  rosMatrix.layout.data_offset = 0;
43 
44  rosMatrix.data.resize(size);
45  int posRow = 0;
46  for (int iCol=0; iCol<nCol; iCol++){
47  for (int iRow=0; iRow<nRow; iRow++){
48  posRow = iCol*nRow;
49  rosMatrix.data.at(posRow + iRow) = eigenMatrix(iRow,iCol);
50  }
51  }
52 
53  return rosMatrix;
54 
55 
56 }
static std::vector<float> ROSEE::Utils::eigenVectorToStdVector ( Eigen::VectorXd  eigenVector)
static

Definition at line 58 of file UtilsEigen.h.

58  {
59 
60  std::vector<float> stdVect(eigenVector.data(), eigenVector.data() + eigenVector.size());
61 
62  return stdVect;
63 }
template<class KeyType , class ValueType >
static std::vector<KeyType> ROSEE::Utils::extract_keys ( std::map< KeyType, ValueType > const &  input_map)
static

Definition at line 95 of file Utils.h.

95  {
96  std::vector<KeyType> retval;
97  for (auto const& element : input_map) {
98  retval.push_back(element.first);
99  }
100  return retval;
101 }
template<class T >
static std::vector<std::string> ROSEE::Utils::extract_keys_merged ( std::map< std::set< std::string >, T > const &  input_map,
unsigned int  max_string_number = 0 
)
static

Extract all the string in the set keys of a map.

All string are put togheter so the original meaning of each set is lost

Parameters
input_mapthe map where extract the keys
max_string_numberthe max number of different string among all the set keys. Useful to not iterate all the map if not necessary. With default value = 0 all map is iterated.
Returns
vector of extracted string of set keys (string in this vect will be unique)

Definition at line 113 of file Utils.h.

114  {
115 
116  std::set<std::string> allStrings;
117  // if else so we do not check in the for the max_string_number if it is not used (ie ==0)
118 
119  if (max_string_number == 0) {
120  for (auto const& element : input_map) {
121  allStrings.insert( element.first.begin(), element.first.end() );
122  }
123 
124  } else {
125  for (auto const& element : input_map) {
126  allStrings.insert(element.first.begin(), element.first.end());
127  if (max_string_number == allStrings.size()){
128  break;
129  }
130  if (max_string_number < allStrings.size() ) {
131  std::cerr << "[ERROR]" << __func__ << " You passed " << max_string_number
132  << " but I found more unique strings in the set keys ( " << allStrings.size()
133  << " found)" << std::endl;
134  return std::vector<std::string>();
135  }
136  }
137  }
138  std::vector<std::string> retval (allStrings.begin(), allStrings.end());
139  return retval;
140 }
template<class T >
static std::vector<std::string> ROSEE::Utils::extract_keys_merged ( std::map< std::pair< std::string, std::string >, T > const &  input_map,
unsigned int  max_string_number = 0 
)
static

See above, this is the version with pair instead of set.

Definition at line 146 of file Utils.h.

147  {
148 
149  std::set<std::string> allStrings;
150  // if else so we do not check in the for the max_string_number if it is not used (ie ==0)
151 
152  if (max_string_number == 0) {
153  for (auto const& element : input_map) {
154  allStrings.insert( element.first.first);
155  allStrings.insert( element.first.second);
156  }
157 
158  } else {
159  for (auto const& element : input_map) {
160  allStrings.insert( element.first.first);
161  allStrings.insert( element.first.second);
162  if (max_string_number == allStrings.size()){
163  break;
164  }
165  if (max_string_number < allStrings.size() ) {
166  std::cerr << "[ERROR]" << __func__ << " You passed " << max_string_number
167  << " but I found more unique strings in the pair keys ( " << allStrings.size()
168  << " found)" << std::endl;
169  return std::vector<std::string>();
170  }
171  }
172  }
173  std::vector<std::string> retval (allStrings.begin(), allStrings.end());
174  return retval;
175 }
static std::vector<std::string> ROSEE::Utils::getFilesInDir ( std::string  pathFolder)
static

Definition at line 55 of file Utils.h.

55  {
56 
57  boost::filesystem::path p (pathFolder);
58  std::vector <std::string> retVect;
59 
60  if (! boost::filesystem::exists(p) ) {
61  std::cerr << "[ERROR " << __func__ << "] path '" << pathFolder << "' does not exists" << std::endl;
62  return retVect;
63  }
64 
65  if (! boost::filesystem::is_directory(p)){
66  std::cerr << "[ERROR " << __func__ << "] path '" << pathFolder << "' is not a directory" << std::endl;
67  return retVect;
68  }
69 
70  for (boost::filesystem::directory_entry& x : boost::filesystem::directory_iterator(p)) {
71  retVect.push_back (x.path().filename().string() );
72  }
73 
74  return retVect;
75 }
static std::string ROSEE::Utils::getPackagePath ( )
static

Definition at line 87 of file Utils.h.

87  {
88 
89  boost::filesystem::path path(__FILE__);
90  path.remove_filename();
91  return path.string() + "/../../";
92 }
template<typename keyType , typename valueType1 , typename valueType2 >
bool ROSEE::Utils::keys_equal ( std::map< keyType, valueType1 > const &  lhs,
std::map< keyType, valueType2 > const &  rhs 
)

Return false if two maps have different keys.

The type of the keys (typename) must be the same obviously, but the values (valueType1 and valueType2) can be anything, because they are not considered

Definition at line 182 of file Utils.h.

182  {
183 
184  auto pred = [] (decltype(*lhs.begin()) a, decltype(*rhs.begin()) b)
185  { return (a.first == b.first); };
186 
187 
188  return lhs.size() == rhs.size()
189  && std::equal(lhs.begin(), lhs.end(), rhs.begin(), pred);
190 }
template<typename RetType , typename... Args>
std::unique_ptr<RetType> ROSEE::Utils::loadObject ( std::string  lib_name,
std::string  function_name,
Args...  args 
)

Utils to dynamically load an object.

This is used to dynamically load a derived object from a node that only knows the base interface. For example, we call the create_object(ros::nodeHandle) method of a derived EEHAL class The object must be a library which will return a RetType pointer with the function_name This function will "convert" to smart pointer for convenience

Parameters
lib_namethe name of the compiled library (eg DummyHal). Do not add the suffix .so
function_nameThe method of
lib_namewhich will return a RetType*.
argsarguments for the function_name , if the case
Returns
std::shared_ptr<RetType> a pointer to the new created object

Definition at line 228 of file Utils.h.

230  {
231 
232  if (lib_name.empty()) {
233 
234  std::cerr << "[Utils::loadObject] ERROR: Please specify lib_name" << std::endl;
235  return nullptr;
236  }
237 
238  std::string lib_name_path = "lib" + lib_name +".so";
239 
240  //clear old errors
241  dlerror();
242 
243  void* lib_handle = dlopen(lib_name_path.c_str(), RTLD_LAZY);
244  auto error = dlerror();
245 
246  if (!lib_handle || error != NULL) {
247  std::cerr << "[Utils::loadObject] ERROR in opening the library: " << error << std::endl;
248  return nullptr;
249  }
250 
251  //clear old errors
252  dlerror();
253 
254  RetType* (*function)(Args... args);
255  function = reinterpret_cast<RetType* (*)(Args... args)>(dlsym(lib_handle, function_name.c_str()));
256  error = dlerror();
257  if ( error != NULL) {
258  std::cerr << "[Utils::loadObject] ERROR in returning the function: " << error << std::endl;
259  return nullptr;
260  }
261 
262  RetType* objectRaw = function(args...);
263 
264  std::unique_ptr<RetType> objectPtr(objectRaw);
265 
266  dlclose(lib_handle);
267 
268  return objectPtr;
269 }
static void ROSEE::Utils::out2file ( std::string  pathFile,
std::string  output 
)
static

Definition at line 44 of file Utils.h.

44  {
45  std::ofstream fout ( pathFile );
46  fout << output;
47 }
static Eigen::MatrixXd ROSEE::Utils::yamlMatrixToEigen ( const YAML::Node &  matrixNode)
static

given a yaml node with a structure like

  • [1, 2, 3]
  • [4, 5, 6] "Convert" this structure into a eigen matrix
Todo:
is there a better way to do this?

Definition at line 18 of file UtilsYAML.h.

18  {
19 
20  //note: they say adding row by row to eigen is tremendously slow,
21  // so we use vector
22  std::vector<std::vector<float>> stdMat;
23  stdMat = matrixNode.as<std::vector<std::vector<float>>>();
24 
25  Eigen::MatrixXd eigenMat(stdMat.size(), stdMat.at(0).size());
26 
27  for (int iRow = 0; iRow<stdMat.size(); iRow++) {
28  for (int iCol = 0; iCol<stdMat.at(0).size(); iCol++) {
29  eigenMat(iRow, iCol) = stdMat.at(iRow).at(iCol);
30  }
31  }
32 
33  return eigenMat;
34 
35 }
static Eigen::VectorXd ROSEE::Utils::yamlVectorToEigen ( const YAML::Node &  vectorNode)
static

Definition at line 37 of file UtilsYAML.h.

37  {
38 
39  //note: they say adding row by row to eigen is tremendously slow,
40  // so we use vector
41  std::vector<float> stdVect;
42  stdVect = vectorNode.as<std::vector<float>>();
43 
44  Eigen::VectorXd eigenVec(stdVect.size());
45 
46  for (int i = 0; i<stdVect.size(); i++) {
47  eigenVec(i) = stdVect.at(i);
48  }
49 
50  return eigenVec;
51 
52 }
Generated on Tue Dec 10 2024 14:57:14 for ROSEndEffector by   doxygen 1.8.11