rosbridge.py
Go to the documentation of this file.
1 import websocket
2 import thread
3 
4 import json
5 import traceback
6 import time
7 
8 import string
9 import random
10 
11 
13  def __init__(self, host, port):
14  self.callbacks = {}
16  self.resp = None
17  self.connection = RosbridgeWSConnection(host, port)
18  self.connection.registerCallback(self.onMessageReceived)
19 
20  def publish(self, topic, obj):
21  pub = {"op": "publish", "topic": topic, "msg": obj}
22  self.send(pub)
23 
24  def subscribe(self, topic, callback, throttle_rate=-1):
25  if self.addCallback(topic, callback):
26  sub = {"op": "subscribe", "topic": topic}
27  if throttle_rate > 0:
28  sub['throttle_rate'] = throttle_rate
29 
30  self.send(sub)
31 
32  def unhook(self, callback):
33  keys_for_deletion = []
34  for key, values in self.callbacks.iteritems():
35  for value in values:
36  if callback == value:
37  print "Found!"
38  values.remove(value)
39  if len(values) == 0:
40  keys_for_deletion.append(key)
41 
42  for key in keys_for_deletion:
43  self.unsubscribe(key)
44  self.callbacks.pop(key)
45 
46  def unsubscribe(self, topic):
47  unsub = {"op": "unsubscribe", "topic": topic}
48  self.send(unsub)
49 
50  def callService(self, serviceName, callback=None, msg=None):
51  id = self.generate_id()
52  call = {"op": "call_service", "id": id, "service": serviceName}
53  if msg != None:
54  call['args'] = msg
55 
56  if callback == None:
57  self.resp = None
58  def internalCB(msg):
59  self.resp = msg
60  return None
61 
62  self.addServiceCallback(id, internalCB)
63  self.send(call)
64 
65  while self.resp == None:
66  time.sleep(0.01)
67 
68  return self.resp
69 
70  self.addServiceCallback(id, callback)
71  self.send(call)
72  return None
73 
74  def send(self, obj):
75  try:
76  self.connection.sendString(json.dumps(obj))
77  except:
78  traceback.print_exc()
79  raise
80 
81  def generate_id(self, chars=16):
82  return ''.join(random.SystemRandom().choice(string.ascii_letters + string.digits) for _ in range(chars))
83 
84  def addServiceCallback(self, id, callback):
85  self.service_callbacks[id] = callback
86 
87  def addCallback(self, topic, callback):
88  if self.callbacks.has_key(topic):
89  self.callbacks[topic].append(callback)
90  return False
91 
92  self.callbacks[topic] = [callback]
93  return True
94 
95  def is_connected(self):
96  return self.connection.connected
97 
98  def is_errored(self):
99  return self.connection.errored
100 
101  def onMessageReceived(self, message):
102  try:
103  # Load the string into a JSON object
104  obj = json.loads(message)
105  # print "Received: ", obj
106 
107  if obj.has_key('op'):
108  option = obj['op']
109  if option == "publish": # A message from a topic we have subscribed to..
110  topic = obj["topic"]
111  msg = obj["msg"]
112  if self.callbacks.has_key(topic):
113  for callback in self.callbacks[topic]:
114  try:
115  callback(msg)
116  except:
117  print "exception on callback", callback, "from", topic
118  traceback.print_exc()
119  raise
120  elif option == "service_response":
121  if obj.has_key("id"):
122  id = obj["id"]
123  values = obj["values"]
124  if self.service_callbacks.has_key(id):
125  try:
126  #print 'id:', id, 'func:', self.service_callbacks[id]
127  self.service_callbacks[id](values)
128  except:
129  print "exception on callback ID:", id
130  traceback.print_exc()
131  raise
132  else:
133  print "Missing ID!"
134  else:
135  print "Recieved unknown option - it was: ", option
136  else:
137  print "No OP key!"
138  except:
139  print "exception in onMessageReceived"
140  print "message", message
141  traceback.print_exc()
142  raise
143 
144 
146  def __init__(self, host, port):
147  self.ws = websocket.WebSocketApp(("ws://%s:%d/" % (host, port)), on_message=self.on_message, on_error=self.on_error, on_close=self.on_close)
148  self.ws.on_open = self.on_open
149  self.run_thread = thread.start_new_thread(self.run, ())
150  self.connected = False
151  self.errored = False
152  self.callbacks = []
153 
154  def on_open(self, ws):
155  print "### ROS bridge connected ###"
156  self.connected=True
157 
158  def sendString(self, message):
159  if not self.connected:
160  print "Error: not connected, could not send message"
161  # TODO: throw exception
162  else:
163  self.ws.send(message)
164 
165  def on_error(self, ws, error):
166  self.errored = True
167  print "Error: %s" % error
168 
169  def on_close(self, ws):
170  self.connected = False
171  print "### ROS bridge closed ###"
172 
173  def run(self, *args):
174  self.ws.run_forever()
175 
176  def on_message(self, ws, message):
177  # Call the handlers
178  for callback in self.callbacks:
179  callback(message)
180 
181  def registerCallback(self, callback):
182  self.callbacks.append(callback)
def unhook(self, callback)
Definition: rosbridge.py:32
def onMessageReceived(self, message)
Definition: rosbridge.py:101
def addServiceCallback(self, id, callback)
Definition: rosbridge.py:84
def subscribe(self, topic, callback, throttle_rate=-1)
Definition: rosbridge.py:24
def addCallback(self, topic, callback)
Definition: rosbridge.py:87
def generate_id(self, chars=16)
Definition: rosbridge.py:81
def __init__(self, host, port)
Definition: rosbridge.py:13
def callback(self, msg_dict)
doesn't work: once ubsubscribed, robot doesn't let us subscribe again if self.pub.get_num_connections() == 0: rospy.loginfo("[%s] stopping to stream messages on topic '%s'", rospy.get_name(), self.topic_config.topic) self.robot.unsubscribe(topic=('/' + self.topic_config.topic))
Definition: mir_bridge.py:246
def publish(self, topic, obj)
Definition: rosbridge.py:20
def callService(self, serviceName, callback=None, msg=None)
Definition: rosbridge.py:50


mir_driver
Author(s): Martin G√ľnther
autogenerated on Wed May 8 2019 02:38:53