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