00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036 import roslib
00037 roslib.load_manifest('industrial_io_client')
00038 import rospy
00039 import yaml
00040
00041
00042 import socket
00043 import struct
00044
00045
00046 class Client:
00047 class DeviceMessage:
00048 def __init__(self):
00049 self.network_data_table = 0
00050 self.device_offset = 0
00051 self.bit_byte_offset = 0
00052 self.value = 0
00053
00054 class Input:
00055 def __init__(self):
00056 self.name = None
00057 self.type = None
00058 self.network_data_table_code = None
00059 self.device_offset_code = None
00060 self.bit_byte_offset_code = None
00061 self.value_code = None
00062 def __repr__(self):
00063 return self.__str__()
00064 def __str__(self):
00065 return '%s: %s %s %s %s %s'%(self.name, self.type, self.network_data_table_code, self.device_offset_code, self.bit_byte_offset_code, self.value_code)
00066
00067 class Output:
00068 def __init__(self):
00069 pass
00070
00071 class SocketClient:
00072 def __init__(self):
00073 self.host = None
00074 self.port = None
00075 self.socket = None
00076
00077 def __init__(self, config_file):
00078 self.inputs = {}
00079 self.outputs = {}
00080 self.device = None
00081 self.subscribers = []
00082 self.publishers = []
00083 self.load_config(config_file)
00084
00085 def load_config(self, config_file):
00086 try:
00087 data = yaml.load(open(config_file))
00088 except yaml.YAMLError as e:
00089 print('YAML parse error')
00090 raise
00091 if 'inputs' in data:
00092 for i in data['inputs']:
00093 inp = self.Input()
00094 if type(i) != dict or len(i) != 1:
00095 raise Exception('parse error')
00096 for k,v in i.iteritems():
00097 inp.name = rospy.resolve_name(k)
00098 if type(v) != dict or \
00099 not v.has_key('type') or \
00100 not v.has_key('transformation'):
00101 raise Exception('parse error')
00102 inp.type = v['type']
00103 tx = v['transformation']
00104 if type(tx) != dict or \
00105 not tx.has_key('network_data_table') or \
00106 not tx.has_key('device_offset') or \
00107 not tx.has_key('bit_byte_offset') or \
00108 not tx.has_key('value'):
00109 raise Exception('parse error')
00110 inp.network_data_table_code = tx['network_data_table']
00111 inp.device_offset_code = tx['device_offset']
00112 inp.bit_byte_offset_code = tx['bit_byte_offset']
00113 inp.value_code = tx['value']
00114 self.inputs[inp.name] = inp
00115 else:
00116 print('No inputs found in configuration; not subscribing to anything')
00117
00118 if 'outputs' in data:
00119 raise Exception('output support not implemented')
00120 else:
00121 print('No outputs found in configuration; not publishing anything')
00122
00123 if 'device' not in data:
00124 raise Exception('no device specified')
00125 dev = data['device']
00126 if type(dev) != dict or not dev.has_key('type'):
00127 raise Exception('parse error on device')
00128 typ = dev['type']
00129 if typ == 'generic_io_socket_server':
00130 if not dev.has_key('host') or not dev.has_key('port'):
00131 raise Exception('parse error on device')
00132 sc = self.SocketClient()
00133 sc.host = str(dev['host'])
00134 sc.port = int(dev['port'])
00135 self.device = sc
00136 else:
00137 raise Exception('unsupported device type')
00138
00139 def handle_ros_message(self, data):
00140 print 'Received message on topic %s (type %s)'%(data._connection_header['topic'], data._connection_header['type'])
00141 if data._connection_header['topic'] not in self.inputs:
00142 print self.inputs
00143 raise Exception('received message on unexpected topic')
00144 inp = self.inputs[data._connection_header['topic']]
00145
00146
00147 m = data
00148
00149 dev_msg = self.DeviceMessage()
00150 dev_msg.network_data_table = int(eval(str(inp.network_data_table_code)))
00151 dev_msg.device_offset = int(eval(str(inp.device_offset_code)))
00152 dev_msg.bit_byte_offset = int(eval(str(inp.bit_byte_offset_code)))
00153 dev_msg.value = int(eval(str(inp.value_code)))
00154
00155 self.write_to_device(dev_msg)
00156
00157 def initialize_device(self):
00158 self.device.socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
00159 print 'Connecting to server at %s:%d...'%(self.device.host, self.device.port)
00160 try:
00161 self.device.socket.connect((self.device.host, self.device.port))
00162 except socket.error as e:
00163 print 'Error connecting to server'
00164 raise
00165 print 'Connected.'
00166
00167 def write_to_device(self, dev_msg):
00168
00169
00170
00171
00172
00173
00174
00175
00176
00177
00178
00179
00180
00181
00182 length = 1*4+3*4+4*4;
00183 fmt_str = '<IIIIIIII'
00184
00185 msg_type_io = 20
00186 comm_type_topic = 1
00187 reply_type_invalid = 0
00188 buffer = struct.pack(fmt_str,
00189 length,
00190 msg_type_io,
00191 comm_type_topic,
00192 reply_type_invalid,
00193 dev_msg.network_data_table,
00194 dev_msg.device_offset,
00195 dev_msg.bit_byte_offset,
00196 dev_msg.value)
00197 self.device.socket.send(buffer)
00198
00199 def read_from_device(self):
00200 pass
00201
00202 def initialize_ros(self):
00203 for inp in self.inputs.values():
00204 type_split = inp.type.split('/')
00205 if len(type_split) != 2:
00206 raise Exception('invalid type')
00207 module = type_split[0]
00208 roslib.load_manifest(module)
00209 module += '.msg'
00210 message = type_split[1]
00211 mod = __import__(module, globals(), locals(), [message])
00212 print("Subscribing to %s (%s)"%(inp.name, inp.type))
00213 self.subscribers.append(rospy.Subscriber(inp.name, getattr(mod, message), self.handle_ros_message))
