test_arm_finder.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 
3 # Copyright 2011 Shadow Robot Company Ltd.
4 #
5 # This program is free software: you can redistribute it and/or modify it
6 # under the terms of the GNU General Public License as published by the Free
7 # Software Foundation version 2 of the License.
8 #
9 # This program is distributed in the hope that it will be useful, but WITHOUT
10 # ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
11 # FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
12 # more details.
13 #
14 # You should have received a copy of the GNU General Public License along
15 # with this program. If not, see <http://www.gnu.org/licenses/>.
16 
17 import rospy
18 import rostest
19 import unittest
20 from sr_utilities.arm_finder import ArmFinder
21 
22 
23 class TestArmFinder(unittest.TestCase):
24 
25  def test_no_hand_no_arm_finder(self):
26  if rospy.has_param("arm"):
27  rospy.delete_param("arm")
28 
29  if rospy.has_param("hand"):
30  rospy.delete_param("hand")
31 
32  if rospy.has_param("robot_description"):
33  rospy.delete_param("robot_description")
34 
35  arm_finder = ArmFinder()
36  self.assertIsNotNone(arm_finder.get_arm_parameters(), "Parameters extracted.")
37  self.assertIsNotNone(arm_finder.get_arm_joints(), "Joints extracted.")
38 
39  self.assertEqual(len(arm_finder.get_arm_parameters().mapping), 0, "It should be zero mapping")
40  self.assertEqual(len(arm_finder.get_arm_parameters().joint_prefix), 0, "It should be zero joint_prefix")
41  self.assertEqual(len(arm_finder.get_arm_joints().keys()), 0, "It should be zero arm joints")
42 
43  def test_one_hand_no_arm_finder(self):
44  if rospy.has_param("arm"):
45  rospy.delete_param("arm")
46 
47  if rospy.has_param("hand"):
48  rospy.delete_param("hand")
49 
50  if rospy.has_param("robot_description"):
51  rospy.delete_param("robot_description")
52 
53  arm_finder = ArmFinder()
54  self.assertIsNotNone(arm_finder.get_arm_parameters(), "Parameters extracted.")
55  self.assertIsNotNone(arm_finder.get_arm_joints(), "Joints extracted.")
56 
57  self.assertEqual(len(arm_finder.get_arm_parameters().mapping), 0, "It should be zero mapping")
58  self.assertEqual(len(arm_finder.get_arm_parameters().joint_prefix), 0, "It should be zero joint_prefix")
59  self.assertEqual(len(arm_finder.get_arm_joints().keys()), 0, "It should be zero arm joints")
60 
61  def test_no_hand_one_arm_finder(self):
62  if rospy.has_param("arm"):
63  rospy.delete_param("arm")
64 
65  if rospy.has_param("hand"):
66  rospy.delete_param("hand")
67 
68  if rospy.has_param("robot_description"):
69  rospy.delete_param("robot_description")
70 
71  rospy.set_param("robot_description", rospy.get_param("no_hand_one_arm_description"))
72 
73  rospy.set_param("arm/joint_prefix/1", "la_")
74  rospy.set_param("arm/mapping/1", "la")
75 
76  arm_finder = ArmFinder()
77 
78  self.assertIsNotNone(arm_finder.get_arm_parameters(), "Parameters extracted.")
79  self.assertIsNotNone(arm_finder.get_arm_joints(), "Joints extracted.")
80 
81  self.assertEqual(len(arm_finder.get_arm_parameters().mapping), 1, "It should be one mapping")
82  self.assertEqual(len(arm_finder.get_arm_parameters().joint_prefix), 1, "It should be one joint_prefix")
83  self.assertEqual(len(arm_finder.get_arm_joints().keys()), 1, "It should be one arm joints")
84 
85  self.assertIn("la", arm_finder.get_arm_parameters().mapping.values(), "It should be la mapping")
86  self.assertIn("la_", arm_finder.get_arm_parameters().joint_prefix.values(), "It should be la_ prefix")
87  self.assertIn("la", arm_finder.get_arm_joints(), "Maping should be in the joints result")
88 
89  def test_one_hand_two_arms_finder(self):
90  if rospy.has_param("arm"):
91  rospy.delete_param("arm")
92 
93  if rospy.has_param("hand"):
94  rospy.delete_param("hand")
95 
96  if rospy.has_param("robot_description"):
97  rospy.delete_param("robot_description")
98 
99  rospy.set_param("robot_description", rospy.get_param("right_hand_two_arms"))
100 
101  rospy.set_param("hand/joint_prefix/1", "rh_")
102  rospy.set_param("hand/mapping/1", "rh")
103 
104  rospy.set_param("arm/joint_prefix/1", "la_")
105  rospy.set_param("arm/mapping/1", "la")
106 
107  rospy.set_param("arm/joint_prefix/2", "ra_")
108  rospy.set_param("arm/mapping/2", "ra")
109 
110  arm_finder = ArmFinder()
111 
112  self.assertIsNotNone(arm_finder.get_arm_parameters(), "Parameters extracted.")
113  self.assertIsNotNone(arm_finder.get_arm_joints(), "Joints extracted.")
114 
115  self.assertEqual(len(arm_finder.get_arm_parameters().mapping), 2, "It should be two mappings")
116  self.assertEqual(len(arm_finder.get_arm_parameters().joint_prefix), 2, "It should be two joint_prefixes")
117  self.assertEqual(len(arm_finder.get_arm_joints().keys()), 2, "It should be two arm joints mappings")
118 
119  self.assertIn("ra", arm_finder.get_arm_parameters().mapping.values(), "It should be ra mapping")
120  self.assertIn("ra_", arm_finder.get_arm_parameters().joint_prefix.values(), "It should be ra_ prefix")
121  self.assertIn("ra", arm_finder.get_arm_joints(), "Maping should be in the joints result")
122  self.assertEqual(len(arm_finder.get_arm_joints()["ra"]), 1, "It should be one arm joint for mapping")
123 
124  self.assertIn("la", arm_finder.get_arm_parameters().mapping.values(), "It should be la mapping")
125  self.assertIn("la_", arm_finder.get_arm_parameters().joint_prefix.values(), "It should be la_ prefix")
126  self.assertIn("la", arm_finder.get_arm_joints(), "Maping should be in the joints result")
127  self.assertEqual(len(arm_finder.get_arm_joints()["la"]), 1, "It should be one arm joint for mapping")
128 
129  def test_two_hands_one_arm_finder(self):
130  if rospy.has_param("arm"):
131  rospy.delete_param("arm")
132 
133  if rospy.has_param("hand"):
134  rospy.delete_param("hand")
135 
136  if rospy.has_param("robot_description"):
137  rospy.delete_param("robot_description")
138 
139  rospy.set_param("robot_description", rospy.get_param("two_hands_left_arm"))
140 
141  rospy.set_param("hand/joint_prefix/1", "rh_")
142  rospy.set_param("hand/mapping/1", "rh")
143 
144  rospy.set_param("hand/joint_prefix/2", "lh_")
145  rospy.set_param("hand/mapping/2", "lh")
146 
147  rospy.set_param("arm/joint_prefix/1", "la_")
148  rospy.set_param("arm/mapping/1", "la")
149 
150  arm_finder = ArmFinder()
151 
152  self.assertEqual(len(arm_finder.get_arm_parameters().mapping), 1, "It should be one mapping")
153  self.assertEqual(len(arm_finder.get_arm_parameters().joint_prefix), 1, "It should be one joint_prefix")
154  self.assertEqual(len(arm_finder.get_arm_joints().keys()), 1, "It should be one arm joints")
155 
156  self.assertIn("la", arm_finder.get_arm_parameters().mapping.values(), "It should be la mapping")
157  self.assertIn("la_", arm_finder.get_arm_parameters().joint_prefix.values(), "It should be la_ prefix")
158  self.assertIn("la", arm_finder.get_arm_joints(), "Maping should be in the joints result")
159 
160 if __name__ == "__main__":
161  rospy.init_node("test_arm_finder")
162  rostest.rosrun("sr_utilities", "test_arm_finder", TestArmFinder)
test_arm_finder.TestArmFinder
Definition: test_arm_finder.py:23
test_arm_finder.TestArmFinder.test_one_hand_two_arms_finder
def test_one_hand_two_arms_finder(self)
Definition: test_arm_finder.py:89
test_arm_finder.TestArmFinder.test_two_hands_one_arm_finder
def test_two_hands_one_arm_finder(self)
Definition: test_arm_finder.py:129
test_arm_finder.TestArmFinder.test_no_hand_one_arm_finder
def test_no_hand_one_arm_finder(self)
Definition: test_arm_finder.py:61
test_arm_finder.TestArmFinder.test_one_hand_no_arm_finder
def test_one_hand_no_arm_finder(self)
Definition: test_arm_finder.py:43
test_arm_finder.TestArmFinder.test_no_hand_no_arm_finder
def test_no_hand_no_arm_finder(self)
Definition: test_arm_finder.py:25

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