test_arm_finder.cpp
Go to the documentation of this file.
1 /*
2  * File: test_arm_finder.cpp
3  * Author: Andriy Petlovanyy <andriy@shadowrobot.com>
4  * Copyright:
5  *
6  * @brief see README.md
7  */
8 #include <gtest/gtest.h>
9 #include <boost/foreach.hpp>
10 #include "ros/ros.h"
11 #include "sr_utilities/sr_arm_finder.hpp"
12 #include <utility>
13 #include <string>
14 #include <map>
15 
16 void ASSERT_MAP_HAS_VALUE(const std::map<std::string, std::string> &map, const std::string &value)
17 {
18  bool found = false;
19  std::pair<std::string, std::string> item;
20  BOOST_FOREACH(item, map)
21  {
22  if (value == item.second)
23  {
24  found = true;
25  }
26  }
27 
28  ASSERT_TRUE(found);
29 }
30 
31 TEST(SrArmFinder, no_hand_no_arm_finder_test)
32 {
33  if (ros::param::has("arm"))
34  {
35  ros::param::del("arm");
36  }
37 
38  if (ros::param::has("hand"))
39  {
40  ros::param::del("hand");
41  }
42 
43  if (ros::param::has("robot_description"))
44  {
45  ros::param::del("robot_description");
46  }
47 
48  ros::param::set("hand/mapping/1", "rh");
49  ros::param::set("hand/joint_prefix/1", "rh_");
50 
51  shadow_robot::SrArmFinder arm_finder;
52 
53  ASSERT_EQ(arm_finder.get_arm_parameters().mapping_.size(), 0);
54  ASSERT_EQ(arm_finder.get_arm_parameters().joint_prefix_.size(), 0);
55  ASSERT_EQ(arm_finder.get_joints().size(), 0);
56 }
57 
58 TEST(SrArmFinder, one_hand_no_arm_finder_test)
59 {
60  if (ros::param::has("arm"))
61  {
62  ros::param::del("arm");
63  }
64 
65  if (ros::param::has("hand"))
66  {
67  ros::param::del("hand");
68  }
69 
70  if (ros::param::has("robot_description"))
71  {
72  ros::param::del("robot_description");
73  }
74 
75  ros::param::set("hand/mapping/1", "rh");
76  ros::param::set("hand/joint_prefix/1", "rh_");
77 
78  shadow_robot::SrArmFinder arm_finder;
79 
80  ASSERT_EQ(arm_finder.get_arm_parameters().mapping_.size(), 0);
81  ASSERT_EQ(arm_finder.get_arm_parameters().joint_prefix_.size(), 0);
82  ASSERT_EQ(arm_finder.get_joints().size(), 0);
83 }
84 
85 TEST(SrArmFinder, no_hand_one_arm_finder_test)
86 {
87  if (ros::param::has("arm"))
88  {
89  ros::param::del("arm");
90  }
91 
92  if (ros::param::has("hand"))
93  {
94  ros::param::del("hand");
95  }
96 
97  if (ros::param::has("robot_description"))
98  {
99  ros::param::del("robot_description");
100  }
101 
102  std::string no_hand_one_arm_description;
103  ros::param::get("no_hand_one_arm_description", no_hand_one_arm_description);
104  ros::param::set("robot_description", no_hand_one_arm_description);
105 
106  ros::param::set("arm/mapping/1", "la");
107  ros::param::set("arm/joint_prefix/1", "la_");
108 
109  shadow_robot::SrArmFinder arm_finder;
110 
111  ASSERT_EQ(arm_finder.get_arm_parameters().mapping_.size(), 1);
112  ASSERT_EQ(arm_finder.get_arm_parameters().joint_prefix_.size(), 1);
113  ASSERT_EQ(arm_finder.get_joints().size(), 1);
114 
115  ASSERT_MAP_HAS_VALUE(arm_finder.get_arm_parameters().mapping_, "la");
116  ASSERT_MAP_HAS_VALUE(arm_finder.get_arm_parameters().joint_prefix_, "la_");
117 
118  ASSERT_GT(arm_finder.get_joints().count("la"), 0);
119 }
120 
121 TEST(SrArmFinder, one_hand_two_arms_finder_test)
122 {
123  if (ros::param::has("arm"))
124  {
125  ros::param::del("arm");
126  }
127 
128  if (ros::param::has("hand"))
129  {
130  ros::param::del("hand");
131  }
132 
133  if (ros::param::has("robot_description"))
134  {
135  ros::param::del("robot_description");
136  }
137 
138  std::string right_hand_two_arms;
139  ros::param::get("right_hand_two_arms", right_hand_two_arms);
140  ros::param::set("robot_description", right_hand_two_arms);
141 
142  ros::param::set("hand/mapping/1", "rh");
143  ros::param::set("hand/joint_prefix/1", "rh_");
144 
145  ros::param::set("arm/mapping/1", "la");
146  ros::param::set("arm/joint_prefix/1", "la_");
147 
148  ros::param::set("arm/mapping/2", "ra");
149  ros::param::set("arm/joint_prefix/2", "ra_");
150 
151  shadow_robot::SrArmFinder arm_finder;
152 
153  ASSERT_EQ(arm_finder.get_arm_parameters().mapping_.size(), 2);
154  ASSERT_EQ(arm_finder.get_arm_parameters().joint_prefix_.size(), 2);
155  ASSERT_EQ(arm_finder.get_joints().size(), 2);
156 
157  ASSERT_MAP_HAS_VALUE(arm_finder.get_arm_parameters().mapping_, "ra");
158  ASSERT_MAP_HAS_VALUE(arm_finder.get_arm_parameters().joint_prefix_, "ra_");
159 
160  ASSERT_EQ(arm_finder.get_joints().count("ra"), 1);
161  ASSERT_EQ(arm_finder.get_joints()["ra"].size(), 1);
162 
163  ASSERT_MAP_HAS_VALUE(arm_finder.get_arm_parameters().mapping_, "la");
164  ASSERT_MAP_HAS_VALUE(arm_finder.get_arm_parameters().joint_prefix_, "la_");
165 
166  ASSERT_EQ(arm_finder.get_joints().count("la"), 1);
167  ASSERT_EQ(arm_finder.get_joints()["la"].size(), 1);
168 }
169 
170 TEST(SrArmFinder, two_hands_one_arm_finder_test)
171 {
172  if (ros::param::has("arm"))
173  {
174  ros::param::del("arm");
175  }
176 
177  if (ros::param::has("hand"))
178  {
179  ros::param::del("hand");
180  }
181 
182  if (ros::param::has("robot_description"))
183  {
184  ros::param::del("robot_description");
185  }
186 
187  std::string two_hands_left_arm;
188  ros::param::get("two_hands_left_arm", two_hands_left_arm);
189  ros::param::set("robot_description", two_hands_left_arm);
190 
191  ros::param::set("hand/mapping/1", "rh");
192  ros::param::set("hand/joint_prefix/1", "rh_");
193 
194  ros::param::set("hand/mapping/2", "lh");
195  ros::param::set("hand/joint_prefix/2", "lh_");
196 
197  ros::param::set("arm/mapping/1", "la");
198  ros::param::set("arm/joint_prefix/1", "la_");
199 
200  shadow_robot::SrArmFinder arm_finder;
201 
202  ASSERT_EQ(arm_finder.get_arm_parameters().mapping_.size(), 1);
203  ASSERT_EQ(arm_finder.get_arm_parameters().joint_prefix_.size(), 1);
204  ASSERT_EQ(arm_finder.get_joints().size(), 1);
205 
206  ASSERT_MAP_HAS_VALUE(arm_finder.get_arm_parameters().mapping_, "la");
207  ASSERT_MAP_HAS_VALUE(arm_finder.get_arm_parameters().joint_prefix_, "la_");
208 
209  ASSERT_EQ(arm_finder.get_joints().count("la"), 1);
210  ASSERT_EQ(arm_finder.get_joints()["la"].size(), 1);
211 }
212 
213 int main(int argc, char **argv)
214 {
215  ros::init(argc, argv, "test_arm_finder");
216  ros::NodeHandle nh;
217  testing::InitGoogleTest(&argc, argv);
218  return RUN_ALL_TESTS();
219 }
220 
sr_arm_finder.hpp
ros::NodeHandle
shadow_robot::ArmConfig::joint_prefix_
std::map< std::string, std::string > joint_prefix_
Definition: sr_arm_finder.hpp:36
ros::init
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
ASSERT_MAP_HAS_VALUE
void ASSERT_MAP_HAS_VALUE(const std::map< std::string, std::string > &map, const std::string &value)
Definition: test_arm_finder.cpp:16
shadow_robot::SrArmFinder::get_arm_parameters
ArmConfig get_arm_parameters()
Definition: sr_arm_finder.cpp:101
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_arm_finder.cpp:213
TEST
TEST(SrArmFinder, no_hand_no_arm_finder_test)
Definition: test_arm_finder.cpp:31
ros::param::has
ROSCPP_DECL bool has(const std::string &key)
ros::param::del
ROSCPP_DECL bool del(const std::string &key)
shadow_robot::ArmConfig::mapping_
std::map< std::string, std::string > mapping_
Definition: sr_arm_finder.hpp:35
shadow_robot::SrArmFinder::get_joints
std::map< std::string, std::vector< std::string > > get_joints()
Definition: sr_arm_finder.cpp:106
shadow_robot::SrArmFinder
Definition: sr_arm_finder.hpp:39

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