Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Load a message/request/goal from standard input #844

Merged
merged 3 commits into from
Aug 3, 2023
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
17 changes: 14 additions & 3 deletions ros2action/ros2action/verb/send_goal.py
Original file line number Diff line number Diff line change
Expand Up @@ -19,6 +19,7 @@
from ros2action.api import ActionGoalPrototypeCompleter
from ros2action.api import ActionTypeCompleter
from ros2action.verb import VerbExtension
from ros2cli.helpers import collect_stdin
from ros2cli.node import NODE_NAME_PREFIX
from rosidl_runtime_py import message_to_yaml
from rosidl_runtime_py import set_message_fields
Expand All @@ -39,9 +40,13 @@ def add_arguments(self, parser, cli_name):
'action_type',
help="Type of the ROS action (e.g. 'example_interfaces/action/Fibonacci')")
arg.completer = ActionTypeCompleter(action_name_key='action_name')
arg = parser.add_argument(
'goal',
group = parser.add_mutually_exclusive_group()
arg = group.add_argument(
'goal', nargs='?', default='{}',
help="Goal request values in YAML format (e.g. '{order: 10}')")
group.add_argument(
'--stdin', action='store_true',
help='Read goal from standard input')
arg.completer = ActionGoalPrototypeCompleter(action_type_key='action_type')
parser.add_argument(
'-f', '--feedback', action='store_true',
Expand All @@ -51,7 +56,13 @@ def main(self, *, args):
feedback_callback = None
if args.feedback:
feedback_callback = _feedback_callback
return send_goal(args.action_name, args.action_type, args.goal, feedback_callback)

if args.stdin:
goal = collect_stdin()
else:
goal = args.goal

return send_goal(args.action_name, args.action_type, goal, feedback_callback)


def _goal_status_to_string(status):
Expand Down
11 changes: 11 additions & 0 deletions ros2cli/ros2cli/helpers.py
Original file line number Diff line number Diff line change
Expand Up @@ -16,6 +16,7 @@
import functools
import inspect
import os
import sys
import time


Expand Down Expand Up @@ -107,3 +108,13 @@ def unsigned_int(string):
if value < 0:
raise ArgumentTypeError('value must be non-negative integer')
return value


def collect_stdin():
lines = b''
while True:
line = sys.stdin.buffer.readline()
if not line:
break
lines += line
return lines
14 changes: 12 additions & 2 deletions ros2service/ros2service/verb/call.py
Original file line number Diff line number Diff line change
Expand Up @@ -16,6 +16,7 @@
import time

import rclpy
from ros2cli.helpers import collect_stdin
from ros2cli.node import NODE_NAME_PREFIX
from ros2service.api import ServiceNameCompleter
from ros2service.api import ServicePrototypeCompleter
Expand All @@ -39,13 +40,17 @@ def add_arguments(self, parser, cli_name):
help="Type of the ROS service (e.g. 'std_srvs/srv/Empty')")
arg.completer = ServiceTypeCompleter(
service_name_key='service_name')
arg = parser.add_argument(
group = parser.add_mutually_exclusive_group()
arg = group.add_argument(
'values', nargs='?', default='{}',
help='Values to fill the service request with in YAML format ' +
"(e.g. '{a: 1, b: 2}'), " +
'otherwise the service request will be published with default values')
arg.completer = ServicePrototypeCompleter(
service_type_key='service_type')
group.add_argument(
'--stdin', action='store_true',
help='Read values from standard input')
parser.add_argument(
'-r', '--rate', metavar='N', type=float,
help='Repeat the call at a specific rate in Hz')
Expand All @@ -55,8 +60,13 @@ def main(self, *, args):
raise RuntimeError('rate must be greater than zero')
period = 1. / args.rate if args.rate else None

if args.stdin:
values = collect_stdin()
else:
values = args.values

return requester(
args.service_type, args.service_name, args.values, period)
args.service_type, args.service_name, values, period)


def requester(service_type, service_name, values, period):
Expand Down
14 changes: 12 additions & 2 deletions ros2topic/ros2topic/verb/pub.py
Original file line number Diff line number Diff line change
Expand Up @@ -19,6 +19,7 @@
import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile
from ros2cli.helpers import collect_stdin
from ros2cli.node.direct import add_arguments as add_direct_node_arguments
from ros2cli.node.direct import DirectNode
from ros2topic.api import add_qos_arguments
Expand Down Expand Up @@ -58,13 +59,17 @@ def add_arguments(self, parser, cli_name):
help="Type of the ROS message (e.g. 'std_msgs/String')")
arg.completer = TopicTypeCompleter(
topic_name_key='topic_name')
arg = parser.add_argument(
group = parser.add_mutually_exclusive_group()
arg = group.add_argument(
'values', nargs='?', default='{}',
help='Values to fill the message with in YAML format '
"(e.g. 'data: Hello World'), "
'otherwise the message will be published with default values')
arg.completer = TopicMessagePrototypeCompleter(
topic_type_key='message_type')
group.add_argument(
'--stdin', action='store_true',
help='Read values from standard input')
parser.add_argument(
'-r', '--rate', metavar='N', type=positive_float, default=1.0,
help='Publishing rate in Hz (default: 1)')
Expand Down Expand Up @@ -115,12 +120,17 @@ def main(args):
if args.once:
times = 1

if args.stdin:
values = collect_stdin()
else:
values = args.values

with DirectNode(args, node_name=args.node_name) as node:
return publisher(
node.node,
args.message_type,
args.topic_name,
args.values,
values,
1. / args.rate,
args.print,
times,
Expand Down