From 565d03e9a517bc9f2878d757a4e57707fa961ab3 Mon Sep 17 00:00:00 2001 From: Victor Mayoral Vilches Date: Tue, 8 Mar 2016 17:01:23 +0100 Subject: [PATCH 01/10] Add rosnode functionality Peer coded with @ahcorde. For now, only rosnode list functionality has been implemented, the rest of them have been left as a TODO. Code has been inspired by ROS 1 ros_comm package. --- rclpy/rclpy/__init__.py | 2 + rclpy/rclpy/rosnode.py | 126 +++++++++++++++++++++++++++++++++++++++ rclpy/src/rclpy/_rclpy.c | 25 ++++++++ 3 files changed, 153 insertions(+) create mode 100644 rclpy/rclpy/rosnode.py diff --git a/rclpy/rclpy/__init__.py b/rclpy/rclpy/__init__.py index a3486f303..24c07cadd 100644 --- a/rclpy/rclpy/__init__.py +++ b/rclpy/rclpy/__init__.py @@ -20,6 +20,8 @@ def init(args): return _rclpy.rclpy_init(args) +def get_node_names(): + return _rclpy.rclpy_get_node_names() def create_node(node_name): node_handle = _rclpy.rclpy_create_node(node_name) diff --git a/rclpy/rclpy/rosnode.py b/rclpy/rclpy/rosnode.py new file mode 100644 index 000000000..d2d222486 --- /dev/null +++ b/rclpy/rclpy/rosnode.py @@ -0,0 +1,126 @@ +# Copyright 2016 Erle Robotics, LLC +# +# A relevant part of the code has been written taking inpiration +# from ROS 1 ros_comm package attributed to Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import rclpy +import time +import os +import errno +import sys + +from optparse import OptionParser + +NAME='rosnode' + +def _rosnode_cmd_ping(argv): + print("NOT IMPLEMENTED\n") + sys.exit(0) + +def _rosnode_cmd_info(argv): + print("NOT IMPLEMENTED\n") + sys.exit(0) + +def _rosnode_cmd_machine(argv): + print("NOT IMPLEMENTED\n") + sys.exit(0) + +def _rosnode_cmd_cleanup(argv): + print("NOT IMPLEMENTED\n") + sys.exit(0) + +def _rosnode_cmd_kill(argv): + print("NOT IMPLEMENTED\n") + sys.exit(0) + +def _rosnode_cmd_list(argv): + """ + Implements rosnode 'list' command. + """ + args = argv[2:] + parser = OptionParser(usage="usage: %prog list", prog=NAME) + parser.add_option("-u", + dest="list_uri", default=False, + action="store_true", + help="list XML-RPC URIs (NOT IMPLEMENTED)") + parser.add_option("-a","--all", + dest="list_all", default=False, + action="store_true", + help="list all information (NOT IMPLEMENTED)") + (options, args) = parser.parse_args(args) + namespace = None + if len(args) > 1: + parser.error("invalid args: you may only specify one namespace") + elif len(args) == 1: + namespace = rosgraph.names.script_resolve_name('rostopic', args[0]) + + # In ROS 1, the rosnode list invocation was performed using: + # rosnode_listnodes(namespace=namespace, list_uri=options.list_uri, list_all=options.list_all) + + result = rclpy.get_node_names() + for node in result: + print(node) + + +def _fullusage(return_error=True): + """ + Prints rosnode usage information. + @param return_error whether to exit with error code os.EX_USAGE + """ + print("""rosnode is a command-line tool for printing information about ROS Nodes. +Commands: +\trosnode ping\ttest connectivity to node (NOT IMPLEMENTED) +\trosnode list\tlist active nodes +\trosnode info\tprint information about node (NOT IMPLEMENTED) +\trosnode machine\tlist nodes running on a particular machine or list machines +\trosnode kill\tkill a running node (NOT IMPLEMENTED) +\trosnode cleanup\tpurge registration information of unreachable nodes (NOT IMPLEMENTED) +Type rosnode -h for more detailed usage, e.g. 'rosnode ping -h' +""") + if return_error: + sys.exit(getattr(os, 'EX_USAGE', 1)) + else: + sys.exit(0) + +def rosnodemain(argv=None): + """ + Prints rosnode main entrypoint. + @param argv: override sys.argv + @param argv: [str] + """ + if argv == None: + argv = sys.argv + if len(argv) == 1: + _fullusage() + try: + command = argv[1] + if command == 'ping': + sys.exit(_rosnode_cmd_ping(argv) or 0) + elif command == 'list': + sys.exit(_rosnode_cmd_list(argv) or 0) + elif command == 'info': + sys.exit(_rosnode_cmd_info(argv) or 0) + elif command == 'machine': + sys.exit(_rosnode_cmd_machine(argv) or 0) + elif command == 'cleanup': + sys.exit(_rosnode_cmd_cleanup(argv) or 0) + elif command == 'kill': + sys.exit(_rosnode_cmd_kill(argv) or 0) + elif command == '--help': + _fullusage(False) + else: + _fullusage() + except KeyboardInterrupt: + pass diff --git a/rclpy/src/rclpy/_rclpy.c b/rclpy/src/rclpy/_rclpy.c index 5433d19f4..8e894d80b 100644 --- a/rclpy/src/rclpy/_rclpy.c +++ b/rclpy/src/rclpy/_rclpy.c @@ -37,6 +37,29 @@ rclpy_init(PyObject * Py_UNUSED(self), PyObject * Py_UNUSED(args)) Py_RETURN_NONE; } +static PyObject * +rclpy_get_node_names(PyObject * Py_UNUSED(self), PyObject * Py_UNUSED(args)) +{ + rcl_strings_t str = rcl_get_zero_initialized_strings(); + + rcl_ret_t ret = rcl_get_node_names(&str); + if (ret != RCL_RET_OK) { + PyErr_Format(PyExc_RuntimeError, + "Failed to get node names: %s", rcl_get_error_string_safe()); + return NULL; + } + + PyObject* list = PyList_New(str.count); + int i; + for(i = 0; i < str.count; i++) + PyList_SetItem(list, i, Py_BuildValue("s", str.data[i])); + + //free memory + rcl_strings_fini(&str); + + return list; +} + static PyObject * rclpy_create_node(PyObject * Py_UNUSED(self), PyObject * args) { @@ -365,6 +388,8 @@ rclpy_shutdown(PyObject * Py_UNUSED(self), PyObject * Py_UNUSED(args)) static PyMethodDef rclpy_methods[] = { {"rclpy_init", rclpy_init, METH_VARARGS, "Initialize RCL."}, + {"rclpy_get_node_names", rclpy_get_node_names, METH_VARARGS, + "Get ROS node names."}, {"rclpy_create_node", rclpy_create_node, METH_VARARGS, "Create a Node."}, {"rclpy_create_publisher", rclpy_create_publisher, METH_VARARGS, From 12397335ac1eba4d57975c7fde6672cb56d5a0c2 Mon Sep 17 00:00:00 2001 From: Victor Mayoral Vilches Date: Sat, 19 Mar 2016 20:15:26 +0100 Subject: [PATCH 02/10] rosnode: commend invalid code --- rclpy/rclpy/rosnode.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/rclpy/rclpy/rosnode.py b/rclpy/rclpy/rosnode.py index d2d222486..b6d03ab0e 100644 --- a/rclpy/rclpy/rosnode.py +++ b/rclpy/rclpy/rosnode.py @@ -64,7 +64,8 @@ def _rosnode_cmd_list(argv): if len(args) > 1: parser.error("invalid args: you may only specify one namespace") elif len(args) == 1: - namespace = rosgraph.names.script_resolve_name('rostopic', args[0]) + #namespace = rosgraph.names.script_resolve_name('rostopic', args[0]) + pass # In ROS 1, the rosnode list invocation was performed using: # rosnode_listnodes(namespace=namespace, list_uri=options.list_uri, list_all=options.list_all) From d7388a83dc0a4926459197a50e4a1eac729040f0 Mon Sep 17 00:00:00 2001 From: Victor Mayoral Vilches Date: Sat, 19 Mar 2016 20:45:18 +0100 Subject: [PATCH 03/10] init: minor style fix --- rclpy/rclpy/__init__.py | 1 - 1 file changed, 1 deletion(-) diff --git a/rclpy/rclpy/__init__.py b/rclpy/rclpy/__init__.py index 24c07cadd..5a55c1e48 100644 --- a/rclpy/rclpy/__init__.py +++ b/rclpy/rclpy/__init__.py @@ -27,7 +27,6 @@ def create_node(node_name): node_handle = _rclpy.rclpy_create_node(node_name) return Node(node_handle) - def spin(node): wait_set = _rclpy.rclpy_get_zero_initialized_wait_set() From 2d7c11c835654f99e095c8da33ce2c6ffb655002 Mon Sep 17 00:00:00 2001 From: Victor Mayoral Vilches Date: Sat, 19 Mar 2016 20:51:43 +0100 Subject: [PATCH 04/10] Add rostopic skelethon --- rclpy/rclpy/rostopic.py | 101 ++++++++++++++++++++++++++++++++++++++++ 1 file changed, 101 insertions(+) create mode 100644 rclpy/rclpy/rostopic.py diff --git a/rclpy/rclpy/rostopic.py b/rclpy/rclpy/rostopic.py new file mode 100644 index 000000000..39a1f2e64 --- /dev/null +++ b/rclpy/rclpy/rostopic.py @@ -0,0 +1,101 @@ +# Copyright 2016 Erle Robotics, LLC +# +# A relevant part of the code has been written taking inpiration +# from ROS 1 ros_comm package attributed to Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import rclpy +import time +import os +import errno +import sys + +from optparse import OptionParser + +NAME='rostopic' + +def _rostopic_cmd_hz(argv): + pass +def _rostopic_cmd_type(argv): + pass +def _rostopic_cmd_list(argv): + pass +def _rostopic_cmd_info(argv): + pass +def _rostopic_cmd_pub(argv): + pass +def _rostopic_cmd_bw(argv): + pass +def _rostopic_cmd_find(argv): + pass +def _rostopic_cmd_delay(argv): + pass +def _rostopic_cmd_echo(argv): + pass + +def _fullusage(): + print("""rostopic is a command-line tool for printing information about ROS Topics. +Commands: +\trostopic bw (NOT IMPLEMENTED)\tdisplay bandwidth used by topic +\trostopic delay (NOT IMPLEMENTED)\tdisplay delay of topic from timestamp in header +\trostopic echo\tprint messages to screen +\trostopic find (NOT IMPLEMENTED)\tfind topics by type +\trostopic hz (NOT IMPLEMENTED)\tdisplay publishing rate of topic +\trostopic info (NOT IMPLEMENTED)\tprint information about active topic +\trostopic list (NOT IMPLEMENTED)\tlist active topics +\trostopic pub (NOT IMPLEMENTED)\tpublish data to topic +\trostopic type (NOT IMPLEMENTED)\tprint topic type +Type rostopic -h for more detailed usage, e.g. 'rostopic echo -h' +""") + sys.exit(getattr(os, 'EX_USAGE', 1)) + +def rostopicmain(argv=None): + if argv is None: + argv=sys.argv + + # TODO FIXME, review + # filter out remapping arguments in case we are being invoked via roslaunch + #argv = rospy.myargv(argv) + + # process argv + if len(argv) == 1: + _fullusage() + try: + command = argv[1] + if command == 'echo': + _rostopic_cmd_echo(argv) + elif command == 'hz': + _rostopic_cmd_hz(argv) + elif command == 'type': + _rostopic_cmd_type(argv) + elif command == 'list': + _rostopic_cmd_list(argv) + elif command == 'info': + _rostopic_cmd_info(argv) + elif command == 'pub': + _rostopic_cmd_pub(argv) + elif command == 'bw': + _rostopic_cmd_bw(argv) + elif command == 'find': + _rostopic_cmd_find(argv) + elif command == 'delay': + _rostopic_cmd_delay(argv) + else: + _fullusage() + except socket.error: + sys.stderr.write("Network communication failed.\n") + sys.exit(1) + except Exception as e: + sys.stderr.write("ERROR: %s\n"%str(e)) + sys.exit(1) \ No newline at end of file From 727c7598b071d21265d130919b6b07b163538379 Mon Sep 17 00:00:00 2001 From: Victor Mayoral Vilches Date: Sat, 19 Mar 2016 20:57:17 +0100 Subject: [PATCH 05/10] style fixes --- rclpy/rclpy/rosnode.py | 5 ++ rclpy/rclpy/rostopic.py | 146 +++++++++++++++++++++++++++++++++++----- 2 files changed, 134 insertions(+), 17 deletions(-) diff --git a/rclpy/rclpy/rosnode.py b/rclpy/rclpy/rosnode.py index b6d03ab0e..26651abf8 100644 --- a/rclpy/rclpy/rosnode.py +++ b/rclpy/rclpy/rosnode.py @@ -25,22 +25,27 @@ NAME='rosnode' +# TODO implement def _rosnode_cmd_ping(argv): print("NOT IMPLEMENTED\n") sys.exit(0) +# TODO implement def _rosnode_cmd_info(argv): print("NOT IMPLEMENTED\n") sys.exit(0) +# TODO implement def _rosnode_cmd_machine(argv): print("NOT IMPLEMENTED\n") sys.exit(0) +# TODO implement def _rosnode_cmd_cleanup(argv): print("NOT IMPLEMENTED\n") sys.exit(0) +# TODO implement def _rosnode_cmd_kill(argv): print("NOT IMPLEMENTED\n") sys.exit(0) diff --git a/rclpy/rclpy/rostopic.py b/rclpy/rclpy/rostopic.py index 39a1f2e64..be5cbe88a 100644 --- a/rclpy/rclpy/rostopic.py +++ b/rclpy/rclpy/rostopic.py @@ -25,37 +25,149 @@ NAME='rostopic' +# TODO implement def _rostopic_cmd_hz(argv): - pass + print("NOT IMPLEMENTED\n") + sys.exit(0) + +# TODO implement def _rostopic_cmd_type(argv): - pass + print("NOT IMPLEMENTED\n") + sys.exit(0) + +# TODO implement def _rostopic_cmd_list(argv): - pass + print("NOT IMPLEMENTED\n") + sys.exit(0) + +# TODO implement def _rostopic_cmd_info(argv): - pass + print("NOT IMPLEMENTED\n") + sys.exit(0) + +# TODO implement def _rostopic_cmd_pub(argv): - pass + print("NOT IMPLEMENTED\n") + sys.exit(0) + +# TODO implement def _rostopic_cmd_bw(argv): - pass + print("NOT IMPLEMENTED\n") + sys.exit(0) + +# TODO implement def _rostopic_cmd_find(argv): - pass + print("NOT IMPLEMENTED\n") + sys.exit(0) + +# TODO implement def _rostopic_cmd_delay(argv): - pass + print("NOT IMPLEMENTED\n") + sys.exit(0) + def _rostopic_cmd_echo(argv): - pass + def expr_eval(expr): + def eval_fn(m): + return eval(expr) + return eval_fn + + args = argv[2:] + from optparse import OptionParser + parser = OptionParser(usage="usage: %prog echo [options] /topic", prog=NAME) + parser.add_option("-b", "--bag", + dest="bag", default=None, + help="echo messages from .bag file", metavar="BAGFILE") + parser.add_option("-p", + dest="plot", default=False, + action="store_true", + help="echo in a plotting friendly format") + parser.add_option("-w", + dest="fixed_numeric_width", default=None, metavar="NUM_WIDTH", + help="fixed width for numeric values") + parser.add_option("--filter", + dest="filter_expr", default=None, + metavar="FILTER-EXPRESSION", + help="Python expression to filter messages that are printed. Expression can use Python builtins as well as m (the message) and topic (the topic name).") + parser.add_option("--nostr", + dest="nostr", default=False, + action="store_true", + help="exclude string fields") + parser.add_option("--noarr", + dest="noarr", default=False, + action="store_true", + help="exclude arrays") + parser.add_option("-c", "--clear", + dest="clear", default=False, + action="store_true", + help="clear screen before printing next message") + parser.add_option("-a", "--all", + dest="all_topics", default=False, + action="store_true", + help="display all message in bag, only valid with -b option") + parser.add_option("-n", + dest="msg_count", default=None, metavar="COUNT", + help="number of messages to echo") + parser.add_option("--offset", + dest="offset_time", default=False, + action="store_true", + help="display time as offsets from current time (in seconds)") + + (options, args) = parser.parse_args(args) + if len(args) > 1: + parser.error("you may only specify one input topic") + if options.all_topics and not options.bag: + parser.error("Display all option is only valid when echoing from bag files") + if options.offset_time and options.bag: + parser.error("offset time option is not valid with bag files") + if options.all_topics: + topic = '' + else: + if len(args) == 0: + parser.error("topic must be specified") + topic = rosgraph.names.script_resolve_name('rostopic', args[0]) + # suppressing output to keep it clean + #if not options.plot: + # print "rostopic: topic is [%s]"%topic + + filter_fn = None + if options.filter_expr: + filter_fn = expr_eval(options.filter_expr) + + try: + msg_count = int(options.msg_count) if options.msg_count else None + except ValueError: + parser.error("COUNT must be an integer") + + try: + fixed_numeric_width = int(options.fixed_numeric_width) if options.fixed_numeric_width else None + if fixed_numeric_width is not None and fixed_numeric_width < 2: + parser.error("Fixed width for numeric values must be at least 2") + except ValueError: + parser.error("NUM_WIDTH must be an integer") + + field_filter_fn = create_field_filter(options.nostr, options.noarr) + callback_echo = CallbackEcho(topic, None, plot=options.plot, + filter_fn=filter_fn, + echo_clear=options.clear, echo_all_topics=options.all_topics, + offset_time=options.offset_time, count=msg_count, + field_filter_fn=field_filter_fn, fixed_numeric_width=fixed_numeric_width) + try: + _rostopic_echo(topic, callback_echo, bag_file=options.bag) + except socket.error: + sys.stderr.write("Network communication failed. Most likely failed to communicate with master.\n") def _fullusage(): print("""rostopic is a command-line tool for printing information about ROS Topics. Commands: -\trostopic bw (NOT IMPLEMENTED)\tdisplay bandwidth used by topic -\trostopic delay (NOT IMPLEMENTED)\tdisplay delay of topic from timestamp in header +\trostopic bw\tdisplay bandwidth used by topic +\trostopic delay\tdisplay delay of topic from timestamp in header \trostopic echo\tprint messages to screen -\trostopic find (NOT IMPLEMENTED)\tfind topics by type -\trostopic hz (NOT IMPLEMENTED)\tdisplay publishing rate of topic -\trostopic info (NOT IMPLEMENTED)\tprint information about active topic -\trostopic list (NOT IMPLEMENTED)\tlist active topics -\trostopic pub (NOT IMPLEMENTED)\tpublish data to topic -\trostopic type (NOT IMPLEMENTED)\tprint topic type +\trostopic find\tfind topics by type +\trostopic hz\tdisplay publishing rate of topic +\trostopic info\tprint information about active topic +\trostopic list\tlist active topics +\trostopic pub\tpublish data to topic +\trostopic type\tprint topic type Type rostopic -h for more detailed usage, e.g. 'rostopic echo -h' """) sys.exit(getattr(os, 'EX_USAGE', 1)) From 8033bf6dceefcca7237a1e4c91c466ecc503c7eb Mon Sep 17 00:00:00 2001 From: Victor Mayoral Vilches Date: Sat, 19 Mar 2016 20:59:27 +0100 Subject: [PATCH 06/10] rostopic: add socket import --- rclpy/rclpy/rostopic.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rclpy/rclpy/rostopic.py b/rclpy/rclpy/rostopic.py index be5cbe88a..0c297cbdc 100644 --- a/rclpy/rclpy/rostopic.py +++ b/rclpy/rclpy/rostopic.py @@ -20,7 +20,7 @@ import os import errno import sys - +import socket from optparse import OptionParser NAME='rostopic' From 37418eb141bc9dacf70e14c1e91cc7c9ef410661 Mon Sep 17 00:00:00 2001 From: Victor Mayoral Vilches Date: Sun, 20 Mar 2016 00:14:52 +0100 Subject: [PATCH 07/10] rostopic echo: minimal test implementation string has been hardcoded as the type that is echoed. Further work will enable other ROS msg types to be printed. --- rclpy/rclpy/rostopic.py | 241 ++++++++++++++++++++++++++++++++++++++-- 1 file changed, 234 insertions(+), 7 deletions(-) diff --git a/rclpy/rclpy/rostopic.py b/rclpy/rclpy/rostopic.py index 0c297cbdc..4a381d4d1 100644 --- a/rclpy/rclpy/rostopic.py +++ b/rclpy/rclpy/rostopic.py @@ -22,6 +22,9 @@ import sys import socket from optparse import OptionParser +from rclpy.qos import qos_profile_default +from std_msgs.msg import String + NAME='rostopic' @@ -124,11 +127,10 @@ def eval_fn(m): else: if len(args) == 0: parser.error("topic must be specified") - topic = rosgraph.names.script_resolve_name('rostopic', args[0]) - # suppressing output to keep it clean - #if not options.plot: - # print "rostopic: topic is [%s]"%topic - + + topic = args[0] + print("rostopic: topic is [%s]"%topic) + filter_fn = None if options.filter_expr: filter_fn = expr_eval(options.filter_expr) @@ -152,9 +154,234 @@ def eval_fn(m): offset_time=options.offset_time, count=msg_count, field_filter_fn=field_filter_fn, fixed_numeric_width=fixed_numeric_width) try: - _rostopic_echo(topic, callback_echo, bag_file=options.bag) + #_rostopic_echo(topic, callback_echo, bag_file=options.bag) + _rostopic_echo_test(topic) except socket.error: - sys.stderr.write("Network communication failed. Most likely failed to communicate with master.\n") + sys.stderr.write("Network communication failed.\n") + +def _rostopic_echo_test(topic): + rclpy.init(None) + node = rclpy.create_node('rostopic') + # TODO FIXME no path resolving for topics yet implemented thereby the initial "/" is + # omited. + sub = node.create_subscription(String, topic[1:], chatter_callback, qos_profile_default) + rclpy.spin(node) + +def chatter_callback(msg): + print('%s' % msg.data) + print("-------") + +class CallbackEcho(object): + """ + Callback instance that can print callback data in a variety of + formats. Used for all variants of rostopic echo + """ + + def __init__(self, topic, msg_eval, plot=False, filter_fn=None, + echo_clear=False, echo_all_topics=False, + offset_time=False, count=None, + field_filter_fn=None, fixed_numeric_width=None): + """ + :param plot: if ``True``, echo in plotting-friendly format, ``bool`` + :param filter_fn: function that evaluates to ``True`` if message is to be echo'd, ``fn(topic, msg)`` + :param echo_all_topics: (optional) if ``True``, echo all messages in bag, ``bool`` + :param offset_time: (optional) if ``True``, display time as offset from current time, ``bool`` + :param count: number of messages to echo, ``None`` for infinite, ``int`` + :param field_filter_fn: filter the fields that are strified for Messages, ``fn(Message)->iter(str)`` + :param fixed_numeric_width: fixed width for numeric values, ``None`` for automatic, ``int`` + """ + if topic and topic[-1] == '/': + topic = topic[:-1] + self.topic = topic + self.msg_eval = msg_eval + self.plot = plot + self.filter_fn = filter_fn + self.fixed_numeric_width = fixed_numeric_width + + self.prefix = '' + self.suffix = '\n---' if not plot else ''# same as YAML document separator, bug #3291 + + self.echo_all_topics = echo_all_topics + self.offset_time = offset_time + + # done tracks when we've exceeded the count + self.done = False + self.max_count = count + self.count = 0 + + # determine which strifying function to use + if plot: + #TODOXXX: need to pass in filter function + self.str_fn = _str_plot + self.sep = '' + else: + #TODOXXX: need to pass in filter function + self.str_fn = self.custom_strify_message + if echo_clear: + self.prefix = '\033[2J\033[;H' + + self.field_filter=field_filter_fn + + # first tracks whether or not we've printed anything yet. Need this for printing plot fields. + self.first = True + + # cache + self.last_topic = None + self.last_msg_eval = None + + def custom_strify_message(self, val, indent='', time_offset=None, current_time=None, field_filter=None, type_information=None, fixed_numeric_width=None): + # ensure to print uint8[] as array of numbers instead of string + if type_information and type_information.startswith('uint8['): + val = [ord(x) for x in val] + return genpy.message.strify_message(val, indent=indent, time_offset=time_offset, current_time=current_time, field_filter=field_filter, fixed_numeric_width=fixed_numeric_width) + + def callback(self, data, callback_args, current_time=None): + """ + Callback to pass to rospy.Subscriber or to call + manually. rospy.Subscriber constructor must also pass in the + topic name as an additional arg + :param data: Message + :param topic: topic name, ``str`` + :param current_time: override calculation of current time, :class:`genpy.Time` + """ + topic = callback_args['topic'] + type_information = callback_args.get('type_information', None) + if self.filter_fn is not None and not self.filter_fn(data): + return + + if self.max_count is not None and self.count >= self.max_count: + self.done = True + return + + try: + msg_eval = self.msg_eval + if topic == self.topic: + pass + elif self.topic.startswith(topic + '/'): + # self.topic is actually a reference to topic field, generate msgeval + if topic == self.last_topic: + # use cached eval + msg_eval = self.last_msg_eval + else: + # generate msg_eval and cache + self.last_msg_eval = msg_eval = msgevalgen(self.topic[len(topic):]) + self.last_topic = topic + elif not self.echo_all_topics: + return + + if msg_eval is not None: + data = msg_eval(data) + + # data can be None if msg_eval returns None + if data is not None: + # NOTE: we do all prints using direct writes to sys.stdout, which works better with piping + + self.count += 1 + + # print fields header for plot + if self.plot and self.first: + sys.stdout.write("%"+_str_plot_fields(data, 'field', self.field_filter)+'\n') + self.first = False + + if self.offset_time: + sys.stdout.write(self.prefix+\ + self.str_fn(data, time_offset=rospy.get_rostime(), + current_time=current_time, field_filter=self.field_filter, type_information=type_information, fixed_numeric_width=self.fixed_numeric_width) + \ + self.suffix + '\n') + else: + sys.stdout.write(self.prefix+\ + self.str_fn(data, + current_time=current_time, field_filter=self.field_filter, type_information=type_information, fixed_numeric_width=self.fixed_numeric_width) + \ + self.suffix + '\n') + + # we have to flush in order before piping to work + sys.stdout.flush() + # #2778 : have to check count after incr to set done flag + if self.max_count is not None and self.count >= self.max_count: + self.done = True + + except IOError: + self.done = True + except: + # set done flag so we exit + self.done = True + traceback.print_exc() + +def create_field_filter(echo_nostr, echo_noarr): + def field_filter(val): + fields = val.__slots__ + field_types = val._slot_types + for f, t in zip(val.__slots__, val._slot_types): + if echo_noarr and '[' in t: + continue + elif echo_nostr and 'string' in t: + continue + yield f + return field_filter + + +def _rostopic_echo(topic, callback_echo, bag_file=None, echo_all_topics=False): + """ + Print new messages on topic to screen. + + :param topic: topic name, ``str`` + :param bag_file: name of bag file to echo messages from or ``None``, ``str`` + """ + # we have to init a node regardless and bag echoing can print timestamps + + if bag_file: + # TODO review + # initialize rospy time due to potential timestamp printing + #rospy.rostime.set_rostime_initialized(True) + #_rostopic_echo_bag(callback_echo, bag_file) + pass + else: + rclpy.init(None) + # Get topic information + msg_class, real_topic, msg_eval = get_topic_class(topic, blocking=True) + if msg_class is None: + # occurs on ctrl-C + return + callback_echo.msg_eval = msg_eval + + # extract type information for submessages + type_information = None + if len(topic) > len(real_topic): + subtopic = topic[len(real_topic):] + subtopic = subtopic.strip('/') + if subtopic: + fields = subtopic.split('/') + submsg_class = msg_class + while fields: + field = fields[0].split('[')[0] + del fields[0] + index = submsg_class.__slots__.index(field) + type_information = submsg_class._slot_types[index] + if fields: + submsg_class = roslib.message.get_message_class(type_information.split('[', 1)[0]) + if not submsg_class: + raise ROSTopicException("Cannot load message class for [%s]. Are your messages built?" % type_information) + + use_sim_time = rospy.get_param('/use_sim_time', False) + sub = rospy.Subscriber(real_topic, msg_class, callback_echo.callback, {'topic': topic, 'type_information': type_information}) + + if use_sim_time: + # #2950: print warning if nothing received for two seconds + + timeout_t = time.time() + 2. + while time.time() < timeout_t and \ + callback_echo.count == 0 and \ + not rospy.is_shutdown() and \ + not callback_echo.done: + _sleep(0.1) + + if callback_echo.count == 0 and \ + not rospy.is_shutdown() and \ + not callback_echo.done: + sys.stderr.write("WARNING: no messages received and simulated time is active.\nIs /clock being published?\n") + + while not rospy.is_shutdown() and not callback_echo.done: + _sleep(0.1) def _fullusage(): print("""rostopic is a command-line tool for printing information about ROS Topics. From 64e07efa25f85ee879b319b3ed8f21c777f3b7f7 Mon Sep 17 00:00:00 2001 From: Victor Mayoral Vilches Date: Sun, 20 Mar 2016 00:32:33 +0100 Subject: [PATCH 08/10] rostopic echo: add additional comments The purpose of pushing this changes is to have an adhoc "rostopic echo" functionality that helps our purposes in the development of H-ROS. A proper implementation should follow the strategy introduced in the implementation of ROS 1 for "rostopic echo". Relevant parts of this previous implementation have been included. --- rclpy/rclpy/rostopic.py | 1 + 1 file changed, 1 insertion(+) diff --git a/rclpy/rclpy/rostopic.py b/rclpy/rclpy/rostopic.py index 4a381d4d1..a3435ba85 100644 --- a/rclpy/rclpy/rostopic.py +++ b/rclpy/rclpy/rostopic.py @@ -154,6 +154,7 @@ def eval_fn(m): offset_time=options.offset_time, count=msg_count, field_filter_fn=field_filter_fn, fixed_numeric_width=fixed_numeric_width) try: + # TODO FIXME, hardcoded to a function that solely supports simple string messages #_rostopic_echo(topic, callback_echo, bag_file=options.bag) _rostopic_echo_test(topic) except socket.error: From faff377fa6f6c2bf6df9a26def85f477265ce500 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Sun, 20 Mar 2016 17:08:33 +0100 Subject: [PATCH 09/10] Add basic structure for get remote topic names and types --- rclpy/rclpy/__init__.py | 3 +++ rclpy/src/rclpy/_rclpy.c | 10 +++++++++- 2 files changed, 12 insertions(+), 1 deletion(-) diff --git a/rclpy/rclpy/__init__.py b/rclpy/rclpy/__init__.py index 5a55c1e48..617a90be2 100644 --- a/rclpy/rclpy/__init__.py +++ b/rclpy/rclpy/__init__.py @@ -23,6 +23,9 @@ def init(args): def get_node_names(): return _rclpy.rclpy_get_node_names() +def get_remote_topic_names_and_types(): + return _rclpy.rclpy_get_remote_topic_names_and_types() + def create_node(node_name): node_handle = _rclpy.rclpy_create_node(node_name) return Node(node_handle) diff --git a/rclpy/src/rclpy/_rclpy.c b/rclpy/src/rclpy/_rclpy.c index 8e894d80b..039dace05 100644 --- a/rclpy/src/rclpy/_rclpy.c +++ b/rclpy/src/rclpy/_rclpy.c @@ -60,6 +60,12 @@ rclpy_get_node_names(PyObject * Py_UNUSED(self), PyObject * Py_UNUSED(args)) return list; } +static PyObject * +rclpy_get_remote_topic_names_and_types(PyObject * Py_UNUSED(self), PyObject * Py_UNUSED(args)) +{ + return NULL; +} + static PyObject * rclpy_create_node(PyObject * Py_UNUSED(self), PyObject * args) { @@ -389,7 +395,9 @@ static PyMethodDef rclpy_methods[] = { {"rclpy_init", rclpy_init, METH_VARARGS, "Initialize RCL."}, {"rclpy_get_node_names", rclpy_get_node_names, METH_VARARGS, - "Get ROS node names."}, + "Get ROS node names."}, + {"rclpy_get_remote_topic_names_and_types", rclpy_get_remote_topic_names_and_types, METH_VARARGS, + "Get ROS topic names and types."}, {"rclpy_create_node", rclpy_create_node, METH_VARARGS, "Create a Node."}, {"rclpy_create_publisher", rclpy_create_publisher, METH_VARARGS, From 8714181ef11a71ab1a34693c88cf5116786a0bc1 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Mon, 21 Mar 2016 10:36:35 +0100 Subject: [PATCH 10/10] complete rclpy_get_remote_topic_names_and_types implementation --- rclpy/rclpy/rostopic.py | 4 +++- rclpy/src/rclpy/_rclpy.c | 18 +++++++++++++++++- 2 files changed, 20 insertions(+), 2 deletions(-) diff --git a/rclpy/rclpy/rostopic.py b/rclpy/rclpy/rostopic.py index a3435ba85..50984d957 100644 --- a/rclpy/rclpy/rostopic.py +++ b/rclpy/rclpy/rostopic.py @@ -40,7 +40,9 @@ def _rostopic_cmd_type(argv): # TODO implement def _rostopic_cmd_list(argv): - print("NOT IMPLEMENTED\n") + result = rclpy.get_remote_topic_names_and_types() + for node in result: + print(node) sys.exit(0) # TODO implement diff --git a/rclpy/src/rclpy/_rclpy.c b/rclpy/src/rclpy/_rclpy.c index 039dace05..e546febcf 100644 --- a/rclpy/src/rclpy/_rclpy.c +++ b/rclpy/src/rclpy/_rclpy.c @@ -63,7 +63,23 @@ rclpy_get_node_names(PyObject * Py_UNUSED(self), PyObject * Py_UNUSED(args)) static PyObject * rclpy_get_remote_topic_names_and_types(PyObject * Py_UNUSED(self), PyObject * Py_UNUSED(args)) { - return NULL; + rcl_strings_t topic_names_string = rcl_get_zero_initialized_strings(); + rcl_strings_t type_names_string = rcl_get_zero_initialized_strings(); + rcl_ret_t topic_ret = rcl_get_remote_topic_names_and_types(&topic_names_string, &type_names_string); + + if(topic_ret != RCL_RET_OK){ + return NULL; + } + + PyObject* list = PyList_New(topic_names_string.count); + int i; + for(i = 0; i < topic_names_string.count; i++) + PyList_SetItem(list, i, Py_BuildValue("s", topic_names_string.data[i])); + + rcl_strings_fini(&topic_names_string); + rcl_strings_fini(&type_names_string); + + return list; } static PyObject *