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