总线伺服舵机SDK使用手册（ROS）¶

#!/usr/bin/env python
#coding:utf-8
'''
舵机控制节点(Demo)
'''
import rospy
import serial
from uservo import UartServoManager
from fashionstar_uart_servo_ros1.msg import SetServoAngle, SetServoDamping
from fashionstar_uart_servo_ros1.srv import QueryServoAngle, QueryServoAngleResponse

# 参数配置
# 角度定义
SERVO_PORT_NAME =  u'/dev/ttyUSB0'      # 舵机串口号 <<< 修改为实际串口号
SERVO_BAUDRATE = 115200                 # 舵机的波特率
SERVO_ID = 0                            # 舵机的ID号
SERVO_HAS_MTURN_FUNC = False            # 舵机是否拥有多圈模式

# 初始化串口
uart = serial.Serial(port=SERVO_PORT_NAME, baudrate=SERVO_BAUDRATE,\
                     parity=serial.PARITY_NONE, stopbits=1,\
                     bytesize=8,timeout=0)
# 初始化舵机管理器
uservo = UartServoManager(uart)

def set_servo_angle_callback(data):
    '''回调函数-设置舵机角度'''
    rospy.loginfo("[Recv] Servo ID ={} Set Angle={}".format(data.id, data.angle))
    # 调用舵机SDK - 设置舵机的角度
    uservo.set_servo_angle(data.id, data.angle, interval=0)

def set_servo_damping_callback(data):
    '''回调函数-设置舵机为阻尼模式'''
    rospy.loginfo("[Recv] Servo ID ={} Set Damping Power={}".format(data.id, data.power))
    # 调用舵机SDK - 设置舵机为阻尼模式
    uservo.set_damping(data.id, data.power)

def query_servo_angle_callback(req):
    '''回调函数-查询舵机角度'''
    # 创建回传对象
    response = QueryServoAngleResponse()
    # 查询舵机的角度
    angle = uservo.query_servo_angle(req.id)
    # 填充回传对象的内容
    response.angle = angle
    rospy.loginfo("[Recv] Servo ID ={} Query Angle={}".format(req.id, angle))

    return response

def uart_manager():    
    # 初始化节点
    rospy.init_node('uservo_node', anonymous=True)
    # 舵机角度设置订阅者
    rospy.Subscriber("set_servo_angle", SetServoAngle, set_servo_angle_callback)
    # 舵机阻尼模式订阅者
    rospy.Subscriber("set_servo_damping", SetServoDamping, set_servo_damping_callback)
    # 舵机角度查询服务
    rospy.Service('query_servo_angle', QueryServoAngle, query_servo_angle_callback)
    # 持续等待， 直到节点结束
    rospy.spin()

if __name__ == '__main__':
    uart_manager()

#!/usr/bin/env python
#coding:utf-8
'''
舵机控制节点(Demo)
'''
import rospy
import serial
from uservo import UartServoManager
from fashionstar_uart_servo_ros1.msg import SetServoAngle, SetServoDamping
from fashionstar_uart_servo_ros1.srv import QueryServoAngle, QueryServoAngleResponse

# 参数配置
# 角度定义
SERVO_PORT_NAME =  u'/dev/ttyUSB0'      # 舵机串口号 <<< 修改为实际串口号
SERVO_BAUDRATE = 115200                 # 舵机的波特率
SERVO_ID = 0                            # 舵机的ID号
SERVO_HAS_MTURN_FUNC = False            # 舵机是否拥有多圈模式

# 初始化串口
uart = serial.Serial(port=SERVO_PORT_NAME, baudrate=SERVO_BAUDRATE,\
                     parity=serial.PARITY_NONE, stopbits=1,\
                     bytesize=8,timeout=0)
# 初始化舵机管理器
uservo = UartServoManager(uart)

def set_servo_angle_callback(data):
    '''回调函数-设置舵机角度'''
    rospy.loginfo("[Recv] Servo ID ={} Set Angle={}".format(data.id, data.angle))
    # 调用舵机SDK - 设置舵机的角度
    uservo.set_servo_angle(data.id, data.angle, interval=0)

def set_servo_damping_callback(data):
    '''回调函数-设置舵机为阻尼模式'''
    rospy.loginfo("[Recv] Servo ID ={} Set Damping Power={}".format(data.id, data.power))
    # 调用舵机SDK - 设置舵机为阻尼模式
    uservo.set_damping(data.id, data.power)

def query_servo_angle_callback(req):
    '''回调函数-查询舵机角度'''
    # 创建回传对象
    response = QueryServoAngleResponse()
    # 查询舵机的角度
    angle = uservo.query_servo_angle(req.id)
    # 填充回传对象的内容
    response.angle = angle
    rospy.loginfo("[Recv] Servo ID ={} Query Angle={}".format(req.id, angle))

    return response

def uart_manager():    
    # 初始化节点
    rospy.init_node('uservo_node', anonymous=True)
    # 舵机角度设置订阅者
    rospy.Subscriber("set_servo_angle", SetServoAngle, set_servo_angle_callback)
    # 舵机阻尼模式订阅者
    rospy.Subscriber("set_servo_damping", SetServoDamping, set_servo_damping_callback)
    # 舵机角度查询服务
    rospy.Service('query_servo_angle', QueryServoAngle, query_servo_angle_callback)
    # 持续等待， 直到节点结束
    rospy.spin()

if __name__ == '__main__':
    uart_manager()

#!/usr/bin/env python
#coding:utf-8

#!/usr/bin/env python
#coding:utf-8
'''
电机角度控制测试节点
'''
import rospy
from fashionstar_uart_servo_ros1.msg import SetServoAngle

SERVO_ID = 0 # 测试的电机ID

def test_set_servo_angle():
    '''回调函数'''
    # 创建节点
    rospy.init_node('test_set_angle_node', anonymous=True) 
    # 创建发布者 
    # - Topic名称为:  'set_servo_angle'
    # - Message格式类型: set_servo_angle
    # - queue_size 消息队列尺寸，自定义
    pub = rospy.Publisher('set_servo_angle', SetServoAngle, queue_size=2)
    # 设置发送节拍 0.2HZ 
    rate = rospy.Rate(0.2)
    # 电机角度
    angle = 90.0
    # 创建信息对象 
    msg = SetServoAngle()

    while not rospy.is_shutdown():
        # 修改电机角度
        angle *= -1.0
        # 填充Message
        msg.id = SERVO_ID
        msg.angle = angle
        rospy.loginfo('Set Servo {} = {}'.format(msg.id, msg.angle))
        pub.publish(msg)
        rate.sleep()

if __name__ == '__main__':
    test_set_servo_angle()

#!/usr/bin/env python
#coding:utf-8
'''
电机角度查询节点
'''
import rospy
from fashionstar_uart_servo_ros1.msg import SetServoDamping
from fashionstar_uart_servo_ros1.srv import QueryServoAngle

SERVO_ID = 0 # 测试的电机ID

def test_query_servo_angle():
    '''回调函数'''
    # 创建节点
    rospy.init_node('test_query_angle_node', anonymous=True) 
    # 等待角度查询服务开启
    srv_query_servo_angle = rospy.ServiceProxy('query_servo_angle', QueryServoAngle)
    # 创建发布者 - 舵机阻尼模式发布器
    set_servo_damping_pub = rospy.Publisher('set_servo_damping', SetServoDamping, queue_size=1)
    # 构造Damping控制指令
    damping_msg = SetServoDamping()
    damping_msg.id = SERVO_ID   # 舵机ID
    damping_msg.power = 400    # 功率
    rospy.sleep(2)
    set_servo_damping_pub.publish(damping_msg) # 设置为Damping模式
    rospy.loginfo("Servo #{} Set Damping Mode, Power={} mW".format(SERVO_ID, damping_msg.power))

    # 设置发送节拍 0.2HZ 
    rate = rospy.Rate(1)

    while not rospy.is_shutdown():
        # 设置舵机为阻尼模式
        # 理论上来讲，发一次就可以了， 但是实际上因为message通信模式
        # 不能保证发出的一定被接受到，所以保险还是多发几次
        # set_servo_damping_pub.publish(damping_msg)

        # 请求query_servo_angle的服务
        try:
            response = srv_query_servo_angle(SERVO_ID)
            # 提取查询的角度
            angle = response.angle
            # 输出角度查询日志
            rospy.loginfo("Servo #{} Angle={}".format(SERVO_ID, angle))
        except rospy.ServiceException as e:
            # 服务并不总是可以被处理，会存在一定概率的服务请求失败的问题
            print("Service Call Failed : {}".format(e))

        # 等待
        rate.sleep()

if __name__ == '__main__':
    test_query_servo_angle()

#!/usr/bin/env python
#coding:utf-8
'''
舵机控制节点
'''
import rospy
import serial
from uservo import UartServoManager
from fashionstar_uart_servo_ros1.msg import SetServoAngle

# 参数配置
# 角度定义
SERVO_PORT_NAME =  u'/dev/ttyUSB0'      # 舵机串口号 <<< 修改为实际串口号
SERVO_BAUDRATE = 115200                 # 舵机的波特率
SERVO_ID = 0                            # 舵机的ID号
SERVO_HAS_MTURN_FUNC = False            # 舵机是否拥有多圈模式

# 初始化串口
uart = serial.Serial(port=SERVO_PORT_NAME, baudrate=SERVO_BAUDRATE,\
                     parity=serial.PARITY_NONE, stopbits=1,\
                     bytesize=8,timeout=0)
# 初始化舵机管理器
uservo = UartServoManager(uart)

def set_servo_angle_callback(data):
    '''回调函数-设置舵机角度'''
    rospy.loginfo("[Recv] Servo ID ={} Set Angle={}".format(data.id, data.angle))
    uservo.set_servo_angle(data.id, data.angle, interval=0)

def uart_manager():    
    # 初始化节点
    rospy.init_node('uservo_node', anonymous=True)
    # 舵机角度订阅者
    rospy.Subscriber("set_servo_angle", SetServoAngle, set_servo_angle_callback)
    # 持续等待， 直到节点结束
    rospy.spin()

if __name__ == '__main__':
    uart_manager()