60 lines
1.5 KiB
Python
Executable File
60 lines
1.5 KiB
Python
Executable File
#! /usr/bin/env python
|
|
import os
|
|
|
|
import rospy
|
|
import tf
|
|
from naoqi import ALProxy
|
|
|
|
from masterloop import inform_masterloop_factory
|
|
from controller import movement, dumb, mp
|
|
|
|
|
|
_inform_masterloop = inform_masterloop_factory('imitator')
|
|
|
|
|
|
TORSO = False
|
|
|
|
|
|
if __name__ == '__main__':
|
|
rospy.init_node('imitator')
|
|
rospy.wait_for_service('inform_masterloop')
|
|
|
|
ll = tf.TransformListener()
|
|
|
|
am = ALProxy('ALAutonomousMoves', os.environ['NAO_IP'], 9559)
|
|
am.setExpressiveListeningEnabled(False)
|
|
|
|
mp.wakeUp()
|
|
|
|
while not rospy.is_shutdown():
|
|
rospy.Rate(10).sleep()
|
|
if not _inform_masterloop('imitate'):
|
|
continue
|
|
rospy.logdebug('IMITATOR: ACTIVE')
|
|
|
|
if TORSO:
|
|
try:
|
|
_, q = ll.lookupTransform('odom',
|
|
'Aruco_0_frame',
|
|
rospy.Time(0))
|
|
rot = tf.transformations.euler_from_quaternion(q)
|
|
mp.setAngles(['LHipYawPitch', 'RHipYawPitch'],
|
|
[-rot[1], -rot[1]], 0.3)
|
|
except Exception as e:
|
|
rospy.logwarn(e)
|
|
|
|
for i, side in enumerate(['L', 'R'], 1):
|
|
try:
|
|
my_arm_xyz, _ = ll.lookupTransform(
|
|
'Aruco_0_frame',
|
|
'Aruco_{}_frame'.format(i),
|
|
rospy.Time(0)
|
|
)
|
|
except Exception as e:
|
|
rospy.logwarn(e)
|
|
continue
|
|
|
|
movement(my_arm_xyz, side, dumb)
|
|
|
|
mp.rest()
|