Skip to content

Commit 0a2e7ea

Browse files
[squash before merge] React on comments
1 parent 8e9f8d9 commit 0a2e7ea

5 files changed

Lines changed: 41 additions & 108 deletions

File tree

ros2service/package.xml

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -18,6 +18,7 @@
1818

1919
<depend>rclpy</depend>
2020
<depend>ros2cli</depend>
21+
<depend>ros2topic</depend>
2122

2223
<exec_depend>python3-yaml</exec_depend>
2324
<exec_depend>rosidl_runtime_py</exec_depend>

ros2service/ros2service/api/__init__.py

Lines changed: 0 additions & 34 deletions
Original file line numberDiff line numberDiff line change
@@ -12,10 +12,6 @@
1212
# See the License for the specific language governing permissions and
1313
# limitations under the License.
1414

15-
from typing import Optional
16-
17-
import rclpy
18-
1915
from rclpy.topic_or_service_is_hidden import topic_or_service_is_hidden
2016
from ros2cli.node.strategy import NodeStrategy
2117
from rosidl_runtime_py import get_service_interfaces
@@ -88,33 +84,3 @@ def __init__(self, *, service_type_key=None):
8884
def __call__(self, prefix, parsed_args, **kwargs):
8985
service = get_service(getattr(parsed_args, self.service_type_key))
9086
return [message_to_yaml(service.Request())]
91-
92-
93-
def profile_configure_short_keys(
94-
profile: rclpy.qos.QoSProfile = None, reliability: str = None,
95-
durability: str = None, depth: Optional[int] = None, history: str = None,
96-
) -> rclpy.qos.QoSProfile:
97-
"""Configure a QoSProfile given a profile, and optional overrides."""
98-
if history:
99-
profile.history = rclpy.qos.QoSHistoryPolicy.get_from_short_key(history)
100-
if durability:
101-
profile.durability = rclpy.qos.QoSDurabilityPolicy.get_from_short_key(durability)
102-
if reliability:
103-
profile.reliability = rclpy.qos.QoSReliabilityPolicy.get_from_short_key(reliability)
104-
if depth and depth >= 0:
105-
profile.depth = depth
106-
else:
107-
if (profile.durability == rclpy.qos.QoSDurabilityPolicy.TRANSIENT_LOCAL
108-
and profile.depth == 0):
109-
profile.depth = 1
110-
111-
112-
def qos_profile_from_short_keys(
113-
preset_profile: str, reliability: str = None, durability: str = None,
114-
depth: Optional[int] = None, history: str = None,
115-
) -> rclpy.qos.QoSProfile:
116-
"""Construct a QoSProfile given the name of a preset, and optional overrides."""
117-
# Build a QoS profile based on user-supplied arguments
118-
profile = rclpy.qos.QoSPresetProfiles.get_from_short_key(preset_profile)
119-
profile_configure_short_keys(profile, reliability, durability, depth, history)
120-
return profile

ros2service/ros2service/verb/call.py

Lines changed: 6 additions & 45 deletions
Original file line numberDiff line numberDiff line change
@@ -16,26 +16,17 @@
1616
import time
1717

1818
import rclpy
19-
from rclpy.qos import QoSDurabilityPolicy
20-
from rclpy.qos import QoSProfile
21-
from rclpy.qos import QoSReliabilityPolicy
19+
from rclpy.qos import QoSPresetProfiles
2220
from ros2cli.node import NODE_NAME_PREFIX
2321
from ros2service.api import ServiceNameCompleter
2422
from ros2service.api import ServicePrototypeCompleter
2523
from ros2service.api import ServiceTypeCompleter
24+
from ros2topic.api import profile_configure_short_keys, add_qos_arguments
2625
from ros2service.verb import VerbExtension
27-
from ros2service.api import profile_configure_short_keys
2826
from rosidl_runtime_py import set_message_fields
2927
import yaml
3028

3129

32-
def get_pub_qos_profile():
33-
return QoSProfile(
34-
reliability=QoSReliabilityPolicy.RELIABLE,
35-
durability=QoSDurabilityPolicy.TRANSIENT_LOCAL,
36-
depth=1)
37-
38-
3930
class CallVerb(VerbExtension):
4031
"""Call a service."""
4132

@@ -60,50 +51,20 @@ def add_arguments(self, parser, cli_name):
6051
parser.add_argument(
6152
'-r', '--rate', metavar='N', type=float,
6253
help='Repeat the call at a specific rate in Hz')
63-
parser.add_argument(
64-
'--qos-profile',
65-
choices=rclpy.qos.QoSPresetProfiles.short_keys(),
66-
help='Quality of service preset profile to publish)')
67-
default_profile = get_pub_qos_profile()
68-
parser.add_argument(
69-
'--qos-depth', metavar='N', type=int, default=-1,
70-
help='Queue size setting to publish with '
71-
'(overrides depth value of --qos-profile option)')
72-
parser.add_argument(
73-
'--qos-history',
74-
choices=rclpy.qos.QoSHistoryPolicy.short_keys(),
75-
help='History of samples setting to publish with '
76-
'(overrides history value of --qos-profile option, default: {})'
77-
.format(default_profile.history.short_key))
78-
parser.add_argument(
79-
'--qos-reliability',
80-
choices=rclpy.qos.QoSReliabilityPolicy.short_keys(),
81-
help='Quality of service reliability setting to publish with '
82-
'(overrides reliability value of --qos-profile option, default: {})'
83-
.format(default_profile.reliability.short_key))
84-
parser.add_argument(
85-
'--qos-durability',
86-
choices=rclpy.qos.QoSDurabilityPolicy.short_keys(),
87-
help='Quality of service durability setting to publish with '
88-
'(overrides durability value of --qos-profile option, default: {})'
89-
.format(default_profile.durability.short_key))
54+
add_qos_arguments(parser, 'services_default')
9055

9156
def main(self, *, args):
9257
if args.rate is not None and args.rate <= 0:
9358
raise RuntimeError('rate must be greater than zero')
9459
period = 1. / args.rate if args.rate else None
9560

96-
qos_profile = get_pub_qos_profile()
97-
98-
qos_profile_name = args.qos_profile
99-
if qos_profile_name:
100-
qos_profile = rclpy.qos.QoSPresetProfiles.get_from_short_key(qos_profile_name)
61+
default_profile = QoSPresetProfiles.get_from_short_key(args.qos_profile)
10162
profile_configure_short_keys(
102-
qos_profile, args.qos_reliability, args.qos_durability,
63+
default_profile, args.qos_reliability, args.qos_durability,
10364
args.qos_depth, args.qos_history)
10465

10566
return requester(
106-
args.service_type, args.service_name, args.values, period, qos_profile)
67+
args.service_type, args.service_name, args.values, period, default_profile)
10768

10869

10970
def requester(service_type, service_name, values, period, qos_profile):

ros2topic/ros2topic/api/__init__.py

Lines changed: 32 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -185,3 +185,35 @@ def qos_profile_from_short_keys(
185185
profile = rclpy.qos.QoSPresetProfiles.get_from_short_key(preset_profile)
186186
profile_configure_short_keys(profile, reliability, durability, depth, history)
187187
return profile
188+
189+
190+
def add_qos_arguments(parser, default_profile_str):
191+
parser.add_argument(
192+
'--qos-profile',
193+
choices=rclpy.qos.QoSPresetProfiles.short_keys(),
194+
default=default_profile_str,
195+
help='Quality of service preset profile to subscribe with (default: {})'
196+
.format(default_profile_str))
197+
default_profile = rclpy.qos.QoSPresetProfiles.get_from_short_key(default_profile_str)
198+
parser.add_argument(
199+
'--qos-depth', metavar='N', type=int,
200+
help='Queue size setting to subscribe with '
201+
'(overrides depth value of --qos-profile option)')
202+
parser.add_argument(
203+
'--qos-history',
204+
choices=rclpy.qos.QoSHistoryPolicy.short_keys(),
205+
help='History of samples setting to subscribe with '
206+
'(overrides history value of --qos-profile option, default: {})'
207+
.format(default_profile.history.short_key))
208+
parser.add_argument(
209+
'--qos-reliability',
210+
choices=rclpy.qos.QoSReliabilityPolicy.short_keys(),
211+
help='Quality of service reliability setting to subscribe with '
212+
'(overrides reliability value of --qos-profile option, default: '
213+
'Automatically match existing publishers )')
214+
parser.add_argument(
215+
'--qos-durability',
216+
choices=rclpy.qos.QoSDurabilityPolicy.short_keys(),
217+
help='Quality of service durability setting to subscribe with '
218+
'(overrides durability value of --qos-profile option, default: '
219+
'Automatically match existing publishers )')

ros2topic/ros2topic/verb/echo.py

Lines changed: 2 additions & 29 deletions
Original file line numberDiff line numberDiff line change
@@ -28,7 +28,7 @@
2828
from ros2cli.node.strategy import add_arguments as add_strategy_node_arguments
2929
from ros2cli.node.strategy import NodeStrategy
3030
from ros2topic.api import get_msg_class
31-
from ros2topic.api import qos_profile_from_short_keys
31+
from ros2topic.api import qos_profile_from_short_keys, add_qos_arguments
3232
from ros2topic.api import TopicNameCompleter
3333
from ros2topic.api import unsigned_int
3434
from ros2topic.verb import VerbExtension
@@ -55,34 +55,7 @@ def add_arguments(self, parser, cli_name):
5555
parser.add_argument(
5656
'message_type', nargs='?',
5757
help="Type of the ROS message (e.g. 'std_msgs/msg/String')")
58-
parser.add_argument(
59-
'--qos-profile',
60-
choices=rclpy.qos.QoSPresetProfiles.short_keys(),
61-
help='Quality of service preset profile to subscribe with (default: {})'
62-
.format(default_profile_str))
63-
default_profile = rclpy.qos.QoSPresetProfiles.get_from_short_key(default_profile_str)
64-
parser.add_argument(
65-
'--qos-depth', metavar='N', type=int,
66-
help='Queue size setting to subscribe with '
67-
'(overrides depth value of --qos-profile option)')
68-
parser.add_argument(
69-
'--qos-history',
70-
choices=rclpy.qos.QoSHistoryPolicy.short_keys(),
71-
help='History of samples setting to subscribe with '
72-
'(overrides history value of --qos-profile option, default: {})'
73-
.format(default_profile.history.short_key))
74-
parser.add_argument(
75-
'--qos-reliability',
76-
choices=rclpy.qos.QoSReliabilityPolicy.short_keys(),
77-
help='Quality of service reliability setting to subscribe with '
78-
'(overrides reliability value of --qos-profile option, default: '
79-
'Automatically match existing publishers )')
80-
parser.add_argument(
81-
'--qos-durability',
82-
choices=rclpy.qos.QoSDurabilityPolicy.short_keys(),
83-
help='Quality of service durability setting to subscribe with '
84-
'(overrides durability value of --qos-profile option, default: '
85-
'Automatically match existing publishers )')
58+
add_qos_arguments(parser, default_profile_str)
8659
parser.add_argument(
8760
'--csv', action='store_true',
8861
help='Output all recursive fields separated by commas (e.g. for '

0 commit comments

Comments
 (0)