def main(args=None):
rclpy.init(args=args)
try:
talker = Talker()
listener = Listener()
# Runs all callbacks in the main thread
executor = SingleThreadedExecutor()
# Add imported nodes to this executor
executor.add_node(talker)
executor.add_node(listener)
try:
# Execute callbacks for both nodes as they become ready
executor.spin()
finally:
executor.shutdown()
listener.destroy_node()
talker.destroy_node()
finally:
rclpy.shutdown()
def setUpClass(cls):
cls.context = rclpy.context.Context()
rclpy.init(context=cls.context)
cls.executor = SingleThreadedExecutor(context=cls.context)
cls.node = rclpy.create_node('TestActionClient', context=cls.context)
cls.mock_action_server = MockActionServer(cls.node)
class TestROS2TopicEchoPub(unittest.TestCase):
# TODO(hidmic): investigate why making use of the same rclpy node, executor
# and context for all tests on a per rmw implementation basis
# makes them fail on Linux-aarch64 when using 'rmw_opensplice_cpp'.
# Presumably, interfaces creation/destruction and/or executor spinning
# on one test is affecting the other.
def setUp(self):
self.context = rclpy.context.Context()
rclpy.init(context=self.context)
self.node = rclpy.create_node(TEST_NODE,
namespace=TEST_NAMESPACE,
context=self.context)
self.executor = SingleThreadedExecutor(context=self.context)
self.executor.add_node(self.node)
def tearDown(self):
self.node.destroy_node()
rclpy.shutdown(context=self.context)
@launch_testing.markers.retry_on_failure(times=5)
def test_pub_basic(self, launch_service, proc_info, proc_output):
params = [('/clitest/topic/pub_basic', False, True),
('/clitest/topic/pub_compatible_qos', True, True),
('/clitest/topic/pub_incompatible_qos', True, False)]
for topic, provide_qos, compatible_qos in params:
with self.subTest(topic=topic,
provide_qos=provide_qos,
compatible_qos=compatible_qos):
# Check for inconsistent arguments
assert provide_qos if not compatible_qos else True
received_message_count = 0
expected_minimum_message_count = 1
expected_maximum_message_count = 5
pub_extra_options = []
subscription_qos_profile = 10
if provide_qos:
if compatible_qos:
# For compatible test, put publisher at very high quality
# and subscription at low
pub_extra_options = [
'--qos-reliability', 'reliable',
'--qos-durability', 'transient_local'
]
subscription_qos_profile = QoSProfile(
depth=10,
reliability=ReliabilityPolicy.BEST_EFFORT,
durability=DurabilityPolicy.VOLATILE)
else:
# For an incompatible example, reverse the quality extremes
# and expect no messages to arrive
pub_extra_options = [
'--qos-reliability', 'best_effort',
'--qos-durability', 'volatile'
]
subscription_qos_profile = QoSProfile(
depth=10,
reliability=ReliabilityPolicy.RELIABLE,
durability=DurabilityPolicy.TRANSIENT_LOCAL)
expected_maximum_message_count = 0
expected_minimum_message_count = 0
future = rclpy.task.Future()
def message_callback(msg):
"""If we receive one message, the test has succeeded."""
nonlocal received_message_count
received_message_count += 1
future.set_result(True)
subscription = self.node.create_subscription(
String, topic, message_callback, subscription_qos_profile)
assert subscription
try:
command_action = ExecuteProcess(
cmd=(['ros2', 'topic', 'pub'] + pub_extra_options +
[topic, 'std_msgs/String', 'data: hello']),
additional_env={'PYTHONUNBUFFERED': '1'},
output='screen')
with launch_testing.tools.launch_process(
launch_service,
command_action,
proc_info,
proc_output,
output_filter=launch_testing_ros.tools.
basic_output_filter(
filtered_rmw_implementation=
get_rmw_implementation_identifier(
))) as command:
self.executor.spin_until_future_complete(
future, timeout_sec=10)
command.wait_for_shutdown(timeout=10)
# Check results
assert (
received_message_count >= expected_minimum_message_count and
received_message_count <= expected_maximum_message_count), \
('Received {} messages from pub on {},'
'which is not in expected range {}-{}').format(
received_message_count, topic,
expected_minimum_message_count,
expected_maximum_message_count
)
finally:
# Cleanup
self.node.destroy_subscription(subscription)
@launch_testing.markers.retry_on_failure(times=5)
def test_pub_times(self, launch_service, proc_info, proc_output):
command_action = ExecuteProcess(
cmd=([
'ros2', 'topic', 'pub', '-t', '5', '-w', '0',
'/clitest/topic/pub_times', 'std_msgs/String', 'data: hello'
]),
additional_env={'PYTHONUNBUFFERED': '1'},
output='screen')
with launch_testing.tools.launch_process(
launch_service,
command_action,
proc_info,
proc_output,
output_filter=launch_testing_ros.tools.basic_output_filter(
filtered_rmw_implementation=
get_rmw_implementation_identifier())) as command:
assert command.wait_for_shutdown(timeout=10)
assert command.exit_code == launch_testing.asserts.EXIT_OK
assert launch_testing.tools.expect_output(expected_lines=[
'publisher: beginning loop',
"publishing #1: std_msgs.msg.String(data='hello')",
'',
"publishing #2: std_msgs.msg.String(data='hello')",
'',
"publishing #3: std_msgs.msg.String(data='hello')",
'',
"publishing #4: std_msgs.msg.String(data='hello')",
'',
"publishing #5: std_msgs.msg.String(data='hello')",
'',
],
text=command.output,
strict=True)
@launch_testing.markers.retry_on_failure(times=5)
def test_echo_basic(self, launch_service, proc_info, proc_output):
params = [
('/clitest/topic/echo_basic', False, True, False),
('/clitest/topic/echo_compatible_qos', True, True, False),
('/clitest/topic/echo_incompatible_qos', True, False, False),
('/clitest/topic/echo_message_lost', False, True, True),
]
for topic, provide_qos, compatible_qos, message_lost in params:
with self.subTest(topic=topic,
provide_qos=provide_qos,
compatible_qos=compatible_qos):
# Check for inconsistent arguments
assert provide_qos if not compatible_qos else True
echo_extra_options = []
publisher_qos_profile = 10
if provide_qos:
if compatible_qos:
# For compatible test, put publisher at very high quality
# and subscription at low
echo_extra_options = [
'--qos-reliability', 'best_effort',
'--qos-durability', 'volatile'
]
publisher_qos_profile = QoSProfile(
depth=10,
reliability=ReliabilityPolicy.RELIABLE,
durability=DurabilityPolicy.TRANSIENT_LOCAL)
else:
# For an incompatible example, reverse the quality extremes
# and expect no messages to arrive
echo_extra_options = [
'--qos-reliability', 'reliable',
'--qos-durability', 'transient_local'
]
publisher_qos_profile = QoSProfile(
depth=10,
reliability=ReliabilityPolicy.BEST_EFFORT,
durability=DurabilityPolicy.VOLATILE)
if not message_lost:
echo_extra_options.append('--no-lost-messages')
publisher = self.node.create_publisher(String, topic,
publisher_qos_profile)
assert publisher
def publish_message():
publisher.publish(String(data='hello'))
publish_timer = self.node.create_timer(0.5, publish_message)
try:
command_action = ExecuteProcess(
cmd=(['ros2', 'topic', 'echo'] + echo_extra_options +
[topic, 'std_msgs/String']),
additional_env={'PYTHONUNBUFFERED': '1'},
output='screen')
with launch_testing.tools.launch_process(
launch_service,
command_action,
proc_info,
proc_output,
output_filter=launch_testing_ros.tools.
basic_output_filter(
filtered_rmw_implementation=
get_rmw_implementation_identifier(
))) as command:
# The future won't complete - we will hit the timeout
self.executor.spin_until_future_complete(
rclpy.task.Future(), timeout_sec=5)
command.wait_for_shutdown(timeout=10)
# Check results
if compatible_qos:
assert command.output, 'Echo CLI printed no output'
assert 'data: hello' in command.output.splitlines(), (
'Echo CLI did not print expected message')
else:
assert command.output, (
'Echo CLI did not print incompatible QoS warning')
assert (
"New publisher discovered on topic '{}', offering incompatible"
' QoS.'.format(topic) in command.output
), ('Echo CLI did not print expected incompatible QoS warning'
)
finally:
# Cleanup
self.node.destroy_timer(publish_timer)
self.node.destroy_publisher(publisher)
@launch_testing.markers.retry_on_failure(times=1)
def test_echo_filter(self, launch_service, proc_info, proc_output):
params = [
('/clitest/topic/echo_filter_all_pass', "m.data=='hello'", True),
('/clitest/topic/echo_filter_all_filtered', "m.data=='success'",
False),
]
for topic, filter_expr, has_output in params:
with self.subTest(topic=topic,
filter_expr=filter_expr,
print_count=10):
# Check for inconsistent arguments
publisher = self.node.create_publisher(String, topic, 10)
assert publisher
def publish_message():
publisher.publish(String(data='hello'))
publish_timer = self.node.create_timer(0.5, publish_message)
try:
command_action = ExecuteProcess(
cmd=(['ros2', 'topic', 'echo'] +
['--filter', filter_expr] +
[topic, 'std_msgs/String']),
additional_env={'PYTHONUNBUFFERED': '1'},
output='screen')
with launch_testing.tools.launch_process(
launch_service,
command_action,
proc_info,
proc_output,
output_filter=launch_testing_ros.tools.
basic_output_filter(
filtered_rmw_implementation=
get_rmw_implementation_identifier(
))) as command:
# The future won't complete - we will hit the timeout
self.executor.spin_until_future_complete(
rclpy.task.Future(), timeout_sec=5)
command.wait_for_shutdown(timeout=10)
# Check results
if has_output:
assert 'hello' in command.output, 'Echo CLI did not output'
else:
assert 'hello' not in command.output, 'All messages should be filtered out'
finally:
# Cleanup
self.node.destroy_timer(publish_timer)
self.node.destroy_publisher(publisher)
@launch_testing.markers.retry_on_failure(times=5)
def test_echo_raw(self, launch_service, proc_info, proc_output):
topic = '/clitest/topic/echo_raw'
publisher = self.node.create_publisher(String, topic, 10)
assert publisher
def publish_message():
publisher.publish(String(data='hello'))
publish_timer = self.node.create_timer(0.5, publish_message)
try:
command_action = ExecuteProcess(
cmd=[
'ros2', 'topic', 'echo', '--raw', topic,
'std_msgs/msg/String'
],
additional_env={'PYTHONUNBUFFERED': '1'},
output='screen')
with launch_testing.tools.launch_process(
launch_service,
command_action,
proc_info,
proc_output,
output_filter=launch_testing_ros.tools.basic_output_filter(
filtered_rmw_implementation=
get_rmw_implementation_identifier())) as command:
# The future won't complete - we will hit the timeout
self.executor.spin_until_future_complete(rclpy.task.Future(),
timeout_sec=5)
assert command.wait_for_output(
functools.partial(
launch_testing.tools.expect_output,
expected_lines=[
"b'\\x00\\x01\\x00\\x00\\x06\\x00\\x00\\x00hello\\x00\\x00\\x00'",
'---',
],
strict=True),
timeout=10), 'Echo CLI did not print expected message'
assert command.wait_for_shutdown(timeout=10)
finally:
# Cleanup
self.node.destroy_timer(publish_timer)
self.node.destroy_publisher(publisher)
@launch_testing.markers.retry_on_failure(times=5)
def test_echo_once(self, launch_service, proc_info, proc_output):
topic = '/clitest/topic/echo_once'
publisher = self.node.create_publisher(String, topic, 10)
assert publisher
def publish_message():
publisher.publish(String(data='hello'))
publish_timer = self.node.create_timer(1.0, publish_message)
try:
command_action = ExecuteProcess(
cmd=[
'ros2', 'topic', 'echo', '--once', topic,
'std_msgs/msg/String'
],
additional_env={'PYTHONUNBUFFERED': '1'},
output='screen')
with launch_testing.tools.launch_process(
launch_service,
command_action,
proc_info,
proc_output,
output_filter=launch_testing_ros.tools.basic_output_filter(
filtered_rmw_implementation=
get_rmw_implementation_identifier())) as command:
# The future won't complete - we will hit the timeout
self.executor.spin_until_future_complete(rclpy.task.Future(),
timeout_sec=3)
assert command.wait_for_shutdown(timeout=5)
assert launch_testing.tools.expect_output(expected_lines=[
'data: hello',
'---',
],
text=command.output,
strict=True)
finally:
# Cleanup
self.node.destroy_timer(publish_timer)
self.node.destroy_publisher(publisher)
def __init__(self, node_name, **kwargs):
Node.__init__(self, node_name, **kwargs)
self.__executor = SingleThreadedExecutor()
def main(args=None):
rclpy.init(args=args)
try:
executor = SingleThreadedExecutor()
node_a = FakeLifecycleNode('A')
node_b = FakeLifecycleNode('B')
executor.add_node(node_a)
executor.add_node(node_b)
lc = LifecycleClient()
try:
lc.configure_system()
executor.spin_once(timeout_sec=1)
executor.spin_once(timeout_sec=1)
sleep(2) # give the system some time to converge
lc.activate_system()
executor.spin_once(timeout_sec=1)
executor.spin_once(timeout_sec=1)
executor.spin_once(timeout_sec=1)
sleep(2) # give the system some time to converge
lc.change_mode('CC')
executor.spin()
finally:
executor.shutdown()
node_a.destroy_node()
node_b.destroy_node()
finally:
rclpy.shutdown()
return 0
def main(args=None):
# Argument parsing and usage
parser = get_parser()
parsed_args = parser.parse_args()
# Configuration variables
qos_policy_name = parsed_args.incompatible_qos_policy_name
qos_profile_publisher = QoSProfile(depth=10)
qos_profile_subscription = QoSProfile(depth=10)
if qos_policy_name == 'durability':
print('Durability incompatibility selected.\n'
'Incompatibility condition: publisher durability kind <'
'subscripition durability kind.\n'
'Setting publisher durability to: VOLATILE\n'
'Setting subscription durability to: TRANSIENT_LOCAL\n')
qos_profile_publisher.durability = \
QoSDurabilityPolicy.VOLATILE
qos_profile_subscription.durability = \
QoSDurabilityPolicy.TRANSIENT_LOCAL
elif qos_policy_name == 'deadline':
print(
'Deadline incompatibility selected.\n'
'Incompatibility condition: publisher deadline > subscription deadline.\n'
'Setting publisher durability to: 2 seconds\n'
'Setting subscription durability to: 1 second\n')
qos_profile_publisher.deadline = Duration(seconds=2)
qos_profile_subscription.deadline = Duration(seconds=1)
elif qos_policy_name == 'liveliness_policy':
print('Liveliness Policy incompatibility selected.\n'
'Incompatibility condition: publisher liveliness policy <'
'subscripition liveliness policy.\n'
'Setting publisher liveliness policy to: AUTOMATIC\n'
'Setting subscription liveliness policy to: MANUAL_BY_TOPIC\n')
qos_profile_publisher.liveliness = \
QoSLivelinessPolicy.AUTOMATIC
qos_profile_subscription.liveliness = \
QoSLivelinessPolicy.MANUAL_BY_TOPIC
elif qos_policy_name == 'liveliness_lease_duration':
print(
'Liveliness lease duration incompatibility selected.\n'
'Incompatibility condition: publisher liveliness lease duration >'
'subscription liveliness lease duration.\n'
'Setting publisher liveliness lease duration to: 2 seconds\n'
'Setting subscription liveliness lease duration to: 1 second\n')
qos_profile_publisher.liveliness_lease_duration = Duration(seconds=2)
qos_profile_subscription.liveliness_lease_duration = Duration(
seconds=1)
elif qos_policy_name == 'reliability':
print(
'Reliability incompatibility selected.\n'
'Incompatibility condition: publisher reliability < subscripition reliability.\n'
'Setting publisher reliability to: BEST_EFFORT\n'
'Setting subscription reliability to: RELIABLE\n')
qos_profile_publisher.reliability = \
QoSReliabilityPolicy.BEST_EFFORT
qos_profile_subscription.reliability = \
QoSReliabilityPolicy.RELIABLE
else:
print('{name} not recognised.'.format(name=qos_policy_name))
parser.print_help()
return 0
# Initialization and configuration
rclpy.init(args=args)
topic = 'incompatible_qos_chatter'
num_msgs = 5
publisher_callbacks = PublisherEventCallbacks(
incompatible_qos=lambda event: get_logger('Talker').info(str(event)))
subscription_callbacks = SubscriptionEventCallbacks(
incompatible_qos=lambda event: get_logger('Listener').info(str(event)))
try:
talker = Talker(topic,
qos_profile_publisher,
event_callbacks=publisher_callbacks,
publish_count=num_msgs)
listener = Listener(topic,
qos_profile_subscription,
event_callbacks=subscription_callbacks)
except UnsupportedEventTypeError as exc:
print()
print(exc, end='\n\n')
print('Please try this demo using a different RMW implementation')
return 1
executor = SingleThreadedExecutor()
executor.add_node(listener)
executor.add_node(talker)
try:
while talker.publish_count < num_msgs:
executor.spin_once()
except KeyboardInterrupt:
pass
except ExternalShutdownException:
return 1
finally:
rclpy.try_shutdown()
return 0
def test_lifecycle_services(request):
lc_node_name = 'test_lifecycle_services_lifecycle'
lc_node = LifecycleNode(lc_node_name)
client_node = Node('test_lifecycle_services_client')
get_state_cli = client_node.create_client(lifecycle_msgs.srv.GetState,
f'/{lc_node_name}/get_state')
change_state_cli = client_node.create_client(
lifecycle_msgs.srv.ChangeState, f'/{lc_node_name}/change_state')
get_available_states_cli = client_node.create_client(
lifecycle_msgs.srv.GetAvailableStates,
f'/{lc_node_name}/get_available_states')
get_available_transitions_cli = client_node.create_client(
lifecycle_msgs.srv.GetAvailableTransitions,
f'/{lc_node_name}/get_available_transitions')
get_transition_graph_cli = client_node.create_client(
lifecycle_msgs.srv.GetAvailableTransitions,
f'/{lc_node_name}/get_transition_graph')
for cli in (
get_state_cli,
change_state_cli,
get_available_states_cli,
get_available_transitions_cli,
get_transition_graph_cli,
):
assert cli.wait_for_service(5.)
# lunch a thread to spin the executor, so we can make sync service calls easily
executor = SingleThreadedExecutor()
executor.add_node(client_node)
executor.add_node(lc_node)
thread = Thread(target=executor.spin)
thread.start()
def cleanup():
# Stop executor and join thread.
# This cleanup is run even if an assertion fails.
executor.shutdown()
thread.join()
request.addfinalizer(cleanup)
# test all services
req = lifecycle_msgs.srv.GetState.Request()
resp = get_state_cli.call(req)
assert resp.current_state.label == 'unconfigured'
req = lifecycle_msgs.srv.ChangeState.Request()
req.transition.id = lifecycle_msgs.msg.Transition.TRANSITION_CONFIGURE
resp = change_state_cli.call(req)
assert resp.success
req = lifecycle_msgs.srv.GetState.Request()
resp = get_state_cli.call(req)
assert resp.current_state.label == 'inactive'
req = lifecycle_msgs.srv.GetAvailableStates.Request()
resp = get_available_states_cli.call(req)
states_labels = {state.label for state in resp.available_states}
assert states_labels == {
'unknown', 'unconfigured', 'inactive', 'active', 'finalized',
'configuring', 'cleaningup', 'shuttingdown', 'activating',
'deactivating', 'errorprocessing', ''
}
req = lifecycle_msgs.srv.GetAvailableTransitions.Request()
resp = get_available_transitions_cli.call(req)
transitions_labels = {
transition_def.transition.label
for transition_def in resp.available_transitions
}
assert transitions_labels == {'activate', 'cleanup', 'shutdown'}
req = lifecycle_msgs.srv.GetAvailableTransitions.Request()
resp = get_transition_graph_cli.call(req)
transitions_labels = {
transition_def.transition.label
for transition_def in resp.available_transitions
}
assert transitions_labels == {
'configure', 'activate', 'cleanup', 'shutdown', 'deactivate',
'transition_error', 'transition_failure', 'transition_success'
}
def _run(self):
from rclpy.executors import SingleThreadedExecutor
executor = SingleThreadedExecutor(context=self._context)
executor.add_node(self._node)
while self._run:
executor.spin_once(timeout_sec=1.0)
def test_add_node_to_executor(self):
self.assertIsNotNone(self.node.handle)
executor = SingleThreadedExecutor(context=self.context)
executor.add_node(self.node)
self.assertIn(self.node, executor.get_nodes())
def test_executor_add_node(self):
self.assertIsNotNone(self.node.handle)
executor = SingleThreadedExecutor(context=self.context)
assert executor.add_node(self.node)
assert not executor.add_node(self.node)
def test_create_task_dependent_coroutines(self):
self.assertIsNotNone(self.node.handle)
executor = SingleThreadedExecutor(context=self.context)
executor.add_node(self.node)
async def coro1():
return 'Sentinel Result 1'
future1 = executor.create_task(coro1)
async def coro2():
nonlocal future1
await future1
return 'Sentinel Result 2'
future2 = executor.create_task(coro2)
# Coro2 is newest task, so it gets to await future1 in this spin
executor.spin_once(timeout_sec=0)
# Coro1 execs in this spin
executor.spin_once(timeout_sec=0)
self.assertTrue(future1.done())
self.assertEqual('Sentinel Result 1', future1.result())
self.assertFalse(future2.done())
# Coro2 passes the await step here (timeout change forces new generator)
executor.spin_once(timeout_sec=1)
self.assertTrue(future2.done())
self.assertEqual('Sentinel Result 2', future2.result())
class TurtlebotEnv():
def __init__(self):
self.node_ = rclpy.create_node('turtlebot3_env')
self.executor = SingleThreadedExecutor()
self.executor.add_node(self.node_)
self.act = 0
self.done = False
self.actions = [
[parameters.LINEAR_FWD_VELOCITY, parameters.ANGULAR_FWD_VELOCITY],
[parameters.LINEAR_STR_VELOCITY, parameters.ANGULAR_VELOCITY],
[parameters.LINEAR_STR_VELOCITY, -parameters.ANGULAR_VELOCITY]
]
self.bonous_reward = 0
self.collision = False
self.collision_tol = 0.125
self.laser_scan_range = [0] * 360
self.states_input = [3.5] * 8
self.zero_div_tol = 0.01
self.range_min = 0.0
self.pub_cmd_vel = self.node_.create_publisher(Twist, 'cmd_vel', 1)
self.sub_scan = self.node_.create_subscription(
LaserScan, 'scan', self.scan_callback, qos_profile_sensor_data)
self.reset_simulation = self.node_.create_client(
Empty, 'reset_simulation')
self.reset_world = self.node_.create_client(Empty, 'reset_world')
self.unpause_proxy = self.node_.create_client(Empty, 'unpause_physics')
self.pause_proxy = self.node_.create_client(Empty, 'pause_physics')
self.get_entity_state = self.node_.create_client(
GetEntityState, 'get_entity_state')
self.set_entity_state = self.node_.create_client(
SetEntityState, 'set_entity_state')
self.scan_msg_received = False
self.t = Thread(target=self.executor.spin)
self.t.start()
def cleanup(self):
self.t.join()
def get_reward(self):
reward = 0
if self.collision is True:
reward = -10
self.done = True
return reward, self.done
elif self.collision is False and self.act is 0:
if abs(min(self.states_input)) >= self.zero_div_tol:
reward = 0.08 - (1 / (min(self.states_input)**2)) * 0.005
else:
reward = -10
if reward > 0:
self.bonous_reward += reward
reward = self.bonous_reward
else:
bonous_discount_factor = 0.6
self.bonous_reward *= bonous_discount_factor
if abs(min(self.states_input)) >= self.zero_div_tol:
reward = 0.02 - (1 / min(self.states_input)) * 0.005
else:
reward = -10
return reward, self.done
def scan_callback(self, LaserScan):
self.scan_msg_received = True
self.laser_scan_range = []
self.range_min = LaserScan.range_min
range_max = LaserScan.range_max
for i in range(len(LaserScan.ranges)):
if LaserScan.ranges[i] == float('Inf'):
self.laser_scan_range.append(range_max)
elif LaserScan.ranges[i] < self.range_min:
self.laser_scan_range.append(self.range_min +
self.collision_tol)
else:
self.laser_scan_range.append(LaserScan.ranges[i])
if self.check_collision():
self.collision = True
self.done = True
self.states_input = []
for i in range(8):
step = int(len(LaserScan.ranges) / 8)
self.states_input.append(
min(self.laser_scan_range[i * step:(i + 1) * step], default=0))
def action_space(self):
return len(self.actions)
def step(self, action):
vel_cmd = Twist()
self.act = action
vel_cmd.linear.x = self.actions[action][0]
vel_cmd.angular.z = self.actions[action][1]
self.pub_cmd_vel.publish(vel_cmd)
vel_cmd.linear.x = 0.0
vel_cmd.angular.z = 0.0
get_reward = self.get_reward()
return self.states_input, get_reward[0], self.done
def check_collision(self):
if min(self.laser_scan_range) < self.range_min + self.collision_tol:
print("Near collision detected... " +
str(min(self.laser_scan_range)))
return True
return False
def stop_action(self):
vel_cmd = Twist()
vel_cmd.linear.x = 0.0
vel_cmd.angular.z = 0.0
self.pub_cmd_vel.publish(vel_cmd)
def reset(self):
self.scan_msg_received = False
self.stop_action
while not self.reset_world.wait_for_service(timeout_sec=1.0):
print('Reset world service is not available...')
self.reset_world.call_async(Empty.Request())
while not self.reset_simulation.wait_for_service(timeout_sec=1.0):
print('Reset simulation service is not available...')
self.reset_simulation.call_async(Empty.Request())
self.laser_scan_range = [0] * 360
self.states_input = [3.5] * 8
while not self.scan_msg_received and rclpy.ok():
sleep(0.1)
self.collision = False
self.done = False
self.bonous_reward = 0
return self.states_input
class TestCreateWhileSpinning(unittest.TestCase):
"""
Test that the executor wakes after an entity is created.
This is a regression test for ros2/rclpy#188.
"""
def setUp(self):
rclpy.init()
self.node = rclpy.create_node('TestCreateWhileSpinning',
namespace='/rclpy')
self.executor = SingleThreadedExecutor()
self.executor.add_node(self.node)
self.exec_thread = threading.Thread(target=self.executor.spin)
self.exec_thread.start()
# Make sure executor is blocked by rcl_wait
time.sleep(TIMEOUT)
def tearDown(self):
self.executor.shutdown()
rclpy.shutdown()
self.exec_thread.join()
self.node.destroy_node()
def test_publish_subscribe(self):
evt = threading.Event()
self.node.create_subscription(BasicTypes, 'foo', lambda msg: evt.set(),
1)
pub = self.node.create_publisher(BasicTypes, 'foo', 1)
pub.publish(BasicTypes())
assert evt.wait(TIMEOUT)
def test_client_server(self):
evt = threading.Event()
def trigger_event(req, resp):
nonlocal evt
evt.set()
return resp
self.node.create_service(BasicTypesSrv, 'foo', trigger_event)
cli = self.node.create_client(BasicTypesSrv, 'foo')
cli.wait_for_service()
cli.call_async(BasicTypesSrv.Request())
assert evt.wait(TIMEOUT)
def test_guard_condition(self):
evt = threading.Event()
guard = self.node.create_guard_condition(lambda: evt.set())
guard.trigger()
assert evt.wait(TIMEOUT)
def test_timer(self):
evt = threading.Event()
self.node.create_timer(TIMEOUT / 10, lambda: evt.set())
assert evt.wait(TIMEOUT)
def test_waitable(self):
evt = threading.Event()
class DummyWaitable(Waitable):
def __init__(self):
super().__init__(ReentrantCallbackGroup())
def is_ready(self, wait_set):
return False
def take_data(self):
return None
async def execute(self, taken_data):
pass
def get_num_entities(self):
return NumberOfEntities(0, 0, 0, 0, 0)
def add_to_wait_set(self, wait_set):
nonlocal evt
evt.set()
pass
self.node.add_waitable(DummyWaitable())
assert evt.wait(TIMEOUT)
class WaitForTopics:
"""
Wait to receive messages on supplied topics.
Example usage:
--------------
from std_msgs.msg import String
# Method 1, using the 'with' keyword
def method_1():
topic_list = [('topic_1', String), ('topic_2', String)]
with WaitForTopics(topic_list, timeout=5.0):
# 'topic_1' and 'topic_2' received at least one message each
print('Given topics are receiving messages !')
# Method 2, calling wait() and shutdown() manually
def method_2():
topic_list = [('topic_1', String), ('topic_2', String)]
wait_for_topics = WaitForTopics(topic_list, timeout=5.0)
assert wait_for_topics.wait()
print('Given topics are receiving messages !')
print(wait_for_topics.topics_not_received()) # Should be an empty set
print(wait_for_topics.topics_received()) # Should be {'topic_1', 'topic_2'}
wait_for_topics.shutdown()
"""
def __init__(self, topic_tuples, timeout=5.0):
self.topic_tuples = topic_tuples
self.timeout = timeout
self.__ros_context = rclpy.Context()
rclpy.init(context=self.__ros_context)
self.__ros_executor = SingleThreadedExecutor(
context=self.__ros_context)
self._prepare_ros_node()
# Start spinning
self.__running = True
self.__ros_spin_thread = Thread(target=self._spin_function)
self.__ros_spin_thread.start()
def _prepare_ros_node(self):
node_name = '_test_node_' +\
''.join(random.choices(string.ascii_uppercase + string.digits, k=10))
self.__ros_node = _WaitForTopicsNode(name=node_name,
node_context=self.__ros_context)
self.__ros_executor.add_node(self.__ros_node)
def _spin_function(self):
while self.__running:
self.__ros_executor.spin_once(1.0)
def wait(self):
self.__ros_node.start_subscribers(self.topic_tuples)
return self.__ros_node.msg_event_object.wait(self.timeout)
def shutdown(self):
self.__running = False
self.__ros_spin_thread.join()
self.__ros_node.destroy_node()
rclpy.shutdown(context=self.__ros_context)
def topics_received(self):
"""Topics that received at least one message."""
return self.__ros_node.received_topics
def topics_not_received(self):
"""Topics that did not receive any messages."""
return self.__ros_node.expected_topics - self.__ros_node.received_topics
def __enter__(self):
if not self.wait():
raise RuntimeError('Did not receive messages on these topics: ',
self.topics_not_received())
return self
def __exit__(self, exep_type, exep_value, trace):
if exep_type is not None:
raise Exception('Exception occured, value: ', exep_value)
self.shutdown()
class ROSAdapter:
"""Wraps rclpy API to ease ROS node usage in `launch_ros` actions."""
def __init__(self,
*,
argv: Optional[List[str]] = None,
autostart: bool = True):
"""
Construct adapter.
:param: argv List of global arguments for rclpy context initialization.
:param: autostart Whether to start adapter on construction or not.
"""
# Do not use `None` here, as `rclpy.init` will use `sys.argv` in that case.
self.__argv = [] if argv is None else argv
self.__ros_context = None
self.__ros_node = None
self.__ros_executor = None
self.__is_running = False
if autostart:
self.start()
def start(self):
"""Start ROS adapter."""
if self.__is_running:
raise RuntimeError(
'Cannot start a ROS adapter that is already running')
self.__ros_context = rclpy.Context()
rclpy.init(args=self.__argv, context=self.__ros_context)
self.__ros_node = rclpy.create_node('launch_ros_{}'.format(
os.getpid()),
context=self.__ros_context)
self.__ros_executor = SingleThreadedExecutor(
context=self.__ros_context)
self.__is_running = True
self.__ros_executor_thread = threading.Thread(target=self._run)
self.__ros_executor_thread.start()
def _run(self):
try:
self.__ros_executor.add_node(self.__ros_node)
while self.__is_running:
# TODO(wjwwood): switch this to `spin()` when it considers
# asynchronously added subscriptions.
# see: https://github.com/ros2/rclpy/issues/188
self.__ros_executor.spin_once(timeout_sec=1.0)
except KeyboardInterrupt:
pass
finally:
self.__ros_executor.remove_node(self.__ros_node)
def shutdown(self):
"""Shutdown ROS adapter."""
if not self.__is_running:
raise RuntimeError(
'Cannot shutdown a ROS adapter that is not running')
self.__is_running = False
self.__ros_executor_thread.join()
self.__ros_node.destroy_node()
rclpy.shutdown(context=self.__ros_context)
@property
def argv(self):
return self.__argv
@property
def ros_context(self):
return self.__ros_context
@property
def ros_node(self):
return self.__ros_node
@property
def ros_executor(self):
return self.__ros_executor
def setup_method(self):
self.context = rclpy.context.Context()
rclpy.init(context=self.context)
self.node = rclpy.create_node('test_rate', context=self.context)
self.executor = SingleThreadedExecutor(context=self.context)
self.executor.add_node(self.node)
def setUpClass(cls):
cls.context = rclpy.context.Context()
rclpy.init(context=cls.context)
cls.executor = SingleThreadedExecutor(context=cls.context)
cls.node = rclpy.create_node('TestBufferClient', context=cls.context)
cls.executor.add_node(cls.node)
class MoveAbsJointHandler():
def __init__(self, path, interval):
super().__init__()
self.pipe_path = path
self.interval = interval
self.result = ' '
self.count = 0
self.move = 0
self.executor = SingleThreadedExecutor()
self.node = MoveAbsJointClient()
self.executor.add_node(self.node)
try:
os.mkfifo(self.pipe_path)
except OSError:
print('[Info] MoveAbsJoint: Pipe: ', self.pipe_path, ' exists.')
def rosHandler(self):
while True:
try:
self.executor.spin_once()
except ExternalShutdownException:
try:
rclpy.init()
except:
a = 1
def run(self):
fd = os.open(self.pipe_path, os.O_CREAT | os.O_RDWR)
thread_node = threading.Thread(target=self.rosHandler)
thread_node.start()
while True:
try:
data = os.read(fd, 400)
if data.decode('utf-8').split(';')[0] == 'MoveAbsJoint':
print('[Info] MoveAbsJoint gets request from Pipe.')
try:
msg = data.decode('utf-8').split(';')[1:-1]
goal = MotionMoveAbsJoint.Goal()
goal.id = 1
goal.joint = [0.0] * 8
joint = [float(x) for x in msg[0].split(',')[:]]
for i in range(len(joint)):
goal.joint[i] = joint[i]
extjoint = [float(x) for x in msg[1].split(',')[:]]
goal.extjoint = [0.0] * 8
for i in range(len(extjoint)):
goal.extjoint[i] = extjoint[i]
load = [float(x) for x in msg[2].split(',')[:]]
goal.load = [0.0] * 10
for i in range(len(load)):
goal.load[i] = load[i]
goal.speed = float(msg[3])
goal.zone = float(msg[4])
time1 = time.time()
self.node.send_goal(goal)
self.count += 1
time2 = time.time()
print('[Info] MoveAbsJoint sends request to ROS.')
print('[Info] Goal:', goal.joint)
print('[Info] Node send time', time2 - time1)
while self.count != self.node._count:
#print(self.count,self.node._count)
time.sleep(self.interval / 2)
self.result = self.node._ret
time1 = time.time()
print('[Info] MoveAbsJoint gets result from ROS.')
print('[Info] Result:', self.result)
print('[Info] Node spin time', time1 - time2)
if self.result > 0:
self.pipe_result = 'y' + ' ' * 399
elif self.result == 0:
time1 = time.time()
print(
'[Info] MoveAbsJoint resends request from ROS.'
)
retry = 0
while self.node._ret <= 0:
time.sleep(self.interval * 16)
self.node.send_goal(goal)
self.count += 1
retry += 1
while self.count != self.node._count:
time.sleep(self.interval / 2)
time2 = time.time()
print('[Info] Node resend times:', retry,
'. Time spent:', time2 - time1)
self.result = self.node._ret
self.pipe_result = 'y' + ' ' * 399
else:
self.pipe_result = 'n1' + ' ' * 398
print('[Error] MoveAbsJoint gets error result.')
os.write(fd, self.pipe_result.encode('utf-8'))
print('[Info] MoveAbsJoint sends result to Pipe.',
self.result)
self.move += 1
print('[Info] MoveAbsJoint counter', self.move)
except:
time.sleep(self.interval / 2)
elif data.decode('utf-8')[0] == 'y' or data.decode(
'utf-8')[0] == 'n':
print('[Info] MoveAbsJoint resends result to Pipe.')
os.write(fd, data)
time.sleep(self.interval)
except:
print('[Error] MoveAbsJoint.')
time.sleep(self.interval / 2)
def test_action_client(self):
"""Test simple action states"""
node = rclpy.create_node('simple_action_test')
node.get_logger().set_level(rclpy.logging.LoggingSeverity.DEBUG)
executor = SingleThreadedExecutor()
def spin():
rclpy.spin(node, executor=executor)
sq = Sequence(['succeeded', 'aborted', 'preempted', 'foobar'],
'succeeded')
sq.userdata['g1'] = g1
sq.userdata['g2'] = g2
sq.userdata['order'] = 1
sq.userdata['goal_alias'] = 1
with sq:
# Test single goal policy
Sequence.add(
'GOAL_STATIC',
SimpleActionState(node, "fibonacci", Fibonacci, goal=g1))
Sequence.add(
'GOAL_KEY',
SimpleActionState(node, "fibonacci", Fibonacci, goal_key='g1'))
Sequence.add(
'GOAL_SLOTS',
SimpleActionState(node,
"fibonacci",
Fibonacci,
goal_slots=['order']))
Sequence.add('GOAL_SLOTS_REMAP',
SimpleActionState(node,
"fibonacci",
Fibonacci,
goal_slots=['order']),
remapping={'order': 'goal_alias'})
# Test goal callback
def goal_cb_0(ud, default_goal):
return Fibonacci.Goal(order=1)
Sequence.add(
'GOAL_CB',
SimpleActionState(node,
"fibonacci",
Fibonacci,
goal_cb=goal_cb_0))
Sequence.add(
'GOAL_CB_LAMBDA',
SimpleActionState(
node,
"fibonacci",
Fibonacci,
goal_cb=lambda ud, goal: Fibonacci.Goal(order=1)))
Sequence.add(
'GOAL_CB_UD',
SimpleActionState(node,
"fibonacci",
Fibonacci,
goal_cb=lambda ud, goal: ud.g1,
input_keys=['g1']))
@cb_interface(input_keys=['g1'])
def goal_cb_1(ud, default_goal):
return ud.g1
Sequence.add(
'GOAL_CB_UD_DECORATOR',
SimpleActionState(node,
"fibonacci",
Fibonacci,
goal_cb=goal_cb_1))
Sequence.add(
'GOAL_CB_ARGS',
SimpleActionState(
node,
"fibonacci",
Fibonacci,
goal_cb=lambda ud, goal, g: Fibonacci.Goal(order=g),
goal_cb_args=[1]))
Sequence.add(
'GOAL_CB_KWARGS',
SimpleActionState(
node,
"fibonacci",
Fibonacci,
goal_cb=lambda ud, goal, gg: Fibonacci.Goal(order=gg),
goal_cb_kwargs={'gg': 1}))
Sequence.add(
'GOAL_CB_ARGS_KWARGS',
SimpleActionState(node,
"fibonacci",
Fibonacci,
goal_cb=lambda ud, goal, g, gg: Fibonacci.
Goal(order=(g - gg)),
goal_cb_args=[2],
goal_cb_kwargs={'gg': 1}))
# Test overriding goal policies
Sequence.add(
'GOAL_STATIC_SLOTS',
SimpleActionState(node,
"fibonacci",
Fibonacci,
goal=g2,
goal_slots=['order']))
Sequence.add(
'GOAL_STATIC_CB',
SimpleActionState(
node,
"fibonacci",
Fibonacci,
goal=g2,
goal_cb=CBInterface(
lambda ud, goal: setattr(goal, 'order', 1),
output_keys=['goal'])))
# Test result policies
Sequence.add(
'RESULT_KEY',
SimpleActionState(node,
"fibonacci",
Fibonacci,
goal=g1,
result_key='res_key'))
Sequence.add('RESULT_KEY_CHECK', AssertUDState(['res_key']))
Sequence.add(
'RESULT_CB',
SimpleActionState(
node,
"fibonacci",
Fibonacci,
goal=g1,
result_cb=CBInterface(
lambda ud, res_stat, res: setattr(ud, 'res_cb', res),
output_keys=['res_cb'])))
Sequence.add('RESULT_CB_CHECK', AssertUDState(['res_cb']))
Sequence.add(
'RESULT_SLOTS',
SimpleActionState(node,
"fibonacci",
Fibonacci,
goal=g1,
result_slots=['sequence']))
Sequence.add('RESULT_SLOTS_CHECK', AssertUDState(['sequence']))
Sequence.add('RESULT_SLOTS_REMAP',
SimpleActionState(node,
"fibonacci",
Fibonacci,
goal=g1,
result_slots=['sequence']),
remapping={'sequence': 'res_alias'})
Sequence.add('RESULT_SLOTS_MAP_CHECK',
AssertUDState(['res_alias']))
Sequence.add(
'RESULT_CB_OUTCOME',
SimpleActionState(node,
"fibonacci",
Fibonacci,
goal=g1,
result_cb=CBInterface(
lambda ud, res_stat, res: 'foobar',
outcomes=['foobar'])))
spinner = threading.Thread(target=spin)
spinner.start()
sq_outcome = sq.execute()
assert sq_outcome == 'foobar'
