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)