def getData(self):
global tfclient
msg = ''
try:
self.cond.acquire()
if not self.initialized:
return msg
self.cond.wait()
if self.topic_type == "sensor_msgs/CompressedImage" or self.topic_type == "sensor_msgs/Image":
msg = img2json(self.last_message, self.topic_type)
elif self.topic_type == "visualization_msgs/Polyline":
frameId = self.last_message.header.frame_id
msg = '{ "points" : ['
for i in range(len(self.last_message.points)):
point = geometry_msgs.msg.PointStamped()
point.header.stamp = self.last_message.header.stamp
point.header.frame_id = frameId
point.point.x = self.last_message.points[i].x
point.point.y = self.last_message.points[i].y
point.point.z = self.last_message.points[i].z
point = tfclient.transformPoint("/map", point)
msg += '{"x": "%f",' % point.point.x
msg += '"y": "%f",' % point.point.y
msg += '"z": "%f"},' % point.point.z
msg += ']}'
elif self.topic_type == "diagnostic_msgs/DiagnosticMessage":
msg = '{ "name": "%s",' % self.qdict['name'][0]
msg += '"label": "%s",' % self.qdict['label'][0]
msg += '"value": "%s"}' % self.last_message
else:
msg = rosjson.ros_message_to_json(self.last_message)
finally:
self.cond.release()
return msg
def _do_GET_topic(self, topics, since, nowait=False, callback=None):
rwttopics = []
for topic in topics:
rwt = self.server.factory.get(topic)
if rwt:
rwttopics.append((topic, rwt))
else:
rwt = self.subscribe(topic)
if rwt: rwttopics.append((topic, rwt))
if len(rwttopics) == 0:
rospy.logwarn("no valid topics")
self.send_failure()
return
(messages, lastSince) = self._get_messages(rwttopics, since, nowait)
buf = cStringIO.StringIO()
buf.write("{")
buf.write('"since": %s,' % lastSince)
buf.write('"msgs": [')
_i = 0
for (topic, msg) in messages:
_i = _i + 1
if _i > 1: buf.write(',')
buf.write("{")
buf.write('"topic": "%s",' % topic)
buf.write('"msg": ')
buf.write(rosjson.ros_message_to_json(msg))
buf.write("}")
buf.write(']')
buf.write('}')
buf = buf.getvalue()
self.send_success(buf, callback)
def _do_GET_topic(self, topics, since, nowait=False, callback=None):
rwttopics = []
for topic in topics:
rwt = self.server.factory.get(topic)
if rwt:
rwttopics.append((topic, rwt))
else:
rwt = self.subscribe(topic)
if rwt: rwttopics.append((topic, rwt))
if len(rwttopics) == 0:
rospy.logwarn("no valid topics")
self.send_failure()
return
(messages, lastSince) = self._get_messages(rwttopics, since, nowait)
buf = cStringIO.StringIO()
buf.write("{")
buf.write('"since": %s,' % lastSince)
buf.write('"msgs": [')
_i = 0
for (topic, msg) in messages:
_i = _i + 1
if _i > 1: buf.write(',')
buf.write("{")
buf.write('"topic": "%s",' % topic)
buf.write('"msg": ')
buf.write(rosjson.ros_message_to_json(msg))
buf.write("}")
buf.write(']')
buf.write('}')
buf = buf.getvalue()
self.send_success(buf, callback)
def handle_ROS(self, path, qdict):
cstring = self.headers.get('Cookie', '')
cookie = Cookie.BaseCookie()
cookie.load(cstring)
path_parts = path.split("/")
username = ""
cmd = path_parts[2]
## parse out the username from the cookie
if cookie.has_key('MB_L1'):
l1 = cookie['MB_L1'].value
if l1.startswith("V1/"):
code = l1[3:]
parts = code.split(":", 1)
username = parts[0]
## Make a service call
## Args can be passed as an ordered list of parameters:
## /ros/service/<service_name>?args=<arg1>&args=<arg2>&<...>
## or as a dictionary of named parameters
## /ros/service/<service_name>?json=<json-encoded dictionary of args>
if cmd == "ping":
self.send_success("{'ping': %f}" % time.time())
if cmd == "service":
name = "/".join(path_parts[3:])
callback = qdict.get("callback", [''])[0]
try:
service_class = rosservice.get_service_class_by_name("/" +
name)
except ROSServiceException:
self.send_failure()
return
request = service_class._request_class()
args = qdict.get('args', None)
if not args:
args = qdict.get('json', ['{}'])[0]
args = eval(args)
try:
now = rospy.get_rostime()
keys = {'now': now, 'auto': rospy.Header(stamp=now)}
roslib.message.fill_message_args(request, args, keys=keys)
except MessageException:
raise ROSServiceException("Not enough arguments to call service.\n"+\
"Args should be: [%s], but are actually [%s] " %
(roslib.message.get_printable_message_args(request), args))
rospy.logdebug("service call: name=%s, args=%s" % (name, args))
rospy.wait_for_service(name)
service_proxy = rospy.ServiceProxy(name, service_class)
try:
msg = service_proxy(request)
except rospy.ServiceException:
self.send_failure()
return
msg = rosjson.ros_message_to_json(msg)
rospy.logdebug("service call: name=%s, resp=%s" % (name, msg))
self.send_success(msg, callback=callback)
return
if cmd == "publish":
topic = string.join(path_parts[3:], "/")
callback = qdict.get("callback", [''])[0]
topic_type = qdict.get("topic_type", [''])[0]
message_klass = roslib.message.get_message_class(topic_type)
message = message_klass()
args = qdict.get('args', None)
if not args:
args = qdict.get('json', ['{}'])[0]
args = eval(args)
try:
now = rospy.get_rostime()
keys = {'now': now, 'auto': rospy.Header(stamp=now)}
roslib.message.fill_message_args(message, args, keys=keys)
except MessageException:
raise ROSServiceException("Not enough arguments to call service.\n"+\
"Args should be: [%s], but are actually [%s] " %
(roslib.message.get_printable_message_args(message), args))
rospy.logdebug("publish: topic=%s, topic_type=%s, args=%s" %
(topic, topic_type, args))
pub = rospy.Publisher(topic, message_klass, latch=True)
try:
pub.publish(message)
except rospy.ServiceException:
self.send_failure()
return
self.send_success(callback=callback)
return
if cmd == "receive":
since = int(qdict.get('since', ['0'])[0])
topics = qdict.get("topic", "")
callback = qdict.get("callback", [''])[0]
nowait = int(qdict.get('nowait', ['0'])[0])
self._do_GET_topic(topics, since, nowait=nowait, callback=callback)
return
def handle_ROS(self, path, qdict):
cstring = self.headers.get('Cookie', '')
cookie = Cookie.BaseCookie()
cookie.load(cstring)
path_parts = path.split("/")
username = ""
cmd = path_parts[2]
## parse out the username from the cookie
if cookie.has_key('MB_L1'):
l1 = cookie['MB_L1'].value
if l1.startswith("V1/"):
code = l1[3:]
parts = code.split(":", 1)
username = parts[0]
## Make a service call
## Args can be passed as an ordered list of parameters:
## /ros/service/<service_name>?args=<arg1>&args=<arg2>&<...>
## or as a dictionary of named parameters
## /ros/service/<service_name>?json=<json-encoded dictionary of args>
if cmd == "ping":
self.send_success("{'ping': %f}" % time.time())
if cmd == "service":
name = "/".join(path_parts[3:])
callback = qdict.get("callback", [''])[0]
try:
service_class = rosservice.get_service_class_by_name("/" + name)
except ROSServiceException:
self.send_failure()
return
request = service_class._request_class()
args = qdict.get('args', None)
if not args:
args = qdict.get('json', ['{}'])[0]
args = eval(args)
try:
now = rospy.get_rostime()
keys = { 'now': now, 'auto': rospy.Header(stamp=now) }
roslib.message.fill_message_args(request, args, keys=keys)
except MessageException:
raise ROSServiceException("Not enough arguments to call service.\n"+\
"Args should be: [%s], but are actually [%s] " %
(roslib.message.get_printable_message_args(request), args))
rospy.logdebug("service call: name=%s, args=%s" % (name, args))
rospy.wait_for_service(name)
service_proxy = rospy.ServiceProxy(name, service_class)
try:
msg = service_proxy(request)
except rospy.ServiceException:
self.send_failure()
return
msg = rosjson.ros_message_to_json(msg)
rospy.logdebug("service call: name=%s, resp=%s" % (name, msg))
self.send_success(msg, callback=callback)
return
if cmd == "publish":
topic = string.join(path_parts[3:], "/")
callback = qdict.get("callback", [''])[0]
topic_type = qdict.get("topic_type", [''])[0]
message_klass = roslib.message.get_message_class(topic_type)
message = message_klass()
args = qdict.get('args', None)
if not args:
args = qdict.get('json', ['{}'])[0]
args = eval(args)
try:
now = rospy.get_rostime()
keys = { 'now': now, 'auto': rospy.Header(stamp=now) }
roslib.message.fill_message_args(message, args, keys=keys)
except MessageException:
raise ROSServiceException("Not enough arguments to call service.\n"+\
"Args should be: [%s], but are actually [%s] " %
(roslib.message.get_printable_message_args(message), args))
rospy.logdebug("publish: topic=%s, topic_type=%s, args=%s" % (topic, topic_type, args))
pub = rospy.Publisher(topic, message_klass, latch=True)
try:
pub.publish(message)
except rospy.ServiceException:
self.send_failure()
return
self.send_success(callback=callback)
return
if cmd == "receive":
since = int(qdict.get('since', ['0'])[0])
topics = qdict.get("topic", "")
callback = qdict.get("callback", [''])[0]
nowait = int(qdict.get('nowait', ['0'])[0])
self._do_GET_topic(topics, since, nowait=nowait, callback=callback)
return
def handle_ROS(self, path, qdict):
cstring = self.headers.get('Cookie', '')
cookie = Cookie.BaseCookie()
cookie.load(cstring)
path_parts = path.split("/")
username = ""
cmd = path_parts[2]
## parse out the username from the cookie
if cookie.has_key('MB_L1'):
l1 = cookie['MB_L1'].value
if l1.startswith("V1/"):
code = l1[3:]
parts = code.split(":", 1)
username = parts[0]
## Make a service call
## Args can be passed as an ordered list of parameters:
## /ros/service/<service_name>?args=<arg1>&args=<arg2>&<...>
## or as a dictionary of named parameters
## /ros/service/<service_name>?json=<json-encoded dictionary of args>
if cmd == "service":
name = path_parts[3]
service_class = rosservice.get_service_class_by_name("/" + name)
request = service_class._request_class()
callback = qdict.get("callback", [''])[0]
args = qdict.get('args', None)
if not args:
args = qdict.get('json', ['{}'])[0]
args = eval(args)
try:
roslib.message.fill_message_args(request, args)
except ROSMessageException:
raise ROSServiceException("Not enough arguments to call service.\n"+\
"Args should be: [%s], but are actually [%s] " %
(roslib.message.get_printable_message_args(request), args))
logging.debug("service call: name=%s, args=%s" % (name, args))
rospy.wait_for_service(name)
service_proxy = rospy.ServiceProxy(name, service_class)
try:
msg = service_proxy(request)
except rospy.ServiceException:
self.send_failure()
return
msg = rosjson.ros_message_to_json(msg)
logging.debug("service call: name=%s, resp=%s" % (name, msg))
self.send_success(msg, callback=callback)
return
if cmd == "receive":
since = int(qdict.get('since', ['0'])[0])
topics = qdict.get("topic", "")
callback = qdict.get("callback", [''])[0]
self._do_GET_topic(topics, since, callback)
return