def test_excutor_spin_until_future_complete_future_done(self):
self.assertIsNotNone(self.node.handle)
executor = SingleThreadedExecutor(context=self.context)
executor.add_node(self.node)
def timer_callback():
pass
timer = self.node.create_timer(0.003, timer_callback)
def set_future_result(future):
time.sleep(0.1)
future.set_result('finished')
# Future complete timeout_sec > 0
future = Future()
self.assertFalse(future.done())
t = threading.Thread(target=lambda: set_future_result(future))
t.start()
start = time.monotonic()
executor.spin_until_future_complete(future=future, timeout_sec=0.2)
end = time.monotonic()
self.assertAlmostEqual(end - start, 0.1, delta=0.01)
self.assertTrue(future.done())
self.assertEqual(future.result(), 'finished')
# Future complete timeout_sec = None
future = Future()
self.assertFalse(future.done())
t = threading.Thread(target=lambda: set_future_result(future))
t.start()
start = time.monotonic()
executor.spin_until_future_complete(future=future, timeout_sec=None)
end = time.monotonic()
self.assertAlmostEqual(end - start, 0.1, delta=0.01)
self.assertTrue(future.done())
self.assertEqual(future.result(), 'finished')
# Future complete timeout < 0
future = Future()
self.assertFalse(future.done())
t = threading.Thread(target=lambda: set_future_result(future))
t.start()
start = time.monotonic()
executor.spin_until_future_complete(future=future, timeout_sec=-1)
end = time.monotonic()
self.assertAlmostEqual(end - start, 0.1, delta=0.01)
self.assertTrue(future.done())
self.assertEqual(future.result(), 'finished')
timer.cancel()
def wait_for_message(self,
topic,
msg_type,
timeout=None,
qos_profile=1):
"""
Wait for one message from topic.
This will create a new subcription to the topic, receive one message, then unsubscribe.
Do not call this method in a callback or a deadlock may occur.
"""
s = None
try:
future = Future()
s = self.new_subscription(msg_type,
topic,
lambda msg: future.set_result(msg),
qos_profile=qos_profile)
rclpy.spin_until_future_complete(self, future, self.executor,
timeout)
finally:
if s is not None:
self.destroy_subscription(s)
return future.result()
def test_orbslam2_buffer():
# bag_wrapper(wrap_node, rosbag_location, kwargs):
rosbag_location = "./test/testing_resources/rosbag/test.bag"
pose_args = {"visualize": False}
orbslam2PoseWrapped = bag_wrapper(Orbslam2Pose, rosbag_location, pose_args)
args = {
"wrap_node": Orbslam2Pose,
"rosbag_location": rosbag_location,
"kwargs": pose_args,
}
p = Process(
target=run_node,
args=(
orbslam2PoseWrapped,
args,
),
)
p.start()
future = Future()
bufferTester = BufferTester(future)
rclpy.spin_until_future_complete(bufferTester, future)
bufferTester.destroy_node()
os.kill(p.pid, signal.SIGKILL)
rclpy.shutdown()
assert future.result() == "Pass"
def test_bag():
rosbag_location = "./test/testing_resources/rosbag/test_short.bag"
p = Process(
target=play_rosbag,
args=(
rosbag_location,
False,
"-l --topics /terrasentia/usb_cam_node/image_raw",
),
)
p.start()
future = Future()
bag_tester = BagTester(future, timeout=5.0)
rclpy.spin_until_future_complete(bag_tester, future)
bag_tester.destroy_node()
# kills test.bag
kill_testbag_cmd = (
"export PYTHONPATH= && . /opt/ros/melodic/setup.sh && rosnode list "
+ "| grep play | xargs rosnode kill"
)
subprocess.Popen(
kill_testbag_cmd,
stdout=subprocess.DEVNULL,
stderr=subprocess.STDOUT,
shell=True,
)
# DO NOT FORGET TO START ROSBRIDGE AND ROSCORE!!!
assert future.result() == "Pass"
def start_a_message_equality_tester_node_and_get_result(future_result : Future, exe : MultiThreadedExecutor, expected_data : Dict):
node = MessageEqualityTesterNode('topic', 'std_msgs/msg/String', expected_data, future_result)
exe.add_node(node)
exe.spin_until_future_complete(future_result)
node.destroy_node()
return future_result.result()
def start_multi_equality_tester(future : Future, exe : MultiThreadedExecutor):
topic_and_expected_values_pairs = [
TopicAndValuesPair('topic1', 'std_msgs/msg/String', {'data' : 'hello1'}),
TopicAndValuesPair('topic2', 'std_msgs/msg/String', {'data' : 'hello2'}),
TopicAndValuesPair('topic3', 'std_msgs/msg/String', {'data' : 'hello3'}),
]
node = MultiMessageEqualityTesterNode(topic_and_expected_values_pairs, EqualityType.ALLOF, future)
exe.add_node(node)
exe.spin_until_future_complete(future)
node.destroy_node()
return future.result()
def _get_version_map_callback(self, future_done: Future, current_state):
"""Callback for when the version map is retrieved.
Parses the result of the future and inserts a new entry.
:param future_done Future that holds the result
"""
result = future_done.result()
if result.success:
try:
gait_version_map = ast.literal_eval(result.message)
message = (f"Starting gait {current_state.state}: "
f"{gait_version_map[current_state.state]}")
self._model.insert_row(Entry(message))
except KeyError:
pass
except ValueError as error:
self._node.get_logger().error(
f"Failed to parse gait version map: {error}")
def test_orbslam2_message():
rosbag_location = "./test/testing_resources/rosbag/test_long.bag"
pose_args = {"visualize": False}
p2 = Process(
target=run_node,
args=(
Orbslam2Pose,
pose_args,
),
)
p2.start()
p = Process(
target=play_rosbag,
args=(
rosbag_location,
False,
"-l --topics /terrasentia/usb_cam_node/image_raw",
),
)
p.start()
future = Future()
pose = PoseTesterValid(future)
rclpy.spin_until_future_complete(pose, future)
pose.destroy_node()
kill_testbag_cmd = (
"export PYTHONPATH= && . /opt/ros/melodic/setup.sh && rosnode list "
+ "rosnode list | grep play | xargs rosnode kill"
)
subprocess.Popen(
kill_testbag_cmd,
stdout=subprocess.DEVNULL,
stderr=subprocess.STDOUT,
shell=True,
)
os.kill(p.pid, signal.SIGKILL)
os.kill(p2.pid, signal.SIGKILL)
assert future.result() == "Pass"
def test_set_result(self):
f = Future()
f.set_result('Sentinel Result')
self.assertEqual('Sentinel Result', f.result())
self.assertTrue(f.done())
