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 main(args=None):
rclpy.init(args=args)
node = SkibotNode()
executor = MultiThreadedExecutor(num_threads=4)
executor.add_node(node)
future = Future()
future.add_done_callback(lambda fut: print("Future is done"))
executor.spin_until_future_complete(future)
node.destroy_node()
rclpy.shutdown()
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 main():
args = parse_args()
rclpy.init()
exe = MultiThreadedExecutor()
future = Future()
node = HelloActionServerNode(args.name, args.delay, future)
node.get_logger().info("Node started")
exe.add_node(node)
try:
exe.spin_until_future_complete(future, timeout_sec=200)
exe.spin_once()
except KeyboardInterrupt:
node.get_logger().info("KeyboardInterrupt: Aborting all goals")
node.abort()
node.get_logger().info("Node finished")
class SubscriberTool():
def __init__(self, node: Node = None):
self.__this_node = node if node is not None else Node('internal_node')
self.__this_executor = MultiThreadedExecutor()
self.__this_executor.add_node(self.__this_node)
self.__msg = {}
def create_subscription(self, topic_name, msg_type):
self.__msg[topic_name] = None
def callback(msg):
self.__this_node.get_logger().debug(
f'called callback {topic_name}', throttle_duration_sec=0.1)
self.__msg[topic_name] = msg
self.__this_node.create_subscription(
msg_type,
topic_name,
callback,
10)
def await_for_msg(self, topic: str, timeout: float = None):
self.__msg[topic] = None
task = self.__this_executor.create_task(self.is_msg, topic)
self.__this_executor.spin_until_future_complete(task, timeout)
self.__this_node.get_logger().debug(f'is taks done? {task.done()}')
self.__this_node.get_logger().debug(f'task result: {task.result().twist.twist}')
return self.__msg[topic]
async def is_msg(self, topic):
rclpy.spin_once(self.__this_node) # force spin
while self.__msg[topic] is None:
continue
return self.__msg[topic]
