diff --git a/ros2topic/ros2topic/api/__init__.py b/ros2topic/ros2topic/api/__init__.py index ed9cf4c45..c9688071c 100644 --- a/ros2topic/ros2topic/api/__init__.py +++ b/ros2topic/ros2topic/api/__init__.py @@ -157,13 +157,11 @@ def __call__(self, prefix, parsed_args, **kwargs): return [message_to_yaml(message())] -def qos_profile_from_short_keys( - preset_profile: str, reliability: str = None, durability: str = None, - depth: Optional[int] = None, history: str = None, +def profile_configure_short_keys( + profile: rclpy.qos.QoSProfile = None, reliability: str = None, + durability: str = None, depth: Optional[int] = None, history: str = None, ) -> rclpy.qos.QoSProfile: - """Construct a QoSProfile given the name of a preset, and optional overrides.""" - # Build a QoS profile based on user-supplied arguments - profile = rclpy.qos.QoSPresetProfiles.get_from_short_key(preset_profile) + """Configure a QoSProfile given a profile, and optional overrides.""" if history: profile.history = rclpy.qos.QoSHistoryPolicy.get_from_short_key(history) if durability: @@ -176,4 +174,14 @@ def qos_profile_from_short_keys( if (profile.durability == rclpy.qos.QoSDurabilityPolicy.TRANSIENT_LOCAL and profile.depth == 0): profile.depth = 1 + + +def qos_profile_from_short_keys( + preset_profile: str, reliability: str = None, durability: str = None, + depth: Optional[int] = None, history: str = None, +) -> rclpy.qos.QoSProfile: + """Construct a QoSProfile given the name of a preset, and optional overrides.""" + # Build a QoS profile based on user-supplied arguments + profile = rclpy.qos.QoSPresetProfiles.get_from_short_key(preset_profile) + profile_configure_short_keys(profile, reliability, durability, depth, history) return profile diff --git a/ros2topic/ros2topic/verb/pub.py b/ros2topic/ros2topic/verb/pub.py index 7a1bc680f..7a05f030a 100644 --- a/ros2topic/ros2topic/verb/pub.py +++ b/ros2topic/ros2topic/verb/pub.py @@ -18,9 +18,11 @@ import rclpy from rclpy.node import Node +from rclpy.qos import QoSDurabilityPolicy from rclpy.qos import QoSProfile +from rclpy.qos import QoSReliabilityPolicy from ros2cli.node.direct import DirectNode -from ros2topic.api import qos_profile_from_short_keys +from ros2topic.api import profile_configure_short_keys from ros2topic.api import TopicMessagePrototypeCompleter from ros2topic.api import TopicNameCompleter from ros2topic.api import TopicTypeCompleter @@ -30,7 +32,6 @@ import yaml MsgType = TypeVar('MsgType') -default_profile_str = 'system_default' def nonnegative_int(inval): @@ -49,6 +50,13 @@ def positive_float(inval): return ret +def get_pub_qos_profile(): + return QoSProfile( + reliability=QoSReliabilityPolicy.RELIABLE, + durability=QoSDurabilityPolicy.TRANSIENT_LOCAL, + depth=1) + + class PubVerb(VerbExtension): """Publish a message to a topic.""" @@ -98,11 +106,8 @@ def add_arguments(self, parser, cli_name): parser.add_argument( '--qos-profile', choices=rclpy.qos.QoSPresetProfiles.short_keys(), - default=default_profile_str, - help='Quality of service preset profile to {} with (default: {})' - .format('publish', default_profile_str)) - default_profile = rclpy.qos.QoSPresetProfiles.get_from_short_key( - default_profile_str) + help='Quality of service preset profile to publish)') + default_profile = get_pub_qos_profile() parser.add_argument( '--qos-depth', metavar='N', type=int, default=-1, help='Queue size setting to publish with ' @@ -131,17 +136,19 @@ def main(self, *, args): def main(args): + qos_profile = get_pub_qos_profile() qos_profile_name = args.qos_profile - if not qos_profile_name: - qos_profile_name = default_profile_str + if qos_profile_name: + qos_profile = rclpy.qos.QoSPresetProfiles.get_from_short_key(qos_profile_name) + profile_configure_short_keys( + qos_profile, args.qos_reliability, args.qos_durability, + args.qos_depth, args.qos_history) - qos_profile = qos_profile_from_short_keys( - qos_profile_name, reliability=args.qos_reliability, durability=args.qos_durability, - depth=args.qos_depth, history=args.qos_history) times = args.times if args.once: times = 1 + with DirectNode(args, node_name=args.node_name) as node: return publisher( node.node,