ROS program for SRI force sensor

Code

The official codes for obtaining the sample data from SRI force sensor are based on the Windows environment. Now, I have modified and changed it into python, incorporating ROS library to make it more practical.

from socket import *
import struct
import rospy
from geometry_msgs.msg import WrenchStamped
import time

import numpy as np
import sys

HOST = '192.168.0.108'
PORT = 4008
BUFSIZ = 8192  
ADDR = (HOST, PORT)

tcpClientSock = socket(AF_INET, SOCK_STREAM)
tcpClientSock.connect(ADDR)

data_ft=[0, 0, 0, 0, 0, 0]

def ft_pub():
    pub = rospy.Publisher('SRI_force_topic',WrenchStamped,queue_size=10)
    rospy.init_node('SRI_node',anonymous=True)
    rate = rospy.Rate(500)

    while not rospy.is_shutdown():

        tcpClientSock.send(b'AT+GOD\r\n')
        data_raw = tcpClientSock.recv(BUFSIZ)
        
        force = []

        if not data_raw:
            break
        # if(hex(data_raw[0]) == hex(0xAA)) and (hex(data_raw[1]) == hex(0x55)):
        #     print("The data is valid!\n")
        index = 6
        for i in range(6):
            value = struct.unpack('f',data_raw[index:index+4])
            force.append(value)
            index += 4
        # squash the last dimension
        force = np.squeeze(force)
            
        fs = WrenchStamped()
        fs.wrench.force.x = float(force[0])
        fs.wrench.force.y  = float(force[1])
        fs.wrench.force.z  = float(force[2])
        fs.wrench.torque.x = float(force[3])
        fs.wrench.torque.y = float(force[4])
        fs.wrench.torque.z = float(force[5])
        fs.header.stamp = rospy.Time.now()
        pub.publish(fs)
        # print(FT)
        sys.stdout.write('fx: %4f,fy: %4f,fz: %4f,rx: %4f,ry: %4f,rz: %4f'%(force[0],force[1],force[2],force[3],force[4],force[5]))
        sys.stdout.write('\r')
        sys.stdout.flush()

        rate.sleep()

    tcpClientSock.close()

if __name__=='__main__':
    try:
        ft_pub()
    except rospy.ROSInterruptException:
        pass
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66