everything seems to work together as expected
(if not perfectly)
This commit is contained in:
@@ -18,6 +18,8 @@ if __name__ == '__main__':
|
||||
rospy.init_node('imitator')
|
||||
rospy.wait_for_service('inform_controller')
|
||||
ll = tf.TransformListener()
|
||||
am = ALProxy('ALAutonomousMoves', os.environ['NAO_IP'], 9559)
|
||||
am.setExpressiveListeningEnabled(False)
|
||||
mp = ALProxy('ALMotion', os.environ['NAO_IP'], 9559)
|
||||
mp.wakeUp()
|
||||
mp.setAngles(['LElbowRoll', 'RElbowRoll', 'LElbowYaw', 'RElbowYaw'],
|
||||
@@ -27,7 +29,7 @@ if __name__ == '__main__':
|
||||
sleep(0.1)
|
||||
if not _inform_controller('imitate'):
|
||||
continue
|
||||
rospy.loginfo('IMITATOR: ACTIVE')
|
||||
rospy.logdebug('IMITATOR: ACTIVE')
|
||||
for i, eff in enumerate(['L',
|
||||
'R'], 1):
|
||||
try:
|
||||
@@ -37,6 +39,7 @@ if __name__ == '__main__':
|
||||
rospy.Time(0)
|
||||
)
|
||||
except Exception as e:
|
||||
rospy.logwarn(e)
|
||||
continue
|
||||
|
||||
sign = 1 if eff == 'L' else -1
|
||||
|
||||
Reference in New Issue
Block a user