51 lines
1.4 KiB
Python
Executable File
51 lines
1.4 KiB
Python
Executable File
#! /usr/bin/env python
|
|
|
|
import rospy
|
|
import tf
|
|
|
|
from masterloop import inform_masterloop_factory
|
|
from controller import movement, dumb, our_cartesian
|
|
|
|
|
|
_inform_masterloop = inform_masterloop_factory('imitator')
|
|
|
|
|
|
TORSO = False
|
|
|
|
|
|
if __name__ == '__main__':
|
|
rospy.init_node('imitator')
|
|
rospy.wait_for_service('inform_masterloop')
|
|
|
|
ll = tf.TransformListener()
|
|
|
|
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, our_cartesian)
|