Skip to content

Commit

Permalink
Add helpers.collect_stdin
Browse files Browse the repository at this point in the history
Signed-off-by: ymd-stella <[email protected]>
  • Loading branch information
ymd-stella committed Aug 2, 2023
1 parent cb661f2 commit 0a4196a
Show file tree
Hide file tree
Showing 4 changed files with 27 additions and 29 deletions.
17 changes: 6 additions & 11 deletions ros2action/ros2action/verb/send_goal.py
Original file line number Diff line number Diff line change
Expand Up @@ -12,15 +12,14 @@
# See the License for the specific language governing permissions and
# limitations under the License.

import sys

from action_msgs.msg import GoalStatus
import rclpy
from rclpy.action import ActionClient
from ros2action.api import action_name_completer
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 Down Expand Up @@ -59,15 +58,11 @@ def main(self, *, args):
feedback_callback = _feedback_callback

if args.stdin:
lines = b""
while True:
line = sys.stdin.buffer.readline()
if not line:
break
lines += line
args.goal = lines

return send_goal(args.action_name, args.action_type, args.goal, feedback_callback)
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: 5 additions & 9 deletions ros2service/ros2service/verb/call.py
Original file line number Diff line number Diff line change
Expand Up @@ -13,10 +13,10 @@
# limitations under the License.

import importlib
import sys
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 Down Expand Up @@ -61,16 +61,12 @@ def main(self, *, args):
period = 1. / args.rate if args.rate else None

if args.stdin:
lines = b""
while True:
line = sys.stdin.buffer.readline()
if not line:
break
lines += line
args.values = lines
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: 5 additions & 9 deletions ros2topic/ros2topic/verb/pub.py
Original file line number Diff line number Diff line change
Expand Up @@ -12,14 +12,14 @@
# See the License for the specific language governing permissions and
# limitations under the License.

import sys
import time
from typing import Optional
from typing import TypeVar

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 @@ -121,20 +121,16 @@ def main(args):
times = 1

if args.stdin:
lines = b""
while True:
line = sys.stdin.buffer.readline()
if not line:
break
lines += line
args.values = lines
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

0 comments on commit 0a4196a

Please sign in to comment.