#!/usr/bin/env python
import rospy
import sys
import math
import time
from std_msgs.msg import Int16, Int64, Float32, Float64MultiArray, String, Head$
import RPi.GPIO as GPIO

pins = [4, 17, 27, 18, 23]
servos = []

for pin in pins:
 GPIO.setmode(GPIO.BCM)
 servos.append(GPIO.setup(pin,GPIO.OUT))
 p=GPIO.PWM(pin,50)
 p.start(7.5)

def _Update_q(q):
 rospy.loginfo(q)


 for index in [0, 1, 2, 3, 4]:
  p = servos[index]
  p.ChangeDutyCycle(q.data[index])
  time.sleep(3)
  print('ok')

def listener():
 rospy.init_node('listener', anonymous=True)
 rospy.Subscriber('/pose', Float64MultiArray, _Update_q)
 rospy.spin()

if __name__ == '__main__':
 listener()