KRS Series Kondo servo with ROS

Description: KRS Series Kondo servo with ROS, embedded, programming

We will going to see the code below my introduction about he code below:
KRS Series Kondo servo with ROS


Please see this code:


#!/usr/bin/env python
# license removed for brevity
import rospy
from geometry_msgs.msg import PointStamped
import serial
import time
import struct
import array
import numpy
com = serial.Serial()
com.port='/dev/ttyUSB0'
com.baudrate=115200
com.bytesize=8
com.parity=serial.PARITY_EVEN
com.stopbits=1
com.timeout=0.1
com.open()
buf=bytearray(3)
temp_=bytearray(2)
target_angle=7500
angle=0.0
a_=270.0/8000.0
b_=-135.0-a_*3500.0
def callback(data):
    global target_angle
    global angle
    angle=data.point.x
    target_angle=(int)((angle-b_)/a_)
    if(target_angle>11500):
        target_angle=11500
    elif(target_angle<3500 ) :
        target_angle=3500
def data_pub():
    pub = rospy.Publisher('servo_angle', PointStamped, queue_size=100)
    rospy.Subscriber("master_angle",  PointStamped, callback)   
    rospy.init_node('servo_node', anonymous=True)
    r = rospy.Rate(100) # 100hz
    global target_angle
    global angle
    data_msg= PointStamped();
    while not rospy.is_shutdown():
        data_msg.header.stamp=rospy.get_rostime()
        buf[0]=0x80
        buf[1]=(target_angle>>7)&0x7f
        buf[2]=target_angle&0x7f
        com.write(buf)
        buf_read=com.read(6)
        if (len(buf_read)==6):
          temp_[0]=ord(buf_read[4])
          temp_[1]=ord(buf_read[5])
          val=temp_[1]|(temp_[0]<<7)
          print "%d" %(val)
          data_msg.point.x=angle
          data_msg.point.y=(float)(val)*a_+b_
          pub.publish(data_msg)
       
        r.sleep()
if __name__ == '__main__':
    try:
        data_pub()
    except rospy.ROSInterruptException: pass