diff --git a/ros2service/package.xml b/ros2service/package.xml
index ada9cfa0d..26262ded8 100644
--- a/ros2service/package.xml
+++ b/ros2service/package.xml
@@ -20,6 +20,7 @@
python3-yaml
rclpy
ros2cli
+ ros2topic
rosidl_runtime_py
ament_copyright
diff --git a/ros2service/ros2service/verb/call.py b/ros2service/ros2service/verb/call.py
index 7ee5dd850..a3cbf9dfc 100644
--- a/ros2service/ros2service/verb/call.py
+++ b/ros2service/ros2service/verb/call.py
@@ -16,12 +16,14 @@
import time
import rclpy
+from rclpy.qos import QoSPresetProfiles
from ros2cli.helpers import collect_stdin
from ros2cli.node import NODE_NAME_PREFIX
from ros2service.api import ServiceNameCompleter
from ros2service.api import ServicePrototypeCompleter
from ros2service.api import ServiceTypeCompleter
from ros2service.verb import VerbExtension
+from ros2topic.api import add_qos_arguments, profile_configure_short_keys
from rosidl_runtime_py import set_message_fields
import yaml
@@ -54,22 +56,28 @@ def add_arguments(self, parser, cli_name):
parser.add_argument(
'-r', '--rate', metavar='N', type=float,
help='Repeat the call at a specific rate in Hz')
+ add_qos_arguments(parser, 'service client', 'services_default')
def main(self, *, args):
if args.rate is not None and args.rate <= 0:
raise RuntimeError('rate must be greater than zero')
period = 1. / args.rate if args.rate else None
+ default_profile = QoSPresetProfiles.get_from_short_key(args.qos_profile)
+ profile_configure_short_keys(
+ default_profile, args.qos_reliability, args.qos_durability,
+ args.qos_depth, args.qos_history)
+
if args.stdin:
values = collect_stdin()
else:
values = args.values
return requester(
- args.service_type, args.service_name, values, period)
+ args.service_type, args.service_name, values, period, default_profile)
-def requester(service_type, service_name, values, period):
+def requester(service_type, service_name, values, period, qos_profile):
# TODO(wjwwood) this logic should come from a rosidl related package
try:
parts = service_type.split('/')
@@ -92,7 +100,10 @@ def requester(service_type, service_name, values, period):
with rclpy.init():
node = rclpy.create_node(NODE_NAME_PREFIX + '_requester_%s_%s' % (package_name, srv_name))
- cli = node.create_client(srv_module, service_name)
+ cli = node.create_client(
+ srv_module,
+ service_name,
+ qos_profile=qos_profile)
request = srv_module.Request()
diff --git a/ros2service/test/test_cli.py b/ros2service/test/test_cli.py
index 8333ddf41..d9fb9d2ad 100644
--- a/ros2service/test/test_cli.py
+++ b/ros2service/test/test_cli.py
@@ -326,3 +326,40 @@ def test_repeated_call(self):
bool_value=True, int32_value=1, float64_value=1.0, string_value='foobar'
)
), timeout=10)
+
+ @launch_testing.markers.retry_on_failure(times=5, delay=1)
+ def test_call_with_qos_option(self):
+ with self.launch_service_command(
+ arguments=[
+ 'call',
+ '--qos-profile',
+ 'system_default',
+ '--qos-depth',
+ '5',
+ '--qos-history',
+ 'system_default',
+ '--qos-reliability',
+ 'system_default',
+ '--qos-durability',
+ 'system_default',
+ '--qos-liveliness',
+ 'system_default',
+ '--qos-liveliness-lease-duration',
+ '0',
+ '/my_ns/echo',
+ 'test_msgs/srv/BasicTypes',
+ '{bool_value: false, int32_value: -1, float64_value: 0.1, string_value: bazbar}'
+ ]
+ ) as service_command:
+ assert service_command.wait_for_shutdown(timeout=10)
+ assert service_command.exit_code == launch_testing.asserts.EXIT_OK
+ assert launch_testing.tools.expect_output(
+ expected_lines=get_echo_call_output(
+ bool_value=False,
+ int32_value=-1,
+ float64_value=0.1,
+ string_value='bazbar'
+ ),
+ text=service_command.output,
+ strict=True
+ )
diff --git a/ros2topic/ros2topic/api/__init__.py b/ros2topic/ros2topic/api/__init__.py
index 7b5b4f94a..62ebd92a4 100644
--- a/ros2topic/ros2topic/api/__init__.py
+++ b/ros2topic/ros2topic/api/__init__.py
@@ -173,7 +173,7 @@ def profile_configure_short_keys(
profile: rclpy.qos.QoSProfile = None, reliability: Optional[str] = None,
durability: Optional[str] = None, depth: Optional[int] = None, history: Optional[str] = None,
liveliness: Optional[str] = None, liveliness_lease_duration_s: Optional[int] = None,
-) -> rclpy.qos.QoSProfile:
+) -> None:
"""Configure a QoSProfile given a profile, and optional overrides."""
if history:
profile.history = rclpy.qos.QoSHistoryPolicy.get_from_short_key(history)
@@ -206,50 +206,56 @@ def qos_profile_from_short_keys(
return profile
-def add_qos_arguments(parser: ArgumentParser, subscribe_or_publish: str, default_profile_str):
+def add_qos_arguments(
+ parser: ArgumentParser, entity_type: str,
+ default_profile_str: str = 'default', extra_message: str = ''
+) -> None:
parser.add_argument(
'--qos-profile',
choices=rclpy.qos.QoSPresetProfiles.short_keys(),
help=(
- f'Quality of service preset profile to {subscribe_or_publish} with'
+ f'Quality of service preset profile to {entity_type} with'
f' (default: {default_profile_str})'),
default=default_profile_str)
default_profile = rclpy.qos.QoSPresetProfiles.get_from_short_key(default_profile_str)
parser.add_argument(
'--qos-depth', metavar='N', type=int,
help=(
- f'Queue size setting to {subscribe_or_publish} with '
- '(overrides depth value of --qos-profile option)'))
+ f'Queue size setting to {entity_type} with '
+ '(overrides depth value of --qos-profile option, default: '
+ f'{default_profile.depth})'))
parser.add_argument(
'--qos-history',
choices=rclpy.qos.QoSHistoryPolicy.short_keys(),
help=(
- f'History of samples setting to {subscribe_or_publish} with '
+ f'History of samples setting to {entity_type} with '
'(overrides history value of --qos-profile option, default: '
f'{default_profile.history.short_key})'))
parser.add_argument(
'--qos-reliability',
choices=rclpy.qos.QoSReliabilityPolicy.short_keys(),
help=(
- f'Quality of service reliability setting to {subscribe_or_publish} with '
+ f'Quality of service reliability setting to {entity_type} with '
'(overrides reliability value of --qos-profile option, default: '
- 'Compatible profile with running endpoints )'))
+ f'{default_profile.reliability.short_key} {extra_message})'))
parser.add_argument(
'--qos-durability',
choices=rclpy.qos.QoSDurabilityPolicy.short_keys(),
help=(
- f'Quality of service durability setting to {subscribe_or_publish} with '
+ f'Quality of service durability setting to {entity_type} with '
'(overrides durability value of --qos-profile option, default: '
- 'Compatible profile with running endpoints )'))
+ f'{default_profile.durability.short_key} {extra_message})'))
parser.add_argument(
'--qos-liveliness',
choices=rclpy.qos.QoSLivelinessPolicy.short_keys(),
help=(
- f'Quality of service liveliness setting to {subscribe_or_publish} with '
- '(overrides liveliness value of --qos-profile option'))
+ f'Quality of service liveliness setting to {entity_type} with '
+ '(overrides liveliness value of --qos-profile option, default '
+ f'{default_profile.liveliness.short_key})'))
parser.add_argument(
'--qos-liveliness-lease-duration-seconds',
type=float,
help=(
- f'Quality of service liveliness lease duration setting to {subscribe_or_publish} '
- 'with (overrides liveliness lease duration value of --qos-profile option'))
+ f'Quality of service liveliness lease duration setting to {entity_type} '
+ 'with (overrides liveliness lease duration value of --qos-profile option, default: '
+ f'{default_profile.liveliness_lease_duration})'))
diff --git a/ros2topic/ros2topic/verb/echo.py b/ros2topic/ros2topic/verb/echo.py
index db72b51ea..56693ecfc 100644
--- a/ros2topic/ros2topic/verb/echo.py
+++ b/ros2topic/ros2topic/verb/echo.py
@@ -57,7 +57,9 @@ def add_arguments(self, parser, cli_name):
parser.add_argument(
'message_type', nargs='?',
help="Type of the ROS message (e.g. 'std_msgs/msg/String')")
- add_qos_arguments(parser, 'subscribe', 'sensor_data')
+ add_qos_arguments(
+ parser, 'subscribe', 'sensor_data',
+ ' / compatible profile with running endpoints')
parser.add_argument(
'--csv', action='store_true',
help=(