test_arm_finder.cpp
Go to the documentation of this file.
1 /*
2 * Copyright 2015 Shadow Robot Company Ltd.
3 *
4 * This program is free software: you can redistribute it and/or modify it
5 * under the terms of the GNU General Public License as published by the Free
6 * Software Foundation version 2 of the License.
7 *
8 * This program is distributed in the hope that it will be useful, but WITHOUT
9 * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
10 * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
11 * more details.
12 *
13 * You should have received a copy of the GNU General Public License along
14 * with this program. If not, see <http://www.gnu.org/licenses/>.
15 */
16 
17 /*
18  * File: test_arm_finder.cpp
19  * Author: Andriy Petlovanyy <andriy@shadowrobot.com>
20  * Copyright:
21  *
22  * @brief see README.md
23  */
24 #include <gtest/gtest.h>
25 #include <boost/foreach.hpp>
26 #include "ros/ros.h"
27 #include "sr_utilities/sr_arm_finder.hpp"
28 #include <utility>
29 #include <string>
30 #include <map>
31 
32 void ASSERT_MAP_HAS_VALUE(const std::map<std::string, std::string> &map, const std::string &value)
33 {
34  bool found = false;
35  std::pair<std::string, std::string> item;
36  BOOST_FOREACH(item, map)
37  {
38  if (value == item.second)
39  {
40  found = true;
41  }
42  }
43 
44  ASSERT_TRUE(found);
45 }
46 
47 TEST(SrArmFinder, no_hand_no_arm_finder_test)
48 {
49  if (ros::param::has("arm"))
50  {
51  ros::param::del("arm");
52  }
53 
54  if (ros::param::has("hand"))
55  {
56  ros::param::del("hand");
57  }
58 
59  if (ros::param::has("robot_description"))
60  {
61  ros::param::del("robot_description");
62  }
63 
64  ros::param::set("hand/mapping/1", "rh");
65  ros::param::set("hand/joint_prefix/1", "rh_");
66 
67  shadow_robot::SrArmFinder arm_finder;
68 
69  ASSERT_EQ(arm_finder.get_arm_parameters().mapping_.size(), 0);
70  ASSERT_EQ(arm_finder.get_arm_parameters().joint_prefix_.size(), 0);
71  ASSERT_EQ(arm_finder.get_joints().size(), 0);
72 }
73 
74 TEST(SrArmFinder, one_hand_no_arm_finder_test)
75 {
76  if (ros::param::has("arm"))
77  {
78  ros::param::del("arm");
79  }
80 
81  if (ros::param::has("hand"))
82  {
83  ros::param::del("hand");
84  }
85 
86  if (ros::param::has("robot_description"))
87  {
88  ros::param::del("robot_description");
89  }
90 
91  ros::param::set("hand/mapping/1", "rh");
92  ros::param::set("hand/joint_prefix/1", "rh_");
93 
94  shadow_robot::SrArmFinder arm_finder;
95 
96  ASSERT_EQ(arm_finder.get_arm_parameters().mapping_.size(), 0);
97  ASSERT_EQ(arm_finder.get_arm_parameters().joint_prefix_.size(), 0);
98  ASSERT_EQ(arm_finder.get_joints().size(), 0);
99 }
100 
101 TEST(SrArmFinder, no_hand_one_arm_finder_test)
102 {
103  if (ros::param::has("arm"))
104  {
105  ros::param::del("arm");
106  }
107 
108  if (ros::param::has("hand"))
109  {
110  ros::param::del("hand");
111  }
112 
113  if (ros::param::has("robot_description"))
114  {
115  ros::param::del("robot_description");
116  }
117 
118  std::string no_hand_one_arm_description;
119  ros::param::get("no_hand_one_arm_description", no_hand_one_arm_description);
120  ros::param::set("robot_description", no_hand_one_arm_description);
121 
122  ros::param::set("arm/mapping/1", "la");
123  ros::param::set("arm/joint_prefix/1", "la_");
124 
125  shadow_robot::SrArmFinder arm_finder;
126 
127  ASSERT_EQ(arm_finder.get_arm_parameters().mapping_.size(), 1);
128  ASSERT_EQ(arm_finder.get_arm_parameters().joint_prefix_.size(), 1);
129  ASSERT_EQ(arm_finder.get_joints().size(), 1);
130 
131  ASSERT_MAP_HAS_VALUE(arm_finder.get_arm_parameters().mapping_, "la");
132  ASSERT_MAP_HAS_VALUE(arm_finder.get_arm_parameters().joint_prefix_, "la_");
133 
134  ASSERT_GT(arm_finder.get_joints().count("la"), 0);
135 }
136 
137 TEST(SrArmFinder, one_hand_two_arms_finder_test)
138 {
139  if (ros::param::has("arm"))
140  {
141  ros::param::del("arm");
142  }
143 
144  if (ros::param::has("hand"))
145  {
146  ros::param::del("hand");
147  }
148 
149  if (ros::param::has("robot_description"))
150  {
151  ros::param::del("robot_description");
152  }
153 
154  std::string right_hand_two_arms;
155  ros::param::get("right_hand_two_arms", right_hand_two_arms);
156  ros::param::set("robot_description", right_hand_two_arms);
157 
158  ros::param::set("hand/mapping/1", "rh");
159  ros::param::set("hand/joint_prefix/1", "rh_");
160 
161  ros::param::set("arm/mapping/1", "la");
162  ros::param::set("arm/joint_prefix/1", "la_");
163 
164  ros::param::set("arm/mapping/2", "ra");
165  ros::param::set("arm/joint_prefix/2", "ra_");
166 
167  shadow_robot::SrArmFinder arm_finder;
168 
169  ASSERT_EQ(arm_finder.get_arm_parameters().mapping_.size(), 2);
170  ASSERT_EQ(arm_finder.get_arm_parameters().joint_prefix_.size(), 2);
171  ASSERT_EQ(arm_finder.get_joints().size(), 2);
172 
173  ASSERT_MAP_HAS_VALUE(arm_finder.get_arm_parameters().mapping_, "ra");
174  ASSERT_MAP_HAS_VALUE(arm_finder.get_arm_parameters().joint_prefix_, "ra_");
175 
176  ASSERT_EQ(arm_finder.get_joints().count("ra"), 1);
177  ASSERT_EQ(arm_finder.get_joints()["ra"].size(), 1);
178 
179  ASSERT_MAP_HAS_VALUE(arm_finder.get_arm_parameters().mapping_, "la");
180  ASSERT_MAP_HAS_VALUE(arm_finder.get_arm_parameters().joint_prefix_, "la_");
181 
182  ASSERT_EQ(arm_finder.get_joints().count("la"), 1);
183  ASSERT_EQ(arm_finder.get_joints()["la"].size(), 1);
184 }
185 
186 TEST(SrArmFinder, two_hands_one_arm_finder_test)
187 {
188  if (ros::param::has("arm"))
189  {
190  ros::param::del("arm");
191  }
192 
193  if (ros::param::has("hand"))
194  {
195  ros::param::del("hand");
196  }
197 
198  if (ros::param::has("robot_description"))
199  {
200  ros::param::del("robot_description");
201  }
202 
203  std::string two_hands_left_arm;
204  ros::param::get("two_hands_left_arm", two_hands_left_arm);
205  ros::param::set("robot_description", two_hands_left_arm);
206 
207  ros::param::set("hand/mapping/1", "rh");
208  ros::param::set("hand/joint_prefix/1", "rh_");
209 
210  ros::param::set("hand/mapping/2", "lh");
211  ros::param::set("hand/joint_prefix/2", "lh_");
212 
213  ros::param::set("arm/mapping/1", "la");
214  ros::param::set("arm/joint_prefix/1", "la_");
215 
216  shadow_robot::SrArmFinder arm_finder;
217 
218  ASSERT_EQ(arm_finder.get_arm_parameters().mapping_.size(), 1);
219  ASSERT_EQ(arm_finder.get_arm_parameters().joint_prefix_.size(), 1);
220  ASSERT_EQ(arm_finder.get_joints().size(), 1);
221 
222  ASSERT_MAP_HAS_VALUE(arm_finder.get_arm_parameters().mapping_, "la");
223  ASSERT_MAP_HAS_VALUE(arm_finder.get_arm_parameters().joint_prefix_, "la_");
224 
225  ASSERT_EQ(arm_finder.get_joints().count("la"), 1);
226  ASSERT_EQ(arm_finder.get_joints()["la"].size(), 1);
227 }
228 
229 int main(int argc, char **argv)
230 {
231  ros::init(argc, argv, "test_arm_finder");
232  ros::NodeHandle nh;
233  testing::InitGoogleTest(&argc, argv);
234  return RUN_ALL_TESTS();
235 }
236 
sr_arm_finder.hpp
ros::NodeHandle
shadow_robot::ArmConfig::joint_prefix_
std::map< std::string, std::string > joint_prefix_
Definition: sr_arm_finder.hpp:37
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:32
shadow_robot::SrArmFinder::get_arm_parameters
ArmConfig get_arm_parameters()
Definition: sr_arm_finder.cpp:100
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:229
TEST
TEST(SrArmFinder, no_hand_no_arm_finder_test)
Definition: test_arm_finder.cpp:47
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:36
shadow_robot::SrArmFinder::get_joints
std::map< std::string, std::vector< std::string > > get_joints()
Definition: sr_arm_finder.cpp:105
shadow_robot::SrArmFinder
Definition: sr_arm_finder.hpp:40

sr_utilities
Author(s): Ugo Cupcic
autogenerated on Mon Feb 28 2022 23:52:19