def test_anymsg(self): # rospy.AnyMsg really only exists at the client level as nothing within # rospy uses its functionality. try: from cStringIO import StringIO except ImportError: from io import StringIO import rospy import rospy.exceptions #trip wires against AnyMsg API m = rospy.AnyMsg() try: m.serialize(StringIO()) self.fail("AnyMsg should not allow serialization") except rospy.exceptions.ROSException: pass teststr = 'foostr-%s' % time.time() m.deserialize(teststr) self.assertEquals(teststr, m._buff) #test AnyMsg ctor error checking try: m = rospy.AnyMsg('foo') self.fail("AnyMsg ctor should not allow args") except: pass
def _threadedCall(self, msg): rosMsg = rospy.AnyMsg() rosMsg._buff = msg rospy.wait_for_service(self._addr[1], timeout=5) serviceFunc = rospy.ServiceProxy(self._addr[1], self._srvCls) return serviceFunc(rosMsg)
def _send(self, msg, msgID, protocol, remoteID): rosMsg = rospy.AnyMsg() rosMsg._buff = msg try: self._publisher.publish(rosMsg) except rospy.ROSInterruptException: pass # TODO: How should the error be returned? except rospy.ROSSerializationException: pass # TODO: How should the error be returned?
def _rceCB(self, resp, event): """ Internally used method to send received message to the ROS Service as response. """ rosResp = rospy.AnyMsg() if _GZIP_LVL: rosResp._buff = zlib.decompress(resp.getvalue()) else: rosResp._buff = resp.getvalue() event.set(rosResp)
def _rceCB(self, msg): """ Internally used method to send received messages to the ROS Publisher. """ rosMsg = rospy.AnyMsg() if _GZIP_LVL: rosMsg._buff = zlib.decompress(msg.getvalue()) else: rosMsg._buff = msg.getvalue() self._pub.publish(rosMsg)
def _send(self, msg, msgID, protocol, remoteID): rosMsg = rospy.AnyMsg() rosMsg._buff = msg try: with self._pendingLock: event = self._pending[msgID] self._pending[msgID] = rosMsg except KeyError: return event.set()
def publish(self, ros_topic, msg_type_str, msg_data): # Publish a message via ROS with self._lock: msg = rospy.AnyMsg() decomp = zlib.decompress(msg_data) msg._buff = decomp if ros_topic not in self._publishers: self._log.info("Publishing to ROS topic %s", ros_topic) mtype_cls = roslib.message.get_message_class(msg_type_str) pub = rospy.Publisher(ros_topic, mtype_cls) self._publishers[ros_topic] = pub pub = self._publishers[ros_topic] pub.publish(msg)
def _rceCB(self, req): """ Internally used method to send received message to the ROS Service as request. """ rosReq = rospy.AnyMsg() if _GZIP_LVL: rosReq._buff = zlib.decompress(req.getvalue()) else: rosReq._buff = req.getvalue() rospy.wait_for_service(self._addr, timeout=5) serviceFunc = rospy.ServiceProxy(self._addr, self._srvCls) rosResp = serviceFunc(rosReq) if _GZIP_LVL: resp = StringIO(zlib.compress(rosResp._buff, _GZIP_LVL)) else: resp = StringIO(rosResp._buff) return resp