在ROS上使用ModBus/Tcp协议控制机械手爪

513
0
2020年11月27日 09时04分

机械臂的控制原本使用的就是ROS进行的,奈何实验室机械臂上买的机械二指夹爪并不支持ROS平台,及没有对应的模型文件和控制的包,因此控制机械手爪就需要其他办法。

 

所使用的机械手爪为nGripper90,如下图所示,查看手爪的用户手册发现,其配套包括ModBus/Tcp、UR Ethernet/IP和I/O控制三种方式,那么在ROS中可以选择使用ModBus/Tcp协议对齐进行简单的控制。

 

1

 

咨询过机械手爪厂家的技术人员后,给出了modbus/tcp控制机械手爪需要操作的寄存器,那就是nUC-S_CB4.0中的3006-3008三个寄存器,其中3006控制机械手爪的位置,内容为0-4095,其中0代表机械手抓闭合,4095代表手爪张开到最大程度;3007为机械手爪的速度,范围是0-1000,默认值是885;力的大小范围是0-1000,默认值为1000。简单的抓取只要向3006地址写位置就行。

 

2

 

首先需要在ROS中安装基本的modbus支持:

 

sudo apt-get install python-pymodbus

sudo apt-get install python-pyasn1 python-twisted-conch

 

接着在ROS上可以找到一个将modbus协议与ros标准通信方式进行转换的包,可以在ROS wiki上找到这个网页(点击这里),打开自己的工作空间进行下载编译:

 

cd catkin_ws/src

git clone https://github.com/HumaRobotics/modbus.git

cd ..

catkin_make

 

首先将机械手爪设置为modbus/tcp的服务端,打开下载文件夹modbus/modbus_client.py文件

 

import rospy
from modbus.modbus_wrapper_client import ModbusWrapperClient 
from std_msgs.msg import Int32MultiArray as HoldingRegister
 
NUM_REGISTERS = 20
ADDRESS_READ_START = 40000
ADDRESS_WRITE_START = 40020
 
if __name__=="__main__":
    rospy.init_node("modbus_client")
    rospy.loginfo("""
    This file shows the usage of the Modbus Wrapper Client.
    To see the read registers of the modbus server use: rostopic echo /modbus_wrapper/input
    To see sent something to the modbus use a publisher on the topic /modbus_wrapper/output
    This file contains a sample publisher.
    """)
    host = "192.168.0.123"
    port = 502
    if rospy.has_param("~ip"):
        host =  rospy.get_param("~ip")
    else:
        rospy.loginfo("For not using the default IP %s, add an arg e.g.: '_ip:=\"192.168.0.199\"'",host)
    if rospy.has_param("~port"):
        port =  rospy.get_param("~port")
    else:
        rospy.loginfo("For not using the default port %d, add an arg e.g.: '_port:=1234'",port)
    # setup modbus client    
    modclient = ModbusWrapperClient(host,port=port,rate=50,reset_registers=False,sub_topic="modbus_wrapper/output",pub_topic="modbus_wrapper/input")
    modclient.setReadingRegisters(ADDRESS_READ_START,NUM_REGISTERS)
    modclient.setWritingRegisters(ADDRESS_WRITE_START,NUM_REGISTERS)
    rospy.loginfo("Setup complete")
    
    # start listening to modbus and publish changes to the rostopic
    modclient.startListening()
    rospy.loginfo("Listener started")
    
    #################
    # Example 1
    # Sets an individual register using the python interface, which can automatically be reset, if a timeout is given.
    register = 40020
    value = 1
    timeout = 0.5
    modclient.setOutput(register,value,timeout)
    rospy.loginfo("Set and individual output")
    #################
    
    
    
    #################
    # Example 2
    # Create a listener that show us a message if anything on the readable modbus registers change
    rospy.loginfo("All done. Listening to inputs... Terminate by Ctrl+c")
    def showUpdatedRegisters(msg):
        rospy.loginfo("Modbus server registers have been updated: %s",str(msg.data))
    sub = rospy.Subscriber("modbus_wrapper/input",HoldingRegister,showUpdatedRegisters,queue_size=500)
    #################
    
    #################
    # Example 3
    # writing to modbus registers using a standard ros publisher
    pub = rospy.Publisher("modbus_wrapper/output",HoldingRegister,queue_size=500)
    output = HoldingRegister()
    output.data = xrange(20,40)
    output2 = HoldingRegister()
    output2.data = xrange(40,20,-1)
    
    rospy.loginfo("Sending arrays to the modbus server")
    while not rospy.is_shutdown():
        rospy.sleep(1)
        pub.publish(output)
        rospy.sleep(1)
        pub.publish(output2)
    #################
    
    # Stops the listener on the modbus
    modclient.stopListening()

 

 

因为控制机械手爪这里只需要对3006单个寄存器进行写寄存器操作,因此将上面程序进行删减:

 

import rospy
from modbus.modbus_wrapper_client import ModbusWrapperClient 
from std_msgs.msg import Int32MultiArray as HoldingRegister
 
NUM_REGISTERS = 1
ADDRESS_WRITE_START = 3006
 
if __name__=="__main__":
    rospy.init_node("modbus_client")
    
    host = "192.168.1.110"
    port = 502
 
    # setup modbus client    
    modclient = ModbusWrapperClient(host,port=port,rate=50,reset_registers=False,sub_topic="modbus_wrapper/output",pub_topic="modbus_wrapper/input")
    modclient.setWritingRegisters(ADDRESS_WRITE_START,NUM_REGISTERS)
    rospy.loginfo("Setup complete")
    
    register = 3006
    value = 0
    timeout = 0
    modclient.setOutput(register,value,timeout)
    rospy.loginfo("Set and individual output")

 

 

使用modbus poll和modbus slave可以进行modbus模拟通信,发现该程序可以更改modbus slave中的数据,但是实际测试机械臂的手爪并不可以。使用modbus poll可以控制机械手爪,查看报文内容如下:

 

01 1A 00 00 00 06 01 06 0B BE 0F FF

 

而使用ROS发送给 modbus slave的报文内容无法直接查看,可以通过Wireshark进行抓包,查看报文内容,以下为抓取到的报文内容:

 

3

 

可以看出该段程序所产生的报文所使用的功能码为16:写多个寄存器,而我是写单个寄存器,通过分析 modbus_wrapper_client以及prmodbus,发现需要将程序这样简单修改即可:

 

import rospy
from modbus.modbus_wrapper_client import ModbusWrapperClient 
from pymodbus.client.sync import ModbusTcpClient
 
if __name__=="__main__":
    rospy.init_node("modbus_client")
    
    host = "192.168.1.110"
    port = 502
 
    # setup modbus client    
    modclient = ModbusWrapperClient(host,port=port,rate=50,reset_registers=False,sub_topic="modbus_wrapper/output",pub_topic="modbus_wrapper/input")
 
    #3006为地址,0为写入的内容
    modclient.client.write_register(3006,0)
    rospy.loginfo("Setup complete")

 

运行方法:

 

roscore

rosrun modbus modbus_client.py

发表评论

后才能评论