def expect_message_within_time(self, timeout, expected_value) -> bool:
"""blocking fucntion"""
expected_msg = Condition()
expected_msg.condition = int(expected_value)
self.__msg_received = Future(executor=self.executor)
timeout_timer = self.create_timer(
timer_period_sec=timeout,
callback=lambda:
(self.get_logger().info("Timeout!"),
self.__msg_received.set_result(False), timeout_timer.cancel()))
#Wait for either timeout or new message to arrive
while not self.__msg_received.done():
pass
#Return true or false based on result of future
if self.__msg_received.result == False:
return False
elif type(self.__msg_received) is not Condition:
self.get_logger().info("Wrong message type stored in future")
return False
else:
pass
def _cancel_goal_async(self, goal_handle):
"""
Send a cancel request for an active goal and asynchronously get the result.
:param goal_handle: Handle to the goal to cancel.
:type goal_handle: :class:`ClientGoalHandle`
:return: a Future instance that completes when the cancel request has been processed.
:rtype: :class:`rclpy.task.Future` instance
"""
if not isinstance(goal_handle, ClientGoalHandle):
raise TypeError(
'Expected type ClientGoalHandle but received {}'.format(
type(goal_handle)))
cancel_request = CancelGoal.Request()
cancel_request.goal_info.goal_id = goal_handle.goal_id
sequence_number = self._client_handle.send_cancel_request(
cancel_request)
if sequence_number in self._pending_cancel_requests:
raise RuntimeError(
'Sequence ({}) conflicts with pending cancel request'.format(
sequence_number))
future = Future()
self._pending_cancel_requests[sequence_number] = future
future.add_done_callback(self._remove_pending_cancel_request)
# Add future so executor is aware
self.add_future(future)
return future
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 call_async(self, request: SrvTypeRequest) -> Future:
"""
Make a service request and asyncronously get the result.
:param request: The service request.
:return: A future that completes when the request does.
:raises: TypeError if the type of the passed request isn't an instance
of the Request type of the provided service when the client was
constructed.
"""
if not isinstance(request, self.srv_type.Request):
raise TypeError()
with self._lock:
with self.handle:
sequence_number = self.__client.send_request(request)
if sequence_number in self._pending_requests:
raise RuntimeError(
f'Sequence ({sequence_number}) conflicts with pending request'
)
future = Future()
self._pending_requests[sequence_number] = future
future.add_done_callback(self.remove_pending_request)
return future
def spin_until_future_complete(self,
future: Future,
timeout_sec: float = None) -> None:
"""Execute callbacks until a given future is done or a timeout occurs."""
# Make sure the future wakes this executor when it is done
future.add_done_callback(lambda x: self.wake())
if timeout_sec is None or timeout_sec < 0:
while self._context.ok(
) and not future.done() and not self._is_shutdown:
self.spin_once_until_future_complete(future, timeout_sec)
else:
start = time.monotonic()
end = start + timeout_sec
timeout_left = timeout_sec
while self._context.ok(
) and not future.done() and not self._is_shutdown:
self.spin_once_until_future_complete(future, timeout_left)
now = time.monotonic()
if now >= end:
return
timeout_left = end - now
def _get_result_async(self, goal_handle):
"""
Request the result for an active goal asynchronously.
:param goal_handle: Handle to the goal to cancel.
:type goal_handle: :class:`ClientGoalHandle`
:return: a Future instance that completes when the get result request has been processed.
:rtype: :class:`rclpy.task.Future` instance
"""
if not isinstance(goal_handle, ClientGoalHandle):
raise TypeError(
'Expected type ClientGoalHandle but received {}'.format(type(goal_handle)))
result_request = self._action_type.Impl.GetResultService.Request()
result_request.goal_id = goal_handle.goal_id
sequence_number = _rclpy_action.rclpy_action_send_result_request(
self._client_handle,
result_request)
if sequence_number in self._pending_result_requests:
raise RuntimeError(
'Sequence ({}) conflicts with pending result request'.format(sequence_number))
future = Future()
self._pending_result_requests[sequence_number] = future
future.add_done_callback(self._remove_pending_result_request)
# Add future so executor is aware
self.add_future(future)
return future
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_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 _test_statistic_publication(self, topic_name: str,
expected_nodes: Iterable[str]):
future = Future()
message_counter = Counter()
lock = Lock()
# arbitrary choice, just tells if it's working for a little while
expected_messages_per_node = 3
# we are receiving stats every 10 seconds, so this should pass in 30s
timeout_sec = 180
def message_callback(msg):
node_name = '/' + msg.measurement_source_name
with lock:
message_counter[node_name] += 1
if all(message_counter[node] >= expected_messages_per_node
for node in expected_nodes):
print('Successfully received all expected messages')
future.set_result(True)
sub = self.node.create_subscription(MetricsMessage,
topic_name,
message_callback,
qos_profile=10)
rclpy.spin_until_future_complete(self.node,
future,
timeout_sec=timeout_sec)
self.assertTrue(
future.done(),
f'Timed out, received message count: {message_counter}')
self.node.destroy_subscription(sub)
def test_await_exception(self):
f = Future()
async def coro():
nonlocal f
return await f
c = coro()
c.send(None)
f.set_exception(RuntimeError('test exception'))
with self.assertRaises(RuntimeError):
c.send(None)
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 main():
done = Future()
ros = RosBackend(done)
rosthread = Thread(target=ros.spin_until_future_complete)
rosthread.start()
gui = TkGUI()
gui.start()
print("shutting down ros...")
done.set_result(True)
rosthread.join()
def main(self, *, args):
if args.lost_messages:
print(
"WARNING: '--lost-messages' is deprecated; lost messages are reported by default",
file=sys.stderr)
self.csv = args.csv
# Validate field selection
self.field = args.field
if self.field is not None:
self.field = list(filter(None, self.field.split('.')))
if not self.field:
raise RuntimeError(f"Invalid field value '{args.field}'")
self.truncate_length = args.truncate_length if not args.full_length else None
self.no_arr = args.no_arr
self.no_str = args.no_str
self.flow_style = args.flow_style
self.filter_fn = None
if args.filter_expr:
self.filter_fn = _expr_eval(args.filter_expr)
self.future = None
if args.once:
self.future = Future()
self.include_message_info = args.include_message_info
with NodeStrategy(args) as node:
qos_profile = self.choose_qos(node, args)
if args.message_type is None:
message_type = get_msg_class(node,
args.topic_name,
include_hidden_topics=True)
else:
try:
message_type = get_message(args.message_type)
except (AttributeError, ModuleNotFoundError, ValueError):
raise RuntimeError('The passed message type is invalid')
if message_type is None:
raise RuntimeError(
'Could not determine the type for the passed topic')
self.subscribe_and_spin(node, args.topic_name, message_type,
qos_profile, args.no_lost_messages,
args.raw)
def test_await(self):
f = Future()
async def coro():
nonlocal f
return await f
c = coro()
c.send(None)
f.set_result('Sentinel Result')
try:
c.send(None)
except StopIteration as e:
self.assertEqual('Sentinel Result', e.value)
def neighbors_receive(self, neighbors, stop_event: Event = None):
"""Receive data from neighbors (waits until data are received from all neighbors)
Args:
neighbors (list): list of neighbors
Returns:
dict: dict containing received data associated to each neighbor in neighbors
"""
if not self.synchronous_mode:
raise Exception(
"Cannot perform synchronous receive because communicator is in asynchronous mode"
)
for j in neighbors:
if j not in self.subscriptions:
raise RuntimeError(
"Cannot receive from {} since it is not an in-neighbor".
format(j))
self.neighbors = neighbors
self.received_data = {} # initialize dictionary of received data
self.future = Future()
for k in self.callback_groups:
self.callback_groups[k].give_authorization()
# receive a single message from all neighbors (check Event every 'timeout' seconds)
timeout = 0.2 if stop_event is not None else None
while not self.future.done():
rclpy.spin_until_future_complete(self.node,
self.future,
executor=self.executor,
timeout_sec=timeout)
# check if external event has been set
if stop_event is not None and stop_event.is_set():
# remove pending messages from topics
for k in self.callback_groups:
self.callback_groups[k].give_authorization(permanent=True)
self.synchronous_mode = False
self.neighbors_receive_asynchronous(neighbors)
self.synchronous_mode = True
for k in self.callback_groups:
self.callback_groups[k].draw_authorization()
break
return self.received_data
def send_goal_async(self, goal, feedback_callback=None, goal_uuid=None):
"""
Send a goal and asynchronously get the result.
The result of the returned Future is set to a ClientGoalHandle when receipt of the goal
is acknowledged by an action server.
:param goal: The goal request.
:type goal: action_type.Goal
:param feedback_callback: Callback function for feedback associated with the goal.
:type feedback_callback: function
:param goal_uuid: Universally unique identifier for the goal.
If None, then a random UUID is generated.
:type: unique_identifier_msgs.UUID
:return: a Future instance to a goal handle that completes when the goal request
has been accepted or rejected.
:rtype: :class:`rclpy.task.Future` instance
:raises: TypeError if the type of the passed goal isn't an instance of
the Goal type of the provided action when the service was
constructed.
"""
if not isinstance(goal, self._action_type.Goal):
raise TypeError()
request = self._action_type.Impl.SendGoalService.Request()
request.goal_id = self._generate_random_uuid(
) if goal_uuid is None else goal_uuid
request.goal = goal
sequence_number = _rclpy_action.rclpy_action_send_goal_request(
self._client_handle, request)
if sequence_number in self._pending_goal_requests:
raise RuntimeError(
'Sequence ({}) conflicts with pending goal request'.format(
sequence_number))
if feedback_callback is not None:
# TODO(jacobperron): Move conversion function to a general-use package
goal_uuid = bytes(request.goal_id.uuid)
self._feedback_callbacks[goal_uuid] = feedback_callback
future = Future()
self._pending_goal_requests[sequence_number] = future
self._sequence_number_to_goal_id[sequence_number] = request.goal_id
future.add_done_callback(self._remove_pending_goal_request)
# Add future so executor is aware
self.add_future(future)
return future
def test_top_map():
try:
rosbag_location = "./test/testing_resources/rosbag/test_long.bag"
p2 = Process(
target=play_rosbag,
args=(rosbag_location, False, ""),
)
p2.start()
future = Future()
topMapTester = TopMapTester(future, timeout=10)
rclpy.spin_until_future_complete(topMapTester, future)
except Exception as e:
raise (e)
else:
topMapTester.destroy_node()
assert topMapTester.future.result() == "Pass"
finally:
# 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,
)
os.kill(p2.pid, signal.SIGKILL)
def send_goal(self):
self.get_logger().info("WAITING FOR NAVIGATION SERVER...")
self.ac.wait_for_server()
self.get_logger().info("NAVIGATION SERVER AVAILABLE...")
self.get_logger().info("SENDING GOAL TO NAVIGATION SERVER...")
self.start_time = time.time()
self.cancel_future = None
self.is_done = False
self.map = None
self.goal_future = self.ac.send_goal_async(self.goal)
self.create_timer(.1, self.timer_callback)
self.future_event = Future()
return self.future_event
def check_for_statistic_publications(expected_nodes: List, num_msgs: int,
expected_topic: str) -> None:
"""
Check that all nodes publish a statistics message.
This will suceed fast if all expected messages were published, otherwise
timeout (default TIMEOUT_SECONDS) if any publishers have not been observed.
:param args:
"""
logging.debug('starting test check_for_statistic_publications')
try:
future = Future()
node = StatisticsListener(future, list(expected_nodes), num_msgs,
expected_topic)
rclpy.spin_until_future_complete(
node, future, timeout_sec=PUBLICATION_TEST_TIMEOUT_SECONDS)
if node.received_all_expected_messages():
logging.info('check_for_statistic_publications success')
else:
raise SystemMetricsEnd2EndTestException(
'check_for_statistic_publications failed.'
' Absent publisher(s): ' +
str(node.lifecycle_nodes_message_counter))
finally:
node.destroy_node()
def test_meng_wp_video():
# Run bag node to test
rosbag_location = "./test/testing_resources/rosbag/test.bag"
p = Process(
target=play_rosbag,
args=(
rosbag_location,
False,
"--topics /terrasentia/usb_cam_node/image_raw --wait-for-subscribers",
),
)
future = Future()
waypointPublisher = WaypointPublisherTester(5, future)
goal = get_goal()
waypointPublisher.goal = goal
waypointPublisher.goal_show = goal[6]
p.start()
rclpy.spin_until_future_complete(waypointPublisher, future)
waypointPublisher.destroy_node()
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,
)
os.kill(p.pid, signal.SIGKILL)
assert len(glob.glob("./test/results/wp/*")) != 0
assert waypointPublisher.twistflag
def main(args):
if not args.csv:
truncate_length = args.truncate_length if not args.full_length else None
callback = subscriber_cb(truncate_length, args.no_arr, args.no_str)
else:
truncate_length = args.truncate_length if not args.full_length else None
callback = subscriber_cb_csv(truncate_length, args.no_arr, args.no_str)
qos_profile = qos_profile_from_short_keys(args.qos_profile,
reliability=args.qos_reliability,
durability=args.qos_durability,
depth=args.qos_depth,
history=args.qos_history)
with NodeStrategy(args) as node:
if args.message_type is None:
message_type = get_msg_class(node,
args.topic_name,
include_hidden_topics=True)
else:
message_type = get_message(args.message_type)
if message_type is None:
raise RuntimeError(
'Could not determine the type for the passed topic')
future = None
if args.once:
future = Future()
callback = subscriber_cb_once_decorator(callback, future)
subscriber(node, args.topic_name, message_type, callback, qos_profile,
args.lost_messages, future, args.timeout)
class TestConditionSubscriberNode(Node):
"""Subscribes to condition messages for the purpose of testing"""
__msg_received: Optional[Future] = None
def __init__(self):
super().__init__('test_cond_sub_node')
self.create_subscription(
Condition,
'acf_process_cell_is_sheilded',
callback=lambda msg: (
self.get_logger().info("recieved {}".format(msg)),
#If a future has been created, set the result as the message, otherwise discard the message
self.__msg_received.set_result(msg)
if self.__msg_received is not None else
(self.get_logger().info("not expecting message"))),
qos_profile=latching_qos)
def expect_message_within_time(self, timeout, expected_value) -> bool:
"""blocking fucntion"""
expected_msg = Condition()
expected_msg.condition = int(expected_value)
self.__msg_received = Future(executor=self.executor)
timeout_timer = self.create_timer(
timer_period_sec=timeout,
callback=lambda:
(self.get_logger().info("Timeout!"),
self.__msg_received.set_result(False), timeout_timer.cancel()))
#Wait for either timeout or new message to arrive
while not self.__msg_received.done():
pass
#Return true or false based on result of future
if self.__msg_received.result == False:
return False
elif type(self.__msg_received) is not Condition:
self.get_logger().info("Wrong message type stored in future")
return False
else:
pass
def start_spin_thread(self, waitable):
waitable.future = Future(executor=self.executor)
self.thr = threading.Thread(
target=self.executor.spin_until_future_complete,
args=(waitable.future, ),
daemon=True)
self.thr.start()
return self.thr
def test_executor_spin_until_future_complete_do_not_wait(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)
# Do not wait timeout_sec = 0
future = Future()
self.assertFalse(future.done())
executor.spin_until_future_complete(future=future, timeout_sec=0)
self.assertFalse(future.done())
timer.cancel()
def test_default_incompatible_qos_callbacks(self):
original_logger = rclpy.logging._root_logger
pub_log_msg = None
sub_log_msg = None
log_msgs_future = Future()
class MockLogger:
def get_child(self, name):
return self
def warn(self, message, once=False):
nonlocal pub_log_msg, sub_log_msg, log_msgs_future
if message.startswith('New subscription discovered'):
pub_log_msg = message
elif message.startswith('New publisher discovered'):
sub_log_msg = message
if pub_log_msg is not None and sub_log_msg is not None:
log_msgs_future.set_result(True)
rclpy.logging._root_logger = MockLogger()
qos_profile_publisher = QoSProfile(
depth=10,
durability=QoSDurabilityPolicy.RMW_QOS_POLICY_DURABILITY_VOLATILE)
self.node.create_publisher(EmptyMsg, self.topic_name,
qos_profile_publisher)
message_callback = Mock()
qos_profile_subscription = QoSProfile(
depth=10,
durability=QoSDurabilityPolicy.
RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL)
self.node.create_subscription(EmptyMsg, self.topic_name,
message_callback,
qos_profile_subscription)
executor = rclpy.executors.SingleThreadedExecutor(context=self.context)
rclpy.spin_until_future_complete(self.node, log_msgs_future, executor,
10.0)
if not self.is_fastrtps:
self.assertEqual(
pub_log_msg,
"New subscription discovered on topic '{}', requesting incompatible QoS. "
'No messages will be sent to it. '
'Last incompatible policy: DURABILITY_QOS_POLICY'.format(
self.topic_name))
self.assertEqual(
sub_log_msg,
"New publisher discovered on topic '{}', offering incompatible QoS. "
'No messages will be received from it. '
'Last incompatible policy: DURABILITY_QOS_POLICY'.format(
self.topic_name))
rclpy.logging._root_logger = original_logger
def main():
parser = argparse.ArgumentParser(
description='CLI tool for checking message equality')
parser.add_argument('--name',
required=True,
help='The name of the message to listen for')
parser.add_argument('--type',
required=True,
help='The type of the message')
parser.add_argument('--expected',
required=True,
help='The expected values (YAML format)')
parser.add_argument('--timeout',
default=None,
type=nonnegative_int,
help='The amount of time to wait for a message')
args = parser.parse_args()
if len(sys.argv) == 1:
parser.print_help(sys.stdout)
sys.exit(0)
values_dictionary = yaml.safe_load(args.expected)
if not isinstance(values_dictionary, dict):
return 'The passed value needs to be a dictionary in YAML format'
rclpy.init()
future = Future()
node = MessageEqualityTesterNode(args.name, args.type, values_dictionary,
future)
rclpy.spin_until_future_complete(node, future, timeout_sec=args.timeout)
if not future.done():
node.get_logger().error("Timeout after " + str(args.timeout) +
" seccond: No message on \"" + args.name +
"\" topic")
node.destroy_node()
rclpy.shutdown()
def call_async(self, req):
"""
Make a service request and asyncronously get the result.
:return: a Future instance that completes when the request does
:rtype: :class:`rclpy.task.Future` instance
"""
sequence_number = _rclpy.rclpy_send_request(self.client_handle, req)
if sequence_number in self._pending_requests:
raise RuntimeError('Sequence (%r) conflicts with pending request' %
sequence_number)
future = Future()
self._pending_requests[sequence_number] = future
future.add_done_callback(self.remove_pending_request)
return future
def spin_until_future_complete(self, future: Future, timeout_sec: float = None) -> None:
"""Execute callbacks until a given future is done or a timeout occurs."""
if timeout_sec is None or timeout_sec < 0:
while self._context.ok() and not future.done():
self.spin_once(timeout_sec=timeout_sec)
else:
start = time.monotonic()
end = start + timeout_sec
timeout_left = timeout_sec
while self._context.ok() and not future.done():
self.spin_once(timeout_sec=timeout_left)
now = time.monotonic()
if now >= end:
return
timeout_left = end - now
def test_excutor_spin_until_future_complete_do_not_wait(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)
# Do not wait timeout_sec = 0
future = Future()
self.assertFalse(future.done())
start = time.monotonic()
executor.spin_until_future_complete(future=future, timeout_sec=0)
end = time.monotonic()
self.assertAlmostEqual(end - start, 0.0, delta=0.01)
self.assertFalse(future.done())
timer.cancel()
def call_async(self, request: SrvTypeRequest) -> Future:
"""
Make a service request and asyncronously get the result.
:param request: The service request.
:return: A future that completes when the request does.
"""
sequence_number = _rclpy.rclpy_send_request(self.client_handle,
request)
if sequence_number in self._pending_requests:
raise RuntimeError('Sequence (%r) conflicts with pending request' %
sequence_number)
future = Future()
self._pending_requests[sequence_number] = future
future.add_done_callback(self.remove_pending_request)
return future
