test_hand_finder.cpp
Go to the documentation of this file.
1 /*
2  * File: test_hand_finder.cpp
3  * Author: Vahid Aminzadeh <vahid@shadowrobot.com>
4  * Copyright:
5  *
6  * @brief see README.md
7  */
8 #include <gtest/gtest.h>
9 #include "ros/ros.h"
10 #include <ros/package.h>
11 #include <utility>
12 #include <iostream>
13 #include <map>
14 #include <vector>
15 #include <string>
16 #include "sr_utilities/sr_hand_finder.hpp"
17 #include <boost/foreach.hpp>
18 
19 using std::vector;
20 using std::map;
21 using std::string;
22 using std::pair;
23 
24 void ASSERT_MAP_HAS_VALUE(const map<string, string> &map, const string &value)
25 {
26  bool found = false;
27  pair<string, string> item;
28  BOOST_FOREACH(item, map)
29  {
30  if (value == item.second)
31  {
32  found = true;
33  }
34  }
35 
36  ASSERT_TRUE(found);
37 }
38 
39 TEST(SrHandFinder, hand_absent_test)
40 {
41  if (ros::param::has("hand"))
42  {
43  ros::param::del("hand");
44  }
45 
46  if (ros::param::has("robot_description"))
47  {
48  ros::param::del("robot_description");
49  }
50 
51  shadow_robot::SrHandFinder hand_finder;
52  map<string, vector<string> > hand_joints(hand_finder.get_joints());
53  ASSERT_EQ(hand_joints.size(), 0);
54  map<string, string> calibration_path(hand_finder.get_calibration_path());
55  ASSERT_EQ(calibration_path.size(), 0);
56  shadow_robot::HandControllerTuning controller_tuning(hand_finder.get_hand_controller_tuning());
57  ASSERT_EQ(controller_tuning.friction_compensation_.size(), 0);
58  ASSERT_EQ(controller_tuning.host_control_.size(), 0);
59  ASSERT_EQ(controller_tuning.motor_control_.size(), 0);
60 }
61 
62 TEST(SrHandFinder, one_hand_no_robot_description_finder_test)
63 {
64  if (ros::param::has("hand"))
65  {
66  ros::param::del("hand");
67  }
68 
69  if (ros::param::has("robot_description"))
70  {
71  ros::param::del("robot_description");
72  }
73 
74  ros::param::set("hand/mapping/1", "right");
75  ros::param::set("hand/joint_prefix/1", "rh_");
76 
77  string ethercat_path = ros::package::getPath("sr_ethercat_hand_config");
78  const string joint_names[] =
79  {
80  "FFJ1", "FFJ2", "FFJ3", "FFJ4", "MFJ1", "MFJ2", "MFJ3", "MFJ4",
81  "RFJ1", "RFJ2", "RFJ3", "RFJ4", "LFJ1", "LFJ2", "LFJ3", "LFJ4", "LFJ5",
82  "THJ1", "THJ2", "THJ3", "THJ4", "THJ5", "WRJ1", "WRJ2"
83  };
84 
85  shadow_robot::SrHandFinder hand_finder;
86  // hand parameters
87  ASSERT_GT(hand_finder.get_hand_parameters().mapping_.size(), 0);
88  ASSERT_GT(hand_finder.get_hand_parameters().joint_prefix_.size(), 0);
89  ASSERT_EQ(hand_finder.get_hand_parameters().mapping_["1"], "right");
90  ASSERT_EQ(hand_finder.get_hand_parameters().joint_prefix_["1"], "rh_");
91 
92  // hand joints
93  ASSERT_EQ(hand_finder.get_joints().size(), 1);
94  ASSERT_GT(hand_finder.get_joints().count("right"), 0);
95  const vector<string> rh_joints = hand_finder.get_joints().at("right");
96  ASSERT_EQ(rh_joints.size(), 24);
97  for (size_t i = 0; i < rh_joints.size(); ++i)
98  {
99  ROS_DEBUG_STREAM(rh_joints[i]);
100  ASSERT_STREQ(rh_joints[i].c_str(), ("rh_" + joint_names[i]).c_str());
101  }
102 
103  // calibration
104  ASSERT_GT(hand_finder.get_calibration_path().size(), 0);
105  const string calibration_path = hand_finder.get_calibration_path().at("right");
106  ROS_DEBUG_STREAM(calibration_path.c_str());
107  ASSERT_STREQ(calibration_path.c_str(), (ethercat_path + "/calibrations/right/" + "calibration.yaml").c_str());
108 
109  // tuning
110  shadow_robot::HandControllerTuning controller_tuning(hand_finder.get_hand_controller_tuning());
111 
112  for (map<string, string>::const_iterator iter = controller_tuning.friction_compensation_.begin();
113  iter != controller_tuning.friction_compensation_.end(); ++iter)
114  {
115  ASSERT_STREQ(iter->second.c_str(), (ethercat_path + "/controls/friction_compensation.yaml").c_str());
116  ROS_DEBUG_STREAM(iter->second);
117  }
118  for (map<string, string>::const_iterator iter = controller_tuning.motor_control_.begin();
119  iter != controller_tuning.motor_control_.end(); ++iter)
120  {
121  ASSERT_STREQ(iter->second.c_str(),
122  (ethercat_path + "/controls/motors/right/motor_board_effort_controllers.yaml").c_str());
123  ROS_DEBUG_STREAM(iter->second);
124  }
125  for (map<string, vector<string> >::const_iterator iter = controller_tuning.host_control_.begin();
126  iter != controller_tuning.host_control_.end(); ++iter)
127  {
128  string host_array[] =
129  {
130  "sr_edc_calibration_controllers.yaml",
131  "sr_edc_joint_velocity_controllers_PWM.yaml",
132  "sr_edc_effort_controllers_PWM.yaml",
133  "sr_edc_joint_velocity_controllers.yaml",
134  "sr_edc_effort_controllers.yaml",
135  "sr_edc_mixed_position_velocity_joint_controllers_PWM.yaml",
136  "sr_edc_joint_position_controllers_PWM.yaml",
137  "sr_edc_mixed_position_velocity_joint_controllers.yaml",
138  "sr_edc_joint_position_controllers.yaml"
139  };
140  vector<string> host_controller_files(host_array, host_array + 9);
141  ASSERT_EQ(host_controller_files.size(), iter->second.size());
142  for (size_t i = 0; i != iter->second.size(); ++i)
143  {
144  ASSERT_STREQ(iter->second[i].c_str(), (ethercat_path + "/controls/host/right/" +
145  host_controller_files[i]).c_str());
146  ROS_DEBUG_STREAM(iter->second[i]);
147  }
148  }
149 }
150 
151 TEST(SrHandFinder, one_hand_no_mapping_no_robot_description_finder_test)
152 {
153  if (ros::param::has("hand"))
154  {
155  ros::param::del("hand");
156  }
157 
158  if (ros::param::has("robot_description"))
159  {
160  ros::param::del("robot_description");
161  }
162 
163  ros::param::set("hand/mapping/1", "");
164  ros::param::set("hand/joint_prefix/1", "rh_");
165 
166  string ethercat_path = ros::package::getPath("sr_ethercat_hand_config");
167  const string joint_names[] =
168  {
169  "FFJ1", "FFJ2", "FFJ3", "FFJ4", "MFJ1", "MFJ2", "MFJ3", "MFJ4",
170  "RFJ1", "RFJ2", "RFJ3", "RFJ4", "LFJ1", "LFJ2", "LFJ3", "LFJ4", "LFJ5",
171  "THJ1", "THJ2", "THJ3", "THJ4", "THJ5", "WRJ1", "WRJ2"
172  };
173 
174  shadow_robot::SrHandFinder hand_finder;
175  // hand parameters
176  ASSERT_GT(hand_finder.get_hand_parameters().mapping_.size(), 0);
177  ASSERT_GT(hand_finder.get_hand_parameters().joint_prefix_.size(), 0);
178  ASSERT_EQ(hand_finder.get_hand_parameters().mapping_["1"], "1");
179  ASSERT_EQ(hand_finder.get_hand_parameters().joint_prefix_["1"], "rh_");
180 
181  // hand joints
182  ASSERT_EQ(hand_finder.get_joints().size(), 1);
183  ASSERT_GT(hand_finder.get_joints().count("1"), 0);
184  const vector<string> rh_joints = hand_finder.get_joints().at("1");
185  ASSERT_EQ(rh_joints.size(), 24);
186  for (size_t i = 0; i < rh_joints.size(); ++i)
187  {
188  ROS_DEBUG_STREAM(rh_joints[i]);
189  ASSERT_STREQ(rh_joints[i].c_str(), ("rh_" + joint_names[i]).c_str());
190  }
191 
192  // calibration
193  ASSERT_GT(hand_finder.get_calibration_path().size(), 0);
194  const string calibration_path = hand_finder.get_calibration_path().at("1");
195  ROS_DEBUG_STREAM(calibration_path.c_str());
196  ASSERT_STREQ(calibration_path.c_str(), (ethercat_path + "/calibrations/1/" + "calibration.yaml").c_str());
197 
198  // tuning
199  shadow_robot::HandControllerTuning controller_tuning(hand_finder.get_hand_controller_tuning());
200 
201  for (map<string, string>::const_iterator iter = controller_tuning.friction_compensation_.begin();
202  iter != controller_tuning.friction_compensation_.end(); ++iter)
203  {
204  ASSERT_STREQ(iter->second.c_str(), (ethercat_path + "/controls/friction_compensation.yaml").c_str());
205  ROS_DEBUG_STREAM(iter->second);
206  }
207  for (map<string, string>::const_iterator iter = controller_tuning.motor_control_.begin();
208  iter != controller_tuning.motor_control_.end(); ++iter)
209  {
210  ASSERT_STREQ(iter->second.c_str(),
211  (ethercat_path + "/controls/motors/1/motor_board_effort_controllers.yaml").c_str());
212  ROS_DEBUG_STREAM(iter->second);
213  }
214  for (map<string, vector<string> >::const_iterator iter = controller_tuning.host_control_.begin();
215  iter != controller_tuning.host_control_.end(); ++iter)
216  {
217  string host_array[] =
218  {
219  "sr_edc_calibration_controllers.yaml",
220  "sr_edc_joint_velocity_controllers_PWM.yaml",
221  "sr_edc_effort_controllers_PWM.yaml",
222  "sr_edc_joint_velocity_controllers.yaml",
223  "sr_edc_effort_controllers.yaml",
224  "sr_edc_mixed_position_velocity_joint_controllers_PWM.yaml",
225  "sr_edc_joint_position_controllers_PWM.yaml",
226  "sr_edc_mixed_position_velocity_joint_controllers.yaml",
227  "sr_edc_joint_position_controllers.yaml"
228  };
229  vector<string> host_controller_files(host_array, host_array + 9);
230  ASSERT_EQ(host_controller_files.size(), iter->second.size());
231  for (size_t i = 0; i != iter->second.size(); ++i)
232  {
233  ASSERT_STREQ(iter->second[i].c_str(), (ethercat_path + "/controls/host/1/" + host_controller_files[i]).c_str());
234  ROS_DEBUG_STREAM(iter->second[i]);
235  }
236  }
237 }
238 
239 TEST(SrHandFinder, one_hand_no_prefix_no_robot_description_finder_test)
240 {
241  if (ros::param::has("hand"))
242  {
243  ros::param::del("hand");
244  }
245 
246  if (ros::param::has("robot_description"))
247  {
248  ros::param::del("robot_description");
249  }
250 
251  ros::param::set("hand/mapping/1", "rh");
252  ros::param::set("hand/joint_prefix/1", "");
253 
254  string ethercat_path = ros::package::getPath("sr_ethercat_hand_config");
255  const string joint_names[] =
256  {
257  "FFJ1", "FFJ2", "FFJ3", "FFJ4", "MFJ1", "MFJ2", "MFJ3", "MFJ4",
258  "RFJ1", "RFJ2", "RFJ3", "RFJ4", "LFJ1", "LFJ2", "LFJ3", "LFJ4", "LFJ5",
259  "THJ1", "THJ2", "THJ3", "THJ4", "THJ5", "WRJ1", "WRJ2"
260  };
261 
262  shadow_robot::SrHandFinder hand_finder;
263  // hand parameters
264  ASSERT_GT(hand_finder.get_hand_parameters().mapping_.size(), 0);
265  ASSERT_GT(hand_finder.get_hand_parameters().joint_prefix_.size(), 0);
266  ASSERT_EQ(hand_finder.get_hand_parameters().mapping_["1"], "rh");
267  ASSERT_EQ(hand_finder.get_hand_parameters().joint_prefix_["1"], "");
268 
269  // hand joints
270  ASSERT_EQ(hand_finder.get_joints().size(), 1);
271  ASSERT_GT(hand_finder.get_joints().count("rh"), 0);
272  const vector<string> rh_joints = hand_finder.get_joints().at("rh");
273  ASSERT_EQ(rh_joints.size(), 24);
274  for (size_t i = 0; i < rh_joints.size(); ++i)
275  {
276  ROS_DEBUG_STREAM(rh_joints[i]);
277  ASSERT_STREQ(rh_joints[i].c_str(), (joint_names[i]).c_str());
278  }
279 
280  // calibration
281  ASSERT_GT(hand_finder.get_calibration_path().size(), 0);
282  const string calibration_path = hand_finder.get_calibration_path().at("rh");
283  ROS_DEBUG_STREAM(calibration_path.c_str());
284  ASSERT_STREQ(calibration_path.c_str(), (ethercat_path + "/calibrations/rh/" + "calibration.yaml").c_str());
285 
286  // tuning
287  shadow_robot::HandControllerTuning controller_tuning(hand_finder.get_hand_controller_tuning());
288 
289  for (map<string, string>::const_iterator iter = controller_tuning.friction_compensation_.begin();
290  iter != controller_tuning.friction_compensation_.end(); ++iter)
291  {
292  ASSERT_STREQ(iter->second.c_str(), (ethercat_path + "/controls/friction_compensation.yaml").c_str());
293  ROS_DEBUG_STREAM(iter->second);
294  }
295  for (map<string, string>::const_iterator iter = controller_tuning.motor_control_.begin();
296  iter != controller_tuning.motor_control_.end(); ++iter)
297  {
298  ASSERT_STREQ(iter->second.c_str(),
299  (ethercat_path + "/controls/motors/rh/motor_board_effort_controllers.yaml").c_str());
300  ROS_DEBUG_STREAM(iter->second);
301  }
302  for (map<string, vector<string> >::const_iterator iter = controller_tuning.host_control_.begin();
303  iter != controller_tuning.host_control_.end(); ++iter)
304  {
305  string host_array[] =
306  {
307  "sr_edc_calibration_controllers.yaml",
308  "sr_edc_joint_velocity_controllers_PWM.yaml",
309  "sr_edc_effort_controllers_PWM.yaml",
310  "sr_edc_joint_velocity_controllers.yaml",
311  "sr_edc_effort_controllers.yaml",
312  "sr_edc_mixed_position_velocity_joint_controllers_PWM.yaml",
313  "sr_edc_joint_position_controllers_PWM.yaml",
314  "sr_edc_mixed_position_velocity_joint_controllers.yaml",
315  "sr_edc_joint_position_controllers.yaml"
316  };
317  vector<string> host_controller_files(host_array, host_array + 9);
318  ASSERT_EQ(host_controller_files.size(), iter->second.size());
319  for (size_t i = 0; i != iter->second.size(); ++i)
320  {
321  ASSERT_STREQ(iter->second[i].c_str(), (ethercat_path + "/controls/host/rh/" + host_controller_files[i]).c_str());
322  ROS_DEBUG_STREAM(iter->second[i]);
323  }
324  }
325 }
326 
327 TEST(SrHandFinder, one_hand_no_mapping_no_prefix_no_robot_description_finder_test)
328 {
329  if (ros::param::has("hand"))
330  {
331  ros::param::del("hand");
332  }
333 
334  if (ros::param::has("robot_description"))
335  {
336  ros::param::del("robot_description");
337  }
338 
339  ros::param::set("hand/mapping/1", "");
340  ros::param::set("hand/joint_prefix/1", "");
341 
342  string ethercat_path = ros::package::getPath("sr_ethercat_hand_config");
343  const string joint_names[] =
344  {
345  "FFJ1", "FFJ2", "FFJ3", "FFJ4", "MFJ1", "MFJ2", "MFJ3", "MFJ4",
346  "RFJ1", "RFJ2", "RFJ3", "RFJ4", "LFJ1", "LFJ2", "LFJ3", "LFJ4", "LFJ5",
347  "THJ1", "THJ2", "THJ3", "THJ4", "THJ5", "WRJ1", "WRJ2"
348  };
349 
350  shadow_robot::SrHandFinder hand_finder;
351  // hand parameters
352  ASSERT_GT(hand_finder.get_hand_parameters().mapping_.size(), 0);
353  ASSERT_GT(hand_finder.get_hand_parameters().joint_prefix_.size(), 0);
354  ASSERT_EQ(hand_finder.get_hand_parameters().mapping_["1"], "1");
355  ASSERT_EQ(hand_finder.get_hand_parameters().joint_prefix_["1"], "");
356 
357  // hand joints
358  ASSERT_EQ(hand_finder.get_joints().size(), 1);
359  ASSERT_GT(hand_finder.get_joints().count("1"), 0);
360  const vector<string> rh_joints = hand_finder.get_joints().at("1");
361  ASSERT_EQ(rh_joints.size(), 24);
362  for (size_t i = 0; i < rh_joints.size(); ++i)
363  {
364  ROS_DEBUG_STREAM(rh_joints[i]);
365  ASSERT_STREQ(rh_joints[i].c_str(), (joint_names[i]).c_str());
366  }
367 
368  // calibration
369  ASSERT_GT(hand_finder.get_calibration_path().size(), 0);
370  const string calibration_path = hand_finder.get_calibration_path().at("1");
371  ROS_DEBUG_STREAM(calibration_path.c_str());
372  ASSERT_STREQ(calibration_path.c_str(), (ethercat_path + "/calibrations/1/" + "calibration.yaml").c_str());
373 
374  // tuning
375  shadow_robot::HandControllerTuning controller_tuning(hand_finder.get_hand_controller_tuning());
376 
377  for (map<string, string>::const_iterator iter = controller_tuning.friction_compensation_.begin();
378  iter != controller_tuning.friction_compensation_.end(); ++iter)
379  {
380  ASSERT_STREQ(iter->second.c_str(), (ethercat_path + "/controls/friction_compensation.yaml").c_str());
381  ROS_DEBUG_STREAM(iter->second);
382  }
383  for (map<string, string>::const_iterator iter = controller_tuning.motor_control_.begin();
384  iter != controller_tuning.motor_control_.end(); ++iter)
385  {
386  ASSERT_STREQ(iter->second.c_str(),
387  (ethercat_path + "/controls/motors/1/motor_board_effort_controllers.yaml").c_str());
388  ROS_DEBUG_STREAM(iter->second);
389  }
390  for (map<string, vector<string> >::const_iterator iter = controller_tuning.host_control_.begin();
391  iter != controller_tuning.host_control_.end(); ++iter)
392  {
393  string host_array[] =
394  {
395  "sr_edc_calibration_controllers.yaml",
396  "sr_edc_joint_velocity_controllers_PWM.yaml",
397  "sr_edc_effort_controllers_PWM.yaml",
398  "sr_edc_joint_velocity_controllers.yaml",
399  "sr_edc_effort_controllers.yaml",
400  "sr_edc_mixed_position_velocity_joint_controllers_PWM.yaml",
401  "sr_edc_joint_position_controllers_PWM.yaml",
402  "sr_edc_mixed_position_velocity_joint_controllers.yaml",
403  "sr_edc_joint_position_controllers.yaml"
404  };
405  vector<string> host_controller_files(host_array, host_array + 9);
406  ASSERT_EQ(host_controller_files.size(), iter->second.size());
407  for (size_t i = 0; i != iter->second.size(); ++i)
408  {
409  ASSERT_STREQ(iter->second[i].c_str(), (ethercat_path + "/controls/host/1/" + host_controller_files[i]).c_str());
410  ROS_DEBUG_STREAM(iter->second[i]);
411  }
412  }
413 }
414 
415 TEST(SrHandFinder, one_hand_robot_description_exists_finger_test)
416 {
417  if (ros::param::has("hand"))
418  {
419  ros::param::del("hand");
420  }
421 
422  if (ros::param::has("robot_description"))
423  {
424  ros::param::del("robot_description");
425  }
426 
427  string right_hand_description;
428  ros::param::get("right_hand_description", right_hand_description);
429  ros::param::set("robot_description", right_hand_description);
430 
431  ros::param::set("hand/mapping/1", "right");
432  ros::param::set("hand/joint_prefix/1", "rh_");
433  string ethercat_path = ros::package::getPath("sr_ethercat_hand_config");
434  const string joint_names[] =
435  {
436  "FFJ1", "FFJ2", "FFJ3", "FFJ4", "MFJ1", "MFJ2", "MFJ3", "MFJ4",
437  "RFJ1", "RFJ2", "RFJ3", "RFJ4", "LFJ1", "LFJ2", "LFJ3", "LFJ4", "LFJ5",
438  "THJ1", "THJ2", "THJ3", "THJ4", "THJ5", "WRJ1", "WRJ2"
439  };
440 
441  shadow_robot::SrHandFinder hand_finder;
442  // hand parameters
443  ASSERT_GT(hand_finder.get_hand_parameters().mapping_.size(), 0);
444  ASSERT_GT(hand_finder.get_hand_parameters().joint_prefix_.size(), 0);
445  ASSERT_EQ(hand_finder.get_hand_parameters().mapping_["1"], "right");
446  ASSERT_EQ(hand_finder.get_hand_parameters().joint_prefix_["1"], "rh_");
447 
448  // hand joints
449  ASSERT_EQ(hand_finder.get_joints().size(), 1);
450  ASSERT_GT(hand_finder.get_joints().count("right"), 0);
451  const vector<string> rh_joints = hand_finder.get_joints().at("right");
452  ASSERT_EQ(rh_joints.size(), 1); // only RFJ4 is there
453  ASSERT_EQ(std::find(rh_joints.begin(), rh_joints.end(), "rh_FFJ3"), rh_joints.end());
454  ASSERT_NE(std::find(rh_joints.begin(), rh_joints.end(), "rh_RFJ4"), rh_joints.end());
455 
456  // calibration
457  ASSERT_GT(hand_finder.get_calibration_path().size(), 0);
458  const string calibration_path = hand_finder.get_calibration_path().at("right");
459  ROS_DEBUG_STREAM(calibration_path.c_str());
460  ASSERT_STREQ(calibration_path.c_str(), (ethercat_path + "/calibrations/right/" + "calibration.yaml").c_str());
461 
462  // tuning
463  shadow_robot::HandControllerTuning controller_tuning(hand_finder.get_hand_controller_tuning());
464 
465  for (map<string, string>::const_iterator iter = controller_tuning.friction_compensation_.begin();
466  iter != controller_tuning.friction_compensation_.end(); ++iter)
467  {
468  ASSERT_STREQ(iter->second.c_str(), (ethercat_path + "/controls/friction_compensation.yaml").c_str());
469  ROS_DEBUG_STREAM(iter->second);
470  }
471  for (map<string, string>::const_iterator iter = controller_tuning.motor_control_.begin();
472  iter != controller_tuning.motor_control_.end(); ++iter)
473  {
474  ASSERT_STREQ(iter->second.c_str(),
475  (ethercat_path + "/controls/motors/right/motor_board_effort_controllers.yaml").c_str());
476  ROS_DEBUG_STREAM(iter->second);
477  }
478  for (map<string, vector<string> >::const_iterator iter = controller_tuning.host_control_.begin();
479  iter != controller_tuning.host_control_.end(); ++iter)
480  {
481  string host_array[] =
482  {
483  "sr_edc_calibration_controllers.yaml",
484  "sr_edc_joint_velocity_controllers_PWM.yaml",
485  "sr_edc_effort_controllers_PWM.yaml",
486  "sr_edc_joint_velocity_controllers.yaml",
487  "sr_edc_effort_controllers.yaml",
488  "sr_edc_mixed_position_velocity_joint_controllers_PWM.yaml",
489  "sr_edc_joint_position_controllers_PWM.yaml",
490  "sr_edc_mixed_position_velocity_joint_controllers.yaml",
491  "sr_edc_joint_position_controllers.yaml"
492  };
493  vector<string> host_controller_files(host_array, host_array + 9);
494  ASSERT_EQ(host_controller_files.size(), iter->second.size());
495  for (size_t i = 0; i != iter->second.size(); ++i)
496  {
497  ASSERT_STREQ(iter->second[i].c_str(), (ethercat_path + "/controls/host/right/"
498  + host_controller_files[i]).c_str());
499  ROS_DEBUG_STREAM(iter->second[i]);
500  }
501  }
502 }
503 
504 TEST(SrHandFinder, two_hand_robot_description_exists_finder_test)
505 {
506  if (ros::param::has("hand"))
507  {
508  ros::param::del("hand");
509  }
510 
511  if (ros::param::has("robot_description"))
512  {
513  ros::param::del("robot_description");
514  }
515 
516  ros::param::set("hand/mapping/1", "right");
517  ros::param::set("hand/joint_prefix/1", "rh_");
518  ros::param::set("hand/mapping/2", "left");
519  ros::param::set("hand/joint_prefix/2", "lh_");
520 
521  string two_hands_description;
522  ros::param::get("two_hands_description", two_hands_description);
523  ros::param::set("robot_description", two_hands_description);
524 
525  const string joint_names[] =
526  {
527  "FFJ1", "FFJ2", "FFJ3", "FFJ4", "MFJ1", "MFJ2", "MFJ3", "MFJ4",
528  "RFJ1", "RFJ2", "RFJ3", "RFJ4", "LFJ1", "LFJ2", "LFJ3", "LFJ4", "LFJ5",
529  "THJ1", "THJ2", "THJ3", "THJ4", "THJ5", "WRJ1", "WRJ2"
530  };
531 
532  unsigned idx = 0;
533  const string dir[] = {"left", "right"};
534 
535  shadow_robot::SrHandFinder hand_finder;
536 
537  ASSERT_EQ(hand_finder.get_hand_parameters().mapping_.size(), 2);
538  ASSERT_EQ(hand_finder.get_hand_parameters().joint_prefix_.size(), 2);
539  ASSERT_EQ(hand_finder.get_joints().size(), 2);
540  ASSERT_GT(hand_finder.get_calibration_path().size(), 0);
541 
542  ASSERT_MAP_HAS_VALUE(hand_finder.get_hand_parameters().mapping_, "right");
543  ASSERT_MAP_HAS_VALUE(hand_finder.get_hand_parameters().mapping_, "left");
544 
545  ASSERT_MAP_HAS_VALUE(hand_finder.get_hand_parameters().joint_prefix_, "rh_");
546  ASSERT_MAP_HAS_VALUE(hand_finder.get_hand_parameters().joint_prefix_, "lh_");
547 
548  ASSERT_GT(hand_finder.get_joints().count("right"), 0);
549  const vector<string> rh_joints = hand_finder.get_joints().at("right");
550  ASSERT_EQ(std::find(rh_joints.begin(), rh_joints.end(), "rh_FFJ3"), rh_joints.end());
551  ASSERT_NE(std::find(rh_joints.begin(), rh_joints.end(), "rh_RFJ4"), rh_joints.end());
552 
553  ASSERT_GT(hand_finder.get_joints().count("left"), 0);
554  const vector<string> lh_joints = hand_finder.get_joints().at("left");
555  ASSERT_EQ(std::find(lh_joints.begin(), lh_joints.end(), "lh_FFJ1"), lh_joints.end());
556  ASSERT_NE(std::find(lh_joints.begin(), lh_joints.end(), "lh_LFJ4"), lh_joints.end());
557 
558  string ethercat_path = ros::package::getPath("sr_ethercat_hand_config");
559  map<string, string> calibration_path(hand_finder.get_calibration_path());
560  idx = 0;
561  for (map<string, string>::const_iterator iter = calibration_path.begin(); iter != calibration_path.end(); ++iter)
562  {
563  ROS_DEBUG_STREAM(iter->first << ":" << iter->second);
564  ASSERT_STREQ(iter->second.c_str(), (ethercat_path + "/calibrations/"
565  + dir[idx] + "/" + "calibration.yaml").c_str());
566  idx++;
567  }
568  shadow_robot::HandControllerTuning controller_tuning(hand_finder.get_hand_controller_tuning());
569 
570  for (map<string, string>::const_iterator iter = controller_tuning.friction_compensation_.begin();
571  iter != controller_tuning.friction_compensation_.end(); ++iter)
572  {
573  ASSERT_STREQ(iter->second.c_str(), (ethercat_path + "/controls/friction_compensation.yaml").c_str());
574  ROS_DEBUG_STREAM(iter->second);
575  }
576 
577  idx = 0;
578  for (map<string, string>::const_iterator iter = controller_tuning.motor_control_.begin();
579  iter != controller_tuning.motor_control_.end(); ++iter)
580  {
581  ASSERT_STREQ(iter->second.c_str(),
582  (ethercat_path + "/controls/motors/" + dir[idx] + "/motor_board_effort_controllers.yaml").c_str());
583  ROS_DEBUG_STREAM(iter->second);
584  idx++;
585  }
586 
587  idx = 0;
588  for (map<string, vector<string> >::const_iterator iter = controller_tuning.host_control_.begin();
589  iter != controller_tuning.host_control_.end(); ++iter)
590  {
591  string host_array[] =
592  {
593  "sr_edc_calibration_controllers.yaml",
594  "sr_edc_joint_velocity_controllers_PWM.yaml",
595  "sr_edc_effort_controllers_PWM.yaml",
596  "sr_edc_joint_velocity_controllers.yaml",
597  "sr_edc_effort_controllers.yaml",
598  "sr_edc_mixed_position_velocity_joint_controllers_PWM.yaml",
599  "sr_edc_joint_position_controllers_PWM.yaml",
600  "sr_edc_mixed_position_velocity_joint_controllers.yaml",
601  "sr_edc_joint_position_controllers.yaml"
602  };
603  vector<string> host_controller_files(host_array, host_array + 9);
604  ASSERT_EQ(host_controller_files.size(), iter->second.size());
605  for (size_t i = 0; i != iter->second.size(); ++i)
606  {
607  ASSERT_STREQ(iter->second[i].c_str(), (ethercat_path + "/controls/host/"
608  + dir[idx] + "/" + host_controller_files[i]).c_str());
609  ROS_DEBUG_STREAM(iter->second[i]);
610  }
611  idx++;
612  }
613 }
614 
615 
616 int main(int argc, char **argv)
617 {
618  ros::init(argc, argv, "test_hand_finder");
619  ros::NodeHandle nh;
620  testing::InitGoogleTest(&argc, argv);
621  return RUN_ALL_TESTS();
622 }
623 
ros::NodeHandle
test_hand_finder.joint_names
list joint_names
Definition: test_hand_finder.py:8
shadow_robot::SrHandFinder
Definition: sr_hand_finder.hpp:45
TEST
TEST(SrHandFinder, hand_absent_test)
Definition: test_hand_finder.cpp:39
shadow_robot::SrHandFinder::get_hand_controller_tuning
HandControllerTuning get_hand_controller_tuning()
Definition: sr_hand_finder.cpp:209
sr_hand_finder.hpp
ros::init
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
shadow_robot::HandControllerTuning
Definition: sr_hand_finder.hpp:38
ASSERT_MAP_HAS_VALUE
void ASSERT_MAP_HAS_VALUE(const map< string, string > &map, const string &value)
Definition: test_hand_finder.cpp:24
shadow_robot::HandConfig::mapping_
std::map< std::string, std::string > mapping_
Definition: sr_hand_finder.hpp:34
ros::param::set
ROSCPP_DECL void set(const std::string &key, const XmlRpc::XmlRpcValue &v)
ros::param::get
ROSCPP_DECL bool get(const std::string &key, std::string &s)
main
int main(int argc, char **argv)
Definition: test_hand_finder.cpp:616
shadow_robot::SrHandFinder::get_hand_parameters
HandConfig get_hand_parameters()
Definition: sr_hand_finder.cpp:53
ros::param::has
ROSCPP_DECL bool has(const std::string &key)
shadow_robot::SrHandFinder::get_calibration_path
std::map< std::string, std::string > get_calibration_path()
Definition: sr_hand_finder.cpp:204
ROS_DEBUG_STREAM
#define ROS_DEBUG_STREAM(args)
shadow_robot::SrHandFinder::get_joints
std::map< std::string, std::vector< std::string > > get_joints()
Definition: sr_hand_finder.cpp:199
ros::package::getPath
ROSLIB_DECL std::string getPath(const std::string &package_name)
ros::param::del
ROSCPP_DECL bool del(const std::string &key)
shadow_robot::HandConfig::joint_prefix_
std::map< std::string, std::string > joint_prefix_
Definition: sr_hand_finder.hpp:35

sr_utilities
Author(s): Ugo Cupcic
autogenerated on Tue Oct 13 2020 03:55:49