hlrc / client / python / hlrc_client / simple_robot_gaze.py @ 660e2343
History | View | Annotate | Download (8.858 KB)
1 | 47de207d | fl | #!/usr/bin/python
|
---|---|---|---|
2 | |||
3 | 1c05bf39 | fl | __author__ = 'fl@techfak'
|
4 | |||
5 | # STD IMPORTS
|
||
6 | import sys |
||
7 | import time |
||
8 | import signal |
||
9 | import logging |
||
10 | import operator |
||
11 | |||
12 | # HLRC
|
||
13 | from hlrc_client import * |
||
14 | |||
15 | # ROS IMPORTS
|
||
16 | import rospy |
||
17 | import roslib |
||
18 | from std_msgs.msg import Header |
||
19 | from std_msgs.msg import String |
||
20 | from people_msgs.msg import Person |
||
21 | from people_msgs.msg import People |
||
22 | |||
23 | |||
24 | class RobotDriver(): |
||
25 | """
|
||
26 | This class holds the robot controller.
|
||
27 | Provides better encapsulation though...
|
||
28 | 660e2343 | Simon Schulz | no, it does NOT.
|
29 | 1c05bf39 | fl | """
|
30 | def __init__(self, _mw, _outscope): |
||
31 | print(">>> Initializing Robot Controller")
|
||
32 | self.mw = _mw
|
||
33 | self.outscope = _outscope
|
||
34 | self.robot_controller = RobotController(self.mw, self.outscope, logging.INFO) |
||
35 | |||
36 | |||
37 | class GazeController(): |
||
38 | """
|
||
39 | The GazeController receives person messages (ROS) and derives
|
||
40 | the nearest person identified. Based on this, the robot's
|
||
41 | joint angle target's are derived using the transformation
|
||
42 | class below
|
||
43 | """
|
||
44 | def __init__(self, _robot_controller, _affine_transform, _inscope): |
||
45 | print(">>> Initializing Gaze Controller")
|
||
46 | self.run = True |
||
47 | self.inscope = _inscope
|
||
48 | self.rc = _robot_controller
|
||
49 | self.at = _affine_transform
|
||
50 | self.nearest_person_x = 0.0 |
||
51 | self.nearest_person_y = 0.0 |
||
52 | signal.signal(signal.SIGINT, self.signal_handler)
|
||
53 | |||
54 | def signal_handler(self, signal, frame): |
||
55 | print ">>> ROS is about to exit (signal %s)..." % str(signal) |
||
56 | self.run = False |
||
57 | |||
58 | def people_callback(self, ros_data): |
||
59 | # Determine the nearest person
|
||
60 | idx = -1
|
||
61 | max_distance = {} |
||
62 | for person in ros_data.people: |
||
63 | idx += 1
|
||
64 | max_distance[str(idx)] = person.position.z
|
||
65 | print ">> Persons found {idx, distance}: ", max_distance |
||
66 | sort = sorted(max_distance.items(), key=operator.itemgetter(1), reverse=True) |
||
67 | print ">> Nearest Face: ", sort |
||
68 | print ">> Index: ", sort[0][0] |
||
69 | print ">> Distance in pixels: ", sort[0][1] |
||
70 | self.nearest_person_x = ros_data.people[int(sort[0][0])].position.x |
||
71 | self.nearest_person_y = ros_data.people[int(sort[0][0])].position.y |
||
72 | print ">> Position in pixels x:", self.nearest_person_x |
||
73 | print ">> Position in pixels y:", self.nearest_person_y |
||
74 | point = [self.nearest_person_x, self.nearest_person_y] |
||
75 | # Derive coordinate mapping
|
||
76 | angles = self.at.derive_mapping_coords(point)
|
||
77 | print "----------------" |
||
78 | if angles is not None: |
||
79 | # Set the robot gaze
|
||
80 | g = RobotGaze() |
||
81 | g.gaze_type = RobotGaze.GAZETARGET_ABSOLUTE |
||
82 | g.pan = angles[0]
|
||
83 | g.tilt = angles[1]
|
||
84 | print ">> Sending Gaze Type:", g |
||
85 | self.rc.robot_controller.set_gaze_target(g, False) |
||
86 | |||
87 | def run_subscriber(self): |
||
88 | print(">>> Initializing Gaze Subscriber")
|
||
89 | person_subscriber = rospy.Subscriber(self.inscope, People, self.people_callback, queue_size=1) |
||
90 | while self.run: |
||
91 | time.sleep(1)
|
||
92 | person_subscriber.unregister() |
||
93 | print ">>> Deactivating ROS Subscriber" |
||
94 | |||
95 | def derive_gaze_angle(self): |
||
96 | pass
|
||
97 | |||
98 | |||
99 | class AffineTransform: |
||
100 | """
|
||
101 | Derives the transformation between screen
|
||
102 | coordinates in pixels and joint axis angles in degree.
|
||
103 | """
|
||
104 | def __init__(self): |
||
105 | print(">>> Initializing Affine Transform")
|
||
106 | # Target ---> The ones you want to map to
|
||
107 | self.target0 = [1.0, 1.0] |
||
108 | self.target1 = [1.0, 1.0] |
||
109 | self.target2 = [1.0, 1.0] |
||
110 | self.target3 = [1.0, 1.0] |
||
111 | |||
112 | # Origin ---> The ones that are mapped to [target0, target1, target2, target3]
|
||
113 | self.origin0 = [1.0, 1.0] |
||
114 | self.origin1 = [1.0, 1.0] |
||
115 | self.origin2 = [1.0, 1.0] |
||
116 | self.origin3 = [1.0, 1.0] |
||
117 | |||
118 | # Divider
|
||
119 | self.divider = 1.0 |
||
120 | |||
121 | # Calculated and mapped Coordinates
|
||
122 | mappedCoords = [1.0, 1.0] |
||
123 | |||
124 | # Affine transformation coefficients
|
||
125 | self.An = 1.0 |
||
126 | self.Bn = 1.0 |
||
127 | self.Cn = 1.0 |
||
128 | self.Dn = 1.0 |
||
129 | self.En = 1.0 |
||
130 | self.Fn = 1.0 |
||
131 | |||
132 | # Test coord
|
||
133 | self.test = [1.0, 1.0] |
||
134 | |||
135 | def set_coords(self): |
||
136 | |||
137 | # This is the target coordinate system
|
||
138 | # Upper left corner
|
||
139 | self.target0[0] = -45.0 |
||
140 | self.target0[1] = 45.0 |
||
141 | |||
142 | # Lower left corner
|
||
143 | self.target1[0] = -45.0 |
||
144 | self.target1[1] = -45.0 |
||
145 | |||
146 | # Upper right corner
|
||
147 | self.target2[0] = 45.0 |
||
148 | self.target2[1] = 45.0 |
||
149 | |||
150 | # Lower right corner
|
||
151 | self.target3[0] = 45.0 |
||
152 | self.target3[1] = -45.0 |
||
153 | |||
154 | # This is the origin system, is mapped to [t0,t1,t2,t3]
|
||
155 | # Upper left corner
|
||
156 | self.origin0[0] = 0.0 |
||
157 | self.origin0[1] = 0.0 |
||
158 | |||
159 | # Lower left corner
|
||
160 | self.origin1[0] = 0.0 |
||
161 | self.origin1[1] = 240.0 |
||
162 | |||
163 | # Upper right corner
|
||
164 | self.origin2[0] = 320.0 |
||
165 | self.origin2[1] = 0.0 |
||
166 | |||
167 | # Lower right corner
|
||
168 | self.origin3[0] = 320.0 |
||
169 | self.origin3[1] = 240.0 |
||
170 | |||
171 | # And finally the test coordinate
|
||
172 | self.test[0] = 512.0 |
||
173 | self.test[1] = 384.0 |
||
174 | |||
175 | def calculate_divider(self): |
||
176 | result = ((self.origin0[0] - self.origin2[0]) * (self.origin1[1] - self.origin2[1])) - \ |
||
177 | ((self.origin1[0] - self.origin2[0]) * (self.origin0[1] - self.origin2[1])) |
||
178 | |||
179 | if result == 0.0: |
||
180 | print(">> Divider is ZERO - Check your Coordinates?")
|
||
181 | sys.exit(1)
|
||
182 | else:
|
||
183 | self.divider = result
|
||
184 | print(">> Divider " + str(self.divider)) |
||
185 | self.calculateAn()
|
||
186 | self.calculateBn()
|
||
187 | self.calculateCn()
|
||
188 | self.calculateDn()
|
||
189 | self.calculateEn()
|
||
190 | self.calculateFn()
|
||
191 | |||
192 | return result
|
||
193 | |||
194 | def calculateAn(self): |
||
195 | result = ((self.target0[0] - self.target2[0]) * (self.origin1[1] - self.origin2[1])) - \ |
||
196 | ((self.target1[0] - self.target2[0]) * (self.origin0[1] - self.origin2[1])) |
||
197 | self.An = result
|
||
198 | print(">> An " + str(self.An)) |
||
199 | return result
|
||
200 | |||
201 | def calculateBn(self): |
||
202 | result = ((self.origin0[0] - self.origin2[0]) * (self.target1[0] - self.target2[0])) - \ |
||
203 | ((self.target0[0] - self.target2[0]) * (self.origin1[0] - self.origin2[0])) |
||
204 | self.Bn = result
|
||
205 | print(">> Bn " + str(self.Bn)) |
||
206 | return result
|
||
207 | |||
208 | def calculateCn(self): |
||
209 | result = (self.origin2[0] * self.target1[0] - self.origin1[0] * self.target2[0]) * self.origin0[1] + \ |
||
210 | (self.origin0[0] * self.target2[0] - self.origin2[0] * self.target0[0]) * self.origin1[1] + \ |
||
211 | (self.origin1[0] * self.target0[0] - self.origin0[0] * self.target1[0]) * self.origin2[1] |
||
212 | self.Cn = result
|
||
213 | print(">> Cn " + str(self.Cn)) |
||
214 | return result
|
||
215 | |||
216 | def calculateDn(self): |
||
217 | result = ((self.target0[1] - self.target2[1]) * (self.origin1[1] - self.origin2[1])) - \ |
||
218 | ((self.target1[1] - self.target2[1]) * (self.origin0[1] - self.origin2[1])) |
||
219 | self.Dn = result
|
||
220 | print(">> Dn " + str(self.Dn)) |
||
221 | return result
|
||
222 | |||
223 | def calculateEn(self): |
||
224 | result = ((self.origin0[0] - self.origin2[0]) * (self.target1[1] - self.target2[1])) - \ |
||
225 | ((self.target0[1] - self.target2[1]) * (self.origin1[0] - self.origin2[0])) |
||
226 | self.En = result
|
||
227 | print(">> En " + str(self.En)) |
||
228 | return result
|
||
229 | |||
230 | def calculateFn(self): |
||
231 | result = (self.origin2[0] * self.target1[1] - self.origin1[0] * self.target2[1]) * self.origin0[1] + \ |
||
232 | (self.origin0[0] * self.target2[1] - self.origin2[0] * self.target0[1]) * self.origin1[1] + \ |
||
233 | (self.origin1[0] * self.target0[1] - self.origin0[0] * self.target1[1]) * self.origin2[1] |
||
234 | self.Fn = result
|
||
235 | print(">> Fn " + str(self.Fn)) |
||
236 | return result
|
||
237 | |||
238 | def derive_mapping_coords(self, point): |
||
239 | # r->x = ((matrixPtr->An * ad->x) + (matrixPtr->Bn * ad->y) + matrixPtr->Cn) / matrixPtr->Divider
|
||
240 | # r->y = ((matrixPtr->Dn * ad->x) + (matrixPtr->En * ad->y) + matrixPtr->Fn) / matrixPtr->Divider
|
||
241 | if self.divider != 0.0: |
||
242 | x = ((self.An * point[0]) + (self.Bn * point[1]) + self.Cn) / self.divider |
||
243 | y = ((self.Dn * point[0]) + (self.En * point[1]) + self.Fn) / self.divider |
||
244 | result = [x, y] |
||
245 | # print ">> Current Coordinate (Face):", point
|
||
246 | print ">> x-pixels were mapped to angle: %s \n>> y-pixels were mapped to angle: %s" % (str(x), str(y)) |
||
247 | return result
|
||
248 | else:
|
||
249 | return None |
||
250 | |||
251 | |||
252 | 61d23251 | fl | def runner(arguments): |
253 | 1c05bf39 | fl | if len(arguments) != 3: |
254 | 013997fb | fl | print(">>> Usage: simple_robot_gaze.py <inscope 'persons_scope'> <outscope 'gaze_target_scope'>\n\n")
|
255 | 1c05bf39 | fl | sys.exit(1)
|
256 | |||
257 | rd = RobotDriver("ROS", sys.argv[2]) |
||
258 | at = AffineTransform() |
||
259 | at.set_coords() |
||
260 | at.calculate_divider() |
||
261 | gc = GazeController(rd, at, sys.argv[1])
|
||
262 | gc.run_subscriber() |
||
263 | |||
264 | if __name__ == '__main__': |
||
265 | 61d23251 | fl | runner(sys.argv) |