Revision 04cf2b6f
| client/python/hlrc_client/MiddlewareROS.py | ||
|---|---|---|
| 179 | 179 |
else: |
| 180 | 180 |
goal.gaze_type = gazetargetGoal.GAZETARGET_RELATIVE |
| 181 | 181 |
|
| 182 |
goal.gaze_timestamp = rospy.Time.from_sec(gaze.timestamp)
|
|
| 182 |
goal.gaze_timestamp = rospy.Time.from_sec(gaze.gaze_timestamp.to_seconds())
|
|
| 183 | 183 |
|
| 184 | 184 |
#send the goal to the server |
| 185 | 185 |
if (blocking): |
| client/python/hlrc_client/RobotGaze.py | ||
|---|---|---|
| 38 | 38 |
self.pan_offset = 0.0 |
| 39 | 39 |
self.tilt_offset = 0.0 |
| 40 | 40 |
self.gaze_type = RobotGaze.GAZETARGET_ABSOLUTE |
| 41 |
self.timestamp = time.time() |
|
| 41 |
self.gaze_timestamp = time.time()
|
|
| 42 | 42 |
|
| 43 | 43 |
def __str__(self): |
| 44 | 44 |
if self.gaze_type == RobotGaze.GAZETARGET_ABSOLUTE: |
| ... | ... | |
| 46 | 46 |
else: |
| 47 | 47 |
type_s = "RELATIVE" |
| 48 | 48 |
return "RobotGaze = { PTR={%6.2f %6.2f %6.2f} vergence=%6.2f PT_offset={%6.2f %6.2f} [%s, ts=%s]}" \
|
| 49 |
% (self.pan, self.tilt, self.roll, self.vergence, self.pan_offset, self.tilt_offset, type_s, self.timestamp) |
|
| 49 |
% (self.pan, self.tilt, self.roll, self.vergence, self.pan_offset, self.tilt_offset, type_s, self.gaze_timestamp) |
|
| client/python/hlrc_client/RobotTimestamp.py | ||
|---|---|---|
| 25 | 25 |
Excellence Initiative. |
| 26 | 26 |
""" |
| 27 | 27 |
import time |
| 28 |
import sys |
|
| 28 | 29 |
|
| 29 | 30 |
class RobotTimestamp: |
| 30 | 31 |
def __init__(self): |
| ... | ... | |
| 32 | 33 |
self.secs = int(seconds) |
| 33 | 34 |
self.nsecs = int((seconds - self.secs) * 1000000000) |
| 34 | 35 |
|
| 36 |
|
|
| 35 | 37 |
@classmethod |
| 36 | 38 |
def from_sec_nsec(cls, _sec, _nsec): |
| 37 | 39 |
cls.sec = _sec |
| 38 | 40 |
cls.nsec = _nsec |
| 39 | 41 |
|
| 42 |
@classmethod |
|
| 43 |
def from_system_time(cls): |
|
| 44 |
seconds = time.time() |
|
| 45 |
cls.secs = int(seconds) |
|
| 46 |
cls.nsecs = int((seconds - self.secs) * 1000000000) |
|
| 47 |
|
|
| 40 | 48 |
def to_seconds(self): |
| 41 |
return self.secs + ((float(self.nsec)) / 1000000000) |
|
| 49 |
return self.secs + ((float(self.nsecs)) / 1000000000)
|
|
| 42 | 50 |
|
| 43 | 51 |
def __str__(self): |
| 44 | 52 |
return "%f" % self.to_seconds() |
| client/python/hlrc_client/hlrc_test_relative_gaze.py | ||
|---|---|---|
| 54 | 54 |
|
| 55 | 55 |
while(r.is_running()): |
| 56 | 56 |
g.gaze_timestamp = RobotTimestamp() |
| 57 |
#add some delazy to make it realistic |
|
| 58 |
time.sleep(1.0 / 50.0) |
|
| 59 |
|
|
| 60 |
print(g) |
|
| 57 | 61 |
g.pan = random.uniform(-20, 20) |
| 58 | 62 |
|
| 59 | 63 |
for t in range(50): |
| client/python/tools/dump_relative_gazetargets.py | ||
|---|---|---|
| 80 | 80 |
id = ids[0] |
| 81 | 81 |
#print("> incoming target for joint %d" % (id))
|
| 82 | 82 |
if (id == 0x01): |
| 83 |
target_neck_pan = msg.target_position[0] |
|
| 83 |
target_neck_pan = - 180.0/math.pi * msg.target_position[0]
|
|
| 84 | 84 |
elif (id == 0x02): |
| 85 |
target_neck_tilt = msg.target_position[0] |
|
| 85 |
target_neck_tilt = 180.0/math.pi * msg.target_position[0]
|
|
| 86 | 86 |
elif (id == 0x04): |
| 87 |
target_eyes_tilt = msg.target_position[0] |
|
| 87 |
target_eyes_tilt = 180.0/math.pi * msg.target_position[0]
|
|
| 88 | 88 |
elif (id == 0x05): |
| 89 |
target_eye_l_pan = msg.target_position[0] |
|
| 89 |
target_eye_l_pan = 180.0/math.pi * msg.target_position[0]
|
|
| 90 | 90 |
elif (id == 0x06): |
| 91 |
target_eye_r_pan = msg.target_position[0] |
|
| 91 |
target_eye_r_pan = 180.0/math.pi * msg.target_position[0]
|
|
| 92 | 92 |
|
| 93 | 93 |
def incoming_humotion_gaze_target(msg): |
| 94 | 94 |
print("> incoming gaze @%f" % (msg.gaze_timestamp.to_sec()))
|
| client/python/tools/plot_relative_gazetargets.sh | ||
|---|---|---|
| 2 | 2 |
#1449653129.190272 -8.841537 0.023177 1449651904.760381 -19.532999 0.000000 -19.532999 0.000000 0.000000 -0.000000 0.007721 -8.910353 -8.911380 #DUMP |
| 3 | 3 |
offset=`cat /tmp/l|head -1|cut -d " " -f 1` |
| 4 | 4 |
gnuplot -persist <<- EOF |
| 5 |
plot "${FILE}" using (\$1-${offset}):2 w l t "pan_now", \
|
|
| 6 |
"${FILE}" using (\$1-${offset}):3 w l t "tilt_now",\
|
|
| 5 |
plot "${FILE}" using (\$1-${offset}):2 w d t "pan_now" , \
|
|
| 6 |
"${FILE}" using (\$1-${offset}):3 w d t "tilt_now" ,\
|
|
| 7 | 7 |
"${FILE}" using (\$4-${offset}):5 t "target rel pan",\
|
| 8 | 8 |
"${FILE}" using (\$1-${offset}):5 t "target rel pan [INCOMING TS]",\
|
| 9 | 9 |
"${FILE}" using (\$4-${offset}):6 t "target rel tilt",\
|
Also available in: Unified diff