def test_await_transform_immediately_available(self):
# wait for a transform that is already available to test short-cut code
buffer = Buffer()
clock = rclpy.clock.Clock()
rclpy_time = clock.now()
transform = self.build_transform('foo', 'bar', rclpy_time)
buffer.set_transform(transform, 'unittest')
coro = buffer.lookup_transform_async('foo', 'bar', rclpy_time)
with self.assertRaises(StopIteration) as cm:
coro.send(None)
self.assertEqual(transform, cm.exception.value)
coro.close()
def test_await_transform_delayed(self):
# wait for a transform that is not yet available
buffer = Buffer()
clock = rclpy.clock.Clock()
rclpy_time = clock.now()
transform = self.build_transform('foo', 'bar', rclpy_time)
coro = buffer.lookup_transform_async('foo', 'bar', rclpy_time)
coro.send(None)
buffer.set_transform(transform, 'unittest')
with self.assertRaises(StopIteration) as cm:
coro.send(None)
self.assertEqual(transform, cm.exception.value)
coro.close()
class TestBroadcasterAndListener(unittest.TestCase):
@classmethod
def setUpClass(cls):
rclpy.init()
cls.buffer = Buffer()
cls.node = rclpy.create_node('test_broadcaster_listener')
cls.broadcaster = TransformBroadcaster(cls.node)
cls.static_broadcaster = StaticTransformBroadcaster(cls.node)
cls.listener = TransformListener(
buffer=cls.buffer, node=cls.node, spin_thread=False)
cls.executor = rclpy.executors.SingleThreadedExecutor()
cls.executor.add_node(cls.node)
@classmethod
def tearDownClass(cls):
rclpy.shutdown()
def setUp(self):
self.buffer = Buffer()
self.listener.buffer = self.buffer
def broadcast_transform(self, target_frame, source_frame, time_stamp):
broadcast_transform = build_transform(
target_frame=target_frame, source_frame=source_frame, stamp=time_stamp)
self.broadcaster.sendTransform(broadcast_transform)
self.executor.spin_once()
return broadcast_transform
def broadcast_static_transform(self, target_frame, source_frame, time_stamp):
broadcast_transform = build_transform(
target_frame=target_frame, source_frame=source_frame, stamp=time_stamp)
self.static_broadcaster.sendTransform(broadcast_transform)
self.executor.spin_once()
return broadcast_transform
def test_broadcaster_and_listener(self):
time_stamp = rclpy.time.Time(seconds=1, nanoseconds=0).to_msg()
broadcasted_transform = self.broadcast_transform(
target_frame='foo', source_frame='bar', time_stamp=time_stamp)
listened_transform = self.buffer.lookup_transform(
target_frame='foo', source_frame='bar', time=time_stamp)
self.assertTrue(broadcasted_transform, listened_transform)
# Execute a coroutine
listened_transform_async = None
coro = self.buffer.lookup_transform_async(
target_frame='foo', source_frame='bar', time=time_stamp)
try:
coro.send(None)
except StopIteration as e:
# The coroutine finished; store the result
coro.close()
listened_transform_async = e.value
self.assertTrue(broadcasted_transform, listened_transform_async)
def test_extrapolation_exception(self):
self.broadcast_transform(
target_frame='foo', source_frame='bar',
time_stamp=rclpy.time.Time(seconds=0.3, nanoseconds=0).to_msg())
self.broadcast_transform(
target_frame='foo', source_frame='bar',
time_stamp=rclpy.time.Time(seconds=0.2, nanoseconds=0).to_msg())
with self.assertRaises(ExtrapolationException) as e:
self.buffer.lookup_transform(
target_frame='foo', source_frame='bar',
time=rclpy.time.Time(seconds=0.1, nanoseconds=0).to_msg())
self.assertTrue(
'Lookup would require extrapolation into the past' in str(e.exception))
with self.assertRaises(ExtrapolationException) as e:
self.buffer.lookup_transform(
target_frame='foo', source_frame='bar',
time=rclpy.time.Time(seconds=0.4, nanoseconds=0).to_msg())
self.assertTrue(
'Lookup would require extrapolation into the future' in str(e.exception))
def test_static_broadcaster_and_listener(self):
broadcasted_transform = self.broadcast_static_transform(
target_frame='foo', source_frame='bar',
time_stamp=rclpy.time.Time(seconds=1.1, nanoseconds=0).to_msg())
listened_transform = self.buffer.lookup_transform(
target_frame='foo', source_frame='bar',
time=rclpy.time.Time(seconds=1.5, nanoseconds=0).to_msg())
self.assertTrue(broadcasted_transform, listened_transform)
