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_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_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_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))
def test_creation(self):
yield test_util.call_with_nodehandle(lambda nh: defer.succeed(None))
