utils.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 #
3 # License: BSD
4 # https://raw.github.com/robotics-in-concert/rocon_multimaster/license/LICENSE
5 #
6 ##############################################################################
7 # Imports
8 ##############################################################################
9 
10 import copy
11 import cPickle as pickle
12 import os
13 import re
14 
15 import rospy
16 
17 from Crypto.PublicKey import RSA
18 import Crypto.Util.number as CUN
19 
20 import gateway_msgs.msg as gateway_msgs
21 
22 ##############################################################################
23 # Constants
24 ##############################################################################
25 
26 # for help in iterating over the set of connection constants
27 connection_types = frozenset([gateway_msgs.ConnectionType.PUBLISHER, gateway_msgs.ConnectionType.SUBSCRIBER,
28  gateway_msgs.ConnectionType.SERVICE, gateway_msgs.ConnectionType.ACTION_CLIENT, gateway_msgs.ConnectionType.ACTION_SERVER])
29 connection_types_list = [gateway_msgs.ConnectionType.PUBLISHER, gateway_msgs.ConnectionType.SUBSCRIBER,
30  gateway_msgs.ConnectionType.SERVICE, gateway_msgs.ConnectionType.ACTION_CLIENT, gateway_msgs.ConnectionType.ACTION_SERVER]
31 connection_type_strings_list = ["publisher", "subscriber", "service", "action_client", "action_server"]
32 action_types = ['/goal', '/cancel', '/status', '/feedback', '/result']
33 
34 ##############################################################################
35 # Rule
36 ##############################################################################
37 
38 
39 class Connection():
40 
41  '''
42  An object that represents a connection containing all the gory details
43  about a connection, allowing a connection to be passed through to a
44  foreign gateway
45 
46  - rule (gateway_msgs.msg.Rule) (containing type,name,node)
47  - type_info (msg type for pubsub or service api for services)
48  - xmlrpc_uri (the xmlrpc node uri for the connection)
49  '''
50 
51  def __init__(self, rule, type_msg, type_info, xmlrpc_uri):
52  """
53  @param type_msg : message type of the topic or service
54  @param type_info : either topic_type (pubsub), service api (service) or ??? (action)
55  @type string
56  """
57  self.rule = rule
58  self.type_msg = type_msg
59  self.type_info = type_info
60  self.xmlrpc_uri = xmlrpc_uri
61 
62  def __eq__(self, other):
63  if isinstance(other, self.__class__):
64  return self.__dict__ == other.__dict__
65  else:
66  return False
67 
68  def __ne__(self, other):
69  return not self.__eq__(other)
70 
71  def __hash__(self):
72  return hash(((self.rule.type, self.rule.name, self.rule.node), self.type_msg, self.type_info, self.xmlrpc_uri))
73 
74  def __str__(self):
75  if self.rule.type == gateway_msgs.ConnectionType.SERVICE:
76  return '{type: %s, name: %s, node: %s, uri: %s, service_api: %s, service_type: %s}' % (
77  self.rule.type, self.rule.name, self.rule.node, self.xmlrpc_uri, self.type_info, self.type_msg)
78  else:
79  return '{type: %s, name: %s, node: %s, uri: %s, topic_type: %s}' % (
80  self.rule.type, self.rule.name, self.rule.node, self.xmlrpc_uri, self.type_info)
81 
82  def __repr__(self):
83  return self.__str__()
84 
85  def inConnectionList(self, connection_list):
86  '''
87  Checks to see if this connection has the same rule
88  as an item in the given connection_list
89 
90  @param connection_list : connection list to trawl.
91  @type utils.Connection[]
92  @return true if this connection rule matches a connection rule in the list
93  @rtype Bool
94  '''
95  for connection in connection_list:
96  if self.hasSameRule(connection):
97  return True
98  return False
99 
100  def hasSameRule(self, connection):
101  '''
102  Checks for equivalency regardless of type_info and xmlrpc_uri.
103 
104  @param connection : connection to compare with
105  @type utils.Connection
106  @return true if equivalent, false otherwise
107  @rtype Bool
108  '''
109  return (self.rule.name == connection.rule.name and
110  self.rule.type == connection.rule.type and
111  self.rule.node == connection.rule.node)
112 
113 ##############################################################################
114 # Registration
115 ##############################################################################
116 
117 
118 class Registration():
119 
120  '''
121  An object that represents a connection registered with the local
122  master (or about to be registered). This has all the gory detail
123  for the connection. It includes the gateway name it originated
124  from as well as master registration information.
125 
126  - connection (the remote connection information)
127  - remote_gateway (the remote gateway from where this connection originated)
128  - local_node (the local anonymously generated node name)
129  '''
130 
131  def __init__(self, connection, remote_gateway, local_node=None):
132  '''
133  @param connection : string identifier storing the remote connection details (type, name, node)
134  @type gateway_msgs.msg.RemoteRule
135 
136  @param remote_gateway : string identifier for where this registration game from
137  @type string
138 
139  @param local_node : the local node that this registration is created under
140  @type string
141  '''
142  self.connection = connection
143  self.remote_gateway = remote_gateway
144  self.local_node = local_node
145 
146  def __eq__(self, other):
147  if isinstance(other, self.__class__):
148  return self.__dict__ == other.__dict__
149  else:
150  return False
151 
152  def __ne__(self, other):
153  return not self.__eq__(other)
154 
155  def __str__(self):
156  return '[%s]%s' % (self.remote_gateway, format_rule(self.connection.rule))
157 
158  def __repr__(self):
159  return self.__str__()
160 
161 ##########################################################################
162 # serialization/deserialization Functions
163 ##########################################################################
164 
165 
166 # def convert(data):
167 # '''
168 # Convert unicode to standard string (Not sure how necessary this is)
169 # http://stackoverflow.com/questions/1254454/fastest-way-to-convert-a-dicts-keys-values-from-unicode-to-str
170 # '''
171 # if isinstance(data, unicode):
172 # return str(data)
173 # elif isinstance(data, collections.Mapping):
174 # return dict(map(convert, data.iteritems()))
175 # elif isinstance(data, collections.Iterable):
176 # return type(data)(map(convert, data))
177 # else:
178 # return data
179 #
180 
181 def serialize(data):
182  # return json.dumps(data)
183  return pickle.dumps(data)
184 
185 
186 def deserialize(str_msg):
187  # return convert(json.loads(str_msg))
188  try:
189  deserialized_data = pickle.loads(str_msg)
190  except ValueError as e:
191  rospy.logwarn("Gateway : error in deserialization [%s]" % e)
192  import traceback
193  print(traceback.format_exc())
194  print("Data : %s" % str_msg)
195  return deserialized_data
196 
197 
198 # TODO :pickle API support directly in Connection object instead of here to make it more visible.
199 def serialize_connection(connection):
200  return serialize([connection.rule.type,
201  connection.rule.name,
202  connection.rule.node,
203  connection.type_msg,
204  connection.type_info,
205  connection.xmlrpc_uri]
206  )
207 
208 
209 def deserialize_connection(connection_str):
210  deserialized_list = deserialize(connection_str)
211  rule = gateway_msgs.Rule(deserialized_list[0],
212  deserialized_list[1],
213  deserialized_list[2]
214  )
215  return Connection(rule, deserialized_list[3], deserialized_list[4], deserialized_list[5])
216 
217 
218 def serialize_connection_request(command, source, connection):
219  return serialize([command, source,
220  connection.rule.type,
221  connection.rule.name,
222  connection.rule.node,
223  connection.type_msg,
224  connection.type_info,
225  connection.xmlrpc_uri]
226  )
227 
228 
229 def serialize_rule_request(command, source, rule):
230  return serialize([command, source, rule.type, rule.name, rule.node])
231 
232 
233 def deserialize_request(request_str):
234  deserialized_list = deserialize(request_str)
235  return deserialized_list[0], deserialized_list[1], deserialized_list[2:]
236 
237 
238 def get_connection_from_list(connection_argument_list):
239  rule = gateway_msgs.Rule(connection_argument_list[0], connection_argument_list[1], connection_argument_list[2])
240  return Connection(rule, connection_argument_list[3], connection_argument_list[4], connection_argument_list[5])
241 
242 
243 def get_rule_from_list(rule_argument_list):
244  return gateway_msgs.Rule(rule_argument_list[0], rule_argument_list[1], rule_argument_list[2])
245 
246 ##########################################################################
247 # Encryption/Decryption
248 ##########################################################################
249 
250 MAX_PLAINTEXT_LENGTH = 256
251 
252 
254  key = RSA.generate(8 * MAX_PLAINTEXT_LENGTH)
255  public_key = key.publickey()
256  return key, public_key
257 
258 
259 def deserialize_key(serialized_key):
260  return RSA.importKey(serialized_key)
261 
262 
263 def serialize_key(key):
264  return key.exportKey()
265 
266 
267 def encrypt(plaintext, public_key):
268  if len(plaintext) > MAX_PLAINTEXT_LENGTH:
269  # TODO need to have arbitrary lengths
270  raise ValueError('Trying to encrypt text longer than ' + MAX_PLAINTEXT_LENGTH + ' bytes!')
271  K = CUN.getRandomNumber(128, os.urandom) # Not used, legacy compatibility
272  ciphertext = public_key.encrypt(plaintext, K)
273  return ciphertext[0]
274  # return plaintext
275 
276 
277 def decrypt(ciphertext, key):
278  return key.decrypt(ciphertext)
279  # return ciphertext
280 
281 
282 def decrypt_connection(connection, key):
283  decrypted_connection = copy.deepcopy(connection)
284  decrypted_connection.type_info = decrypt(connection.type_info, key)
285  decrypted_connection.xmlrpc_uri = decrypt(connection.xmlrpc_uri, key)
286  return decrypted_connection
287 
288 
289 def encrypt_connection(connection, key):
290  encrypted_connection = copy.deepcopy(connection)
291  encrypted_connection.type_info = encrypt(connection.type_info, key)
292  encrypted_connection.xmlrpc_uri = encrypt(connection.xmlrpc_uri, key)
293  return encrypted_connection
294 
295 ##########################################################################
296 # Regex
297 ##########################################################################
298 
299 
300 def is_all_pattern(pattern):
301  '''
302  Convenience function for detecting the 'all' pattern.
303 
304  @param pattern : the name rule string for the flip all concept
305  @type str
306  @return true if matching, false otherwise
307  @rtype Bool
308  '''
309  if pattern == ".*":
310  return True
311  else:
312  return False
313 
314 
315 ##########################################################################
316 # Formatters
317 ##########################################################################
318 
319 def format_rule(rule):
320  return '[%s][%s][%s]' % (rule.type, rule.name, rule.node)
321 
322 ##########################################################################
323 # Factories
324 ##########################################################################
325 
326 
327 def create_empty_connection_type_dictionary(collection_type=list):
328  """
329  Used to initialise a dictionary with rule type keys
330  and empty lists or sets (or whatever collection_type passed as argument).
331  """
332  dic = {}
333  for connection_type in connection_types:
334  dic[connection_type] = collection_type()
335  return dic
336 
337 difflist = lambda l1, l2: [x for x in l1 if x not in l2] # diff of lists
338 
339 ##########################################################################
340 # Conversion from Connection Cache Proxy channels (as passed in callback)
341 # to Gateway connections
342 ##########################################################################
343 
344 
345 def _get_connections_from_service_chan_dict(ccproxy_channel_dict, connection_type):
346  connections = set()
347  for name, service in ccproxy_channel_dict.iteritems():
348  service_name = service.name
349  service_type = service.type
350  service_uri = service.xmlrpc_uri
351  nodes = service.nodes
352  for node in nodes:
353  connection = Connection(gateway_msgs.Rule(connection_type, service_name, node[0]), service_type, service_uri, node[1])
354  connections.add(connection)
355  return connections
356 
357 
358 def _get_connections_from_pub_sub_chan_dict(ccproxy_channel_dict, connection_type):
359  connections = set()
360  for name, topic in ccproxy_channel_dict.iteritems():
361  topic_name = topic.name
362  topic_type = topic.type
363  nodes = topic.nodes
364  for node in nodes:
365  connection = Connection(gateway_msgs.Rule(connection_type, topic_name, node[0]), topic_type, topic_type, node[1])
366  connections.add(connection)
367  return connections
368 
369 
370 def _get_connections_from_action_chan_dict(ccproxy_channel_dict, connection_type):
371  connections = set()
372  for name, action in ccproxy_channel_dict.iteritems():
373  action_name = action.name
374  goal_topic_type = action.type
375  nodes = action.nodes
376  for node in nodes:
377  connection = Connection(gateway_msgs.Rule(connection_type, action_name, node[0]), goal_topic_type, goal_topic_type, node[1]) # node_uri
378  connections.add(connection)
379  return connections
def decrypt(ciphertext, key)
Definition: utils.py:277
def __eq__(self, other)
Definition: utils.py:62
def is_all_pattern(pattern)
Regex.
Definition: utils.py:300
def create_empty_connection_type_dictionary(collection_type=list)
Factories.
Definition: utils.py:327
def decrypt_connection(connection, key)
Definition: utils.py:282
def _get_connections_from_action_chan_dict(ccproxy_channel_dict, connection_type)
Definition: utils.py:370
def __init__(self, connection, remote_gateway, local_node=None)
Definition: utils.py:131
def serialize(data)
Definition: utils.py:181
def hasSameRule(self, connection)
Definition: utils.py:100
def get_rule_from_list(rule_argument_list)
Definition: utils.py:243
def __init__(self, rule, type_msg, type_info, xmlrpc_uri)
Definition: utils.py:51
def generate_private_public_key()
Definition: utils.py:253
def serialize_connection_request(command, source, connection)
Definition: utils.py:218
def deserialize(str_msg)
Definition: utils.py:186
def serialize_connection(connection)
Definition: utils.py:199
def format_rule(rule)
Formatters.
Definition: utils.py:319
def __ne__(self, other)
Definition: utils.py:68
def __ne__(self, other)
Definition: utils.py:152
def deserialize_request(request_str)
Definition: utils.py:233
def get_connection_from_list(connection_argument_list)
Definition: utils.py:238
def _get_connections_from_pub_sub_chan_dict(ccproxy_channel_dict, connection_type)
Definition: utils.py:358
def encrypt_connection(connection, key)
Definition: utils.py:289
def serialize_rule_request(command, source, rule)
Definition: utils.py:229
def inConnectionList(self, connection_list)
Definition: utils.py:85
def serialize_key(key)
Definition: utils.py:263
def deserialize_key(serialized_key)
Definition: utils.py:259
def encrypt(plaintext, public_key)
Definition: utils.py:267
def __eq__(self, other)
Definition: utils.py:146
def deserialize_connection(connection_str)
Definition: utils.py:209
def _get_connections_from_service_chan_dict(ccproxy_channel_dict, connection_type)
Conversion from Connection Cache Proxy channels (as passed in callback) to Gateway connections...
Definition: utils.py:345


rocon_gateway
Author(s): Daniel Stonier , Jihoon Lee , Piyush Khandelwal
autogenerated on Mon Jun 10 2019 14:40:10