def test_tf(self): @defer.inlineCallbacks def f(nh): tf_broadcaster = tf.TransformBroadcaster(nh) @apply @util.cancellableInlineCallbacks def publish_thread(): try: while True: t = nh.get_time() tf_broadcaster.send_transform( TransformStamped( header=Header( stamp=t, frame_id='/parent', ), child_frame_id='/child', transform=Transform( translation=Vector3(*[1, 2, 3]), rotation=Quaternion( *transformations.quaternion_about_axis( t.to_sec(), [0, 0, 1])), ), )) yield nh.sleep(0.1) except Exception: traceback.print_exc() try: tf_listener = tf.TransformListener( nh, history_length=genpy.Duration(1) ) # short history length so that we cover history being truncated try: yield tf_listener.get_transform( '/parent', '/child', nh.get_time() - genpy.Duration(100)) except tf.TooPastError: pass else: self.fail('expected TooPastError') start_time = nh.get_time() for i in xrange(200): time = start_time + genpy.Duration.from_sec(1 + i / 100) dt = 1e-3 transform = yield tf_listener.get_transform( '/parent', '/child', time) transform2 = yield tf_listener.get_transform( '/parent', '/child', time + genpy.Duration.from_sec(1e-3)) assert numpy.allclose((transform2 - transform) / dt, [0, 0, 0, 0, 0, 1]) finally: yield publish_thread.cancel() publish_thread.addErrback( lambda fail: fail.trap(defer.CancelledError)) yield test_util.call_with_nodehandle(f)
def test_advertise(self): @defer.inlineCallbacks def f(nh): from std_msgs.msg import Int32 pub = nh.advertise('/my_topic', Int32, latching=True) pub.publish(Int32(42)) sub = nh.subscribe('/my_topic', Int32) yield sub.get_next_message() assert sub.get_last_message().data == 42 yield test_util.call_with_nodehandle(f)
def test_params(self): @defer.inlineCallbacks def f(nh): k = '/my_param' v = ['hi', 2] assert not (yield nh.has_param(k)) yield nh.set_param(k, v) assert (yield nh.has_param(k)) assert (yield nh.get_param(k)) == v yield nh.delete_param(k) assert not (yield nh.has_param(k)) yield test_util.call_with_nodehandle(f)
def test_tf(self): @defer.inlineCallbacks def f(nh): tf_broadcaster = tf.TransformBroadcaster(nh) @apply @util.cancellableInlineCallbacks def publish_thread(): try: while True: t = nh.get_time() tf_broadcaster.send_transform(TransformStamped( header=Header( stamp=t, frame_id='/parent', ), child_frame_id='/child', transform=Transform( translation=Vector3(*[1, 2, 3]), rotation=Quaternion(*transformations.quaternion_about_axis(t.to_sec(), [0, 0, 1])), ), )) yield nh.sleep(0.1) except Exception: traceback.print_exc() try: tf_listener = tf.TransformListener(nh, history_length=genpy.Duration( 1)) # short history length so that we cover history being truncated try: yield tf_listener.get_transform('/parent', '/child', nh.get_time() - genpy.Duration(100)) except tf.TooPastError: pass else: self.fail('expected TooPastError') start_time = nh.get_time() for i in xrange(200): time = start_time + genpy.Duration.from_sec(1 + i / 100) dt = 1e-3 transform = yield tf_listener.get_transform('/parent', '/child', time) transform2 = yield tf_listener.get_transform( '/parent', '/child', time + genpy.Duration.from_sec(1e-3)) assert numpy.allclose((transform2 - transform) / dt, [0, 0, 0, 0, 0, 1]) finally: yield publish_thread.cancel() publish_thread.addErrback(lambda fail: fail.trap(defer.CancelledError)) yield test_util.call_with_nodehandle(f)
def test_service(self): @defer.inlineCallbacks def f(nh): from roscpp_tutorials.srv import TwoInts, TwoIntsRequest, TwoIntsResponse @util.cancellableInlineCallbacks def callback(req): yield util.wall_sleep(.5) defer.returnValue(TwoIntsResponse(sum=req.a + req.b)) nh.advertise_service('/my_service', TwoInts, callback) s = nh.get_service_client('/my_service', TwoInts) yield s.wait_for_service() assert (yield s(TwoIntsRequest(a=10, b=30))).sum == 40 assert (yield s(TwoIntsRequest(a=-10, b=30))).sum == 20 yield test_util.call_with_nodehandle(f)
def test_creation(self): yield test_util.call_with_nodehandle(lambda nh: defer.succeed(None))