From 5400f704df4cffaa8d024eb4ec4959677a69d71e Mon Sep 17 00:00:00 2001 From: Gary Servin Date: Thu, 30 Oct 2014 15:24:47 -0300 Subject: [PATCH 01/14] Do not use ifaddrs on Android as it is not natively supported --- clients/roscpp/src/libros/transport/transport.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/clients/roscpp/src/libros/transport/transport.cpp b/clients/roscpp/src/libros/transport/transport.cpp index 599bea0ec5..deeb4c7bf7 100644 --- a/clients/roscpp/src/libros/transport/transport.cpp +++ b/clients/roscpp/src/libros/transport/transport.cpp @@ -34,11 +34,14 @@ #include "ros/transport/transport.h" #include "ros/console.h" -#include #include #include #include +#if !defined(__ANDROID__) +#include +#endif + #ifndef NI_MAXHOST #define NI_MAXHOST 1025 #endif @@ -68,6 +71,7 @@ Transport::Transport() gethostname(our_hostname, sizeof(our_hostname)-1); allowed_hosts_.push_back(std::string(our_hostname)); allowed_hosts_.push_back("localhost"); +#if !defined(__ANDROID__) // for ipv4 loopback, we'll explicitly search for 127.* in isHostAllowed() // now we need to iterate all local interfaces and add their addresses // from the getifaddrs manpage: (maybe something similar for windows ?) @@ -96,6 +100,7 @@ Transport::Transport() } allowed_hosts_.push_back(std::string(addr)); } +#endif } bool Transport::isHostAllowed(const std::string &host) const From 880e1fba344938423e51bdf9d99ff8649728e792 Mon Sep 17 00:00:00 2001 From: Gary Servin Date: Mon, 3 Nov 2014 17:39:44 -0300 Subject: [PATCH 02/14] Do not use Python when building for Android --- utilities/roslz4/CMakeLists.txt | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/utilities/roslz4/CMakeLists.txt b/utilities/roslz4/CMakeLists.txt index 1b6bac9e49..6e539bf98d 100644 --- a/utilities/roslz4/CMakeLists.txt +++ b/utilities/roslz4/CMakeLists.txt @@ -25,6 +25,7 @@ include_directories(include ${lz4_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS}) add_library(roslz4 src/lz4s.c src/xxhash.c) target_link_libraries(roslz4 ${lz4_LIBRARIES} ${catkin_LIBRARIES}) +if(NOT ANDROID) # Python bindings set(Python_ADDITIONAL_VERSIONS "${PYTHON_VERSION_MAJOR}.${PYTHON_VERSION_MINOR}") find_package(PythonLibs "${PYTHON_VERSION_MAJOR}.${PYTHON_VERSION_MINOR}" REQUIRED) @@ -35,6 +36,7 @@ set_target_properties( roslz4_py PROPERTIES OUTPUT_NAME roslz4 PREFIX "_" SUFFIX ".so" LIBRARY_OUTPUT_DIRECTORY ${CATKIN_DEVEL_PREFIX}/${PYTHON_INSTALL_DIR}/roslz4) target_link_libraries(roslz4_py roslz4 ${catkin_LIBRARIES} ${PYTHON_LIBRARIES}) +endif() install(TARGETS roslz4 ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} @@ -42,8 +44,10 @@ install(TARGETS roslz4 RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) +if(NOT ANDROID) install(TARGETS roslz4_py LIBRARY DESTINATION ${CATKIN_PACKAGE_PYTHON_DESTINATION}) +endif() install(DIRECTORY include/${PROJECT_NAME} DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION} From 3a35455eb9ea731bc54e41bfd9317eb18bcf5a33 Mon Sep 17 00:00:00 2001 From: Dirk Thomas Date: Tue, 4 Nov 2014 10:17:49 -0800 Subject: [PATCH 03/14] make param functions thread-safe --- clients/rospy/src/rospy/client.py | 32 +++++++++++++++++++------------ 1 file changed, 20 insertions(+), 12 deletions(-) diff --git a/clients/rospy/src/rospy/client.py b/clients/rospy/src/rospy/client.py index d17560b592..3ae0b93506 100644 --- a/clients/rospy/src/rospy/client.py +++ b/clients/rospy/src/rospy/client.py @@ -41,6 +41,7 @@ import socket import struct import sys +from threading import Lock import time import random import yaml @@ -339,6 +340,7 @@ def init_node(name, argv=None, anonymous=False, log_level=None, disable_rostime= #_master_proxy is a MasterProxy wrapper _master_proxy = None +_master_proxy_lock = Lock() def get_master(env=os.environ): """ @@ -351,10 +353,12 @@ def get_master(env=os.environ): @raise Exception: if server cannot be located or system cannot be initialized """ - global _master_proxy - if _master_proxy is not None: - return _master_proxy - _master_proxy = rospy.msproxy.MasterProxy(rosgraph.get_master_uri()) + global _master_proxy, _master_proxy_lock + if _master_proxy is None: + with _master_proxy_lock: + if _master_proxy is None: + _master_proxy = rospy.msproxy.MasterProxy( + rosgraph.get_master_uri()) return _master_proxy ######################################################### @@ -422,13 +426,17 @@ def wait_for_message(topic, topic_type, timeout=None): # Param Server Access _param_server = None +_param_server_lock = Lock() def _init_param_server(): """ Initialize parameter server singleton """ - global _param_server + global _param_server, _param_server_lock if _param_server is None: - _param_server = get_master() #in the future param server will be a service + with _param_server_lock: + if _param_server is None: + _param_server = get_master() + return _param_server_lock # class and singleton to distinguish whether or not user has passed us a default value class _Unspecified(object): pass @@ -438,7 +446,7 @@ def get_param(param_name, default=_unspecified): """ Retrieve a parameter from the param server - NOTE: this method is not thread-safe. + NOTE: this method is thread-safe. @param default: (optional) default value to return if key is not set @type default: any @@ -460,7 +468,7 @@ def get_param_names(): """ Retrieve list of parameter names. - NOTE: this method is not thread-safe. + NOTE: this method is thread-safe. @return: parameter names @rtype: [str] @@ -477,7 +485,7 @@ def set_param(param_name, param_value): """ Set a parameter on the param server - NOTE: this method is not thread-safe. + NOTE: this method is thread-safe. @param param_name: parameter name @type param_name: str @@ -497,7 +505,7 @@ def search_param(param_name): """ Search for a parameter on the param server - NOTE: this method is not thread-safe. + NOTE: this method is thread-safe. @param param_name: parameter name @type param_name: str @@ -512,7 +520,7 @@ def delete_param(param_name): """ Delete a parameter on the param server - NOTE: this method is not thread-safe. + NOTE: this method is thread-safe. @param param_name: parameter name @type param_name: str @@ -526,7 +534,7 @@ def has_param(param_name): """ Test if parameter exists on the param server - NOTE: this method is not thread-safe. + NOTE: this method is thread-safe. @param param_name: parameter name @type param_name: str From 7eb6e85622bdf33baaacab5c7fa5fa461d13332e Mon Sep 17 00:00:00 2001 From: Dirk Thomas Date: Tue, 4 Nov 2014 10:41:35 -0800 Subject: [PATCH 04/14] only create SubscriberStatisticsLogger when enabled --- clients/rospy/src/rospy/impl/statistics.py | 22 +++++++++------------- clients/rospy/src/rospy/topics.py | 7 +++++-- 2 files changed, 14 insertions(+), 15 deletions(-) diff --git a/clients/rospy/src/rospy/impl/statistics.py b/clients/rospy/src/rospy/impl/statistics.py index 32c79882df..f8d33ea4c4 100644 --- a/clients/rospy/src/rospy/impl/statistics.py +++ b/clients/rospy/src/rospy/impl/statistics.py @@ -49,6 +49,15 @@ class SubscriberStatisticsLogger(): this class basically just keeps a collection of ConnectionStatisticsLogger. """ + @classmethod + def is_enabled(cls): + # disable statistics if node can't talk to parameter server + # which is the case in unit tests + try: + return rospy.get_param("/enable_statistics", False) + except Exception: + return False + def __init__(self, subscriber): self.subscriber = subscriber self.connections = dict() @@ -59,13 +68,6 @@ def read_parameters(self): Fetch window parameters from parameter server """ - # disable statistics if node can't talk to parameter server which is the case in unit tests - try: - self.enabled = rospy.get_param("/enable_statistics", False) - except: - self.enabled = False - return - # Range of window length, in seconds self.min_elements = rospy.get_param("/statistics_window_min_elements", 10) self.max_elements = rospy.get_param("/statistics_window_max_elements", 100) @@ -76,9 +78,6 @@ def read_parameters(self): self.max_window = rospy.get_param("/statistics_window_max_size", 64) self.min_window = rospy.get_param("/statistics_window_min_size", 4) - def is_enable_statistics(self): - return self.enabled - def callback(self, msg, publisher, stat_bytes): """ This method is called for every message that has been received. @@ -93,9 +92,6 @@ def callback(self, msg, publisher, stat_bytes): instance. """ - if not self.enabled: - return - # /clock is special, as it is subscribed very early # also exclude /statistics to reduce noise. if self.subscriber.name == "/clock" or self.subscriber.name == "/statistics": diff --git a/clients/rospy/src/rospy/topics.py b/clients/rospy/src/rospy/topics.py index 5a42617337..9e7ec197fa 100644 --- a/clients/rospy/src/rospy/topics.py +++ b/clients/rospy/src/rospy/topics.py @@ -560,7 +560,9 @@ def __init__(self, name, data_class): self.queue_size = None self.buff_size = DEFAULT_BUFF_SIZE self.tcp_nodelay = False - self.statistics_logger = SubscriberStatisticsLogger(self); + self.statistics_logger = SubscriberStatisticsLogger(self) \ + if SubscriberStatisticsLogger.is_enabled() \ + else None def close(self): """close I/O and release resources""" @@ -701,7 +703,8 @@ def receive_callback(self, msgs, connection): # save reference to avoid lock callbacks = self.callbacks for msg in msgs: - self.statistics_logger.callback(msg, connection.callerid_pub, connection.stat_bytes) + if self.statistics_logger: + self.statistics_logger.callback(msg, connection.callerid_pub, connection.stat_bytes) for cb, cb_args in callbacks: self._invoke_callback(msg, cb, cb_args) From 99498b13361c995350a6182b4496f61478697e4a Mon Sep 17 00:00:00 2001 From: Dirk Thomas Date: Tue, 4 Nov 2014 10:52:33 -0800 Subject: [PATCH 05/14] avoid storing subscriber reference in SubscriberStatisticsLogger --- clients/rospy/src/rospy/impl/statistics.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/clients/rospy/src/rospy/impl/statistics.py b/clients/rospy/src/rospy/impl/statistics.py index f8d33ea4c4..64e837e411 100644 --- a/clients/rospy/src/rospy/impl/statistics.py +++ b/clients/rospy/src/rospy/impl/statistics.py @@ -59,7 +59,7 @@ def is_enabled(cls): return False def __init__(self, subscriber): - self.subscriber = subscriber + self.subscriber_name = subscriber.name self.connections = dict() self.read_parameters() @@ -94,14 +94,14 @@ def callback(self, msg, publisher, stat_bytes): # /clock is special, as it is subscribed very early # also exclude /statistics to reduce noise. - if self.subscriber.name == "/clock" or self.subscriber.name == "/statistics": + if self.subscriber_name == "/clock" or self.subscriber_name == "/statistics": return try: # create ConnectionStatisticsLogger for new connections logger = self.connections.get(publisher) if logger is None: - logger = ConnectionStatisticsLogger(self.subscriber.name, rospy.get_name(), publisher) + logger = ConnectionStatisticsLogger(self.subscriber_name, rospy.get_name(), publisher) self.connections[publisher] = logger # delegate stuff to that instance From 3070c3b79d0cab22cd2f32da3f66a6d7e1664974 Mon Sep 17 00:00:00 2001 From: Alexander Krupenkin Date: Tue, 4 Nov 2014 23:10:06 +0300 Subject: [PATCH 06/14] Fix: TCPROS header validation crash when `callerid` header is not set --- clients/rospy/src/rospy/impl/tcpros_base.py | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/clients/rospy/src/rospy/impl/tcpros_base.py b/clients/rospy/src/rospy/impl/tcpros_base.py index 21b376e740..40179c6998 100644 --- a/clients/rospy/src/rospy/impl/tcpros_base.py +++ b/clients/rospy/src/rospy/impl/tcpros_base.py @@ -581,9 +581,10 @@ def _validate_header(self, header): for required in ['md5sum', 'type']: if not required in header: raise TransportInitError("header missing required field [%s]"%required) - self.md5sum = header['md5sum'] - self.callerid_pub = header['callerid'] self.type = header['type'] + self.md5sum = header['md5sum'] + if 'callerid' in header: + self.callerid_pub = header['callerid'] if header.get('latching', '0') == '1': self.is_latched = True From b541ce6c640ed69cb0b217e594b304e0f0fd08dd Mon Sep 17 00:00:00 2001 From: Dirk Thomas Date: Wed, 5 Nov 2014 11:15:08 -0800 Subject: [PATCH 07/14] unregister statistics publisher --- clients/rospy/src/rospy/impl/statistics.py | 8 ++++++++ clients/rospy/src/rospy/topics.py | 3 +++ 2 files changed, 11 insertions(+) diff --git a/clients/rospy/src/rospy/impl/statistics.py b/clients/rospy/src/rospy/impl/statistics.py index 64e837e411..60855b9263 100644 --- a/clients/rospy/src/rospy/impl/statistics.py +++ b/clients/rospy/src/rospy/impl/statistics.py @@ -109,6 +109,11 @@ def callback(self, msg, publisher, stat_bytes): except Exception as e: rospy.logerr("Unexpected error during statistics measurement: %s", str(e)) + def shutdown(self): + for logger in self.connections.values(): + logger.shutdown() + self.connections.clear() + class ConnectionStatisticsLogger(): """ @@ -255,3 +260,6 @@ def callback(self, subscriber_statistics_logger, msg, stat_bytes): if self.last_pub_time + self.pub_frequency < arrival_time: self.last_pub_time = arrival_time self.sendStatistics(subscriber_statistics_logger) + + def shutdown(self): + self.pub.unregister() diff --git a/clients/rospy/src/rospy/topics.py b/clients/rospy/src/rospy/topics.py index 9e7ec197fa..24b91ef2ba 100644 --- a/clients/rospy/src/rospy/topics.py +++ b/clients/rospy/src/rospy/topics.py @@ -570,6 +570,9 @@ def close(self): if self.callbacks: del self.callbacks[:] self.callbacks = None + if self.statistics_logger: + self.statistics_logger.shutdown() + self.statistics_logger = None def set_tcp_nodelay(self, tcp_nodelay): """ From 52e392111bf769b44b2bf696ef7a42a376cd1b84 Mon Sep 17 00:00:00 2001 From: Dirk Thomas Date: Sat, 8 Nov 2014 17:44:45 -0800 Subject: [PATCH 08/14] fix comment (fix #529) --- clients/roscpp/include/ros/param.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/clients/roscpp/include/ros/param.h b/clients/roscpp/include/ros/param.h index c6ea9de906..37beb9b610 100644 --- a/clients/roscpp/include/ros/param.h +++ b/clients/roscpp/include/ros/param.h @@ -598,6 +598,6 @@ void param(const std::string& param_name, T& param_val, const T& default_val) } // namespace param -} // namespace param +} // namespace ros #endif // ROSCPP_PARAM_H From 9e24bcf0e5724a160b2636acfde2cb363773ffa7 Mon Sep 17 00:00:00 2001 From: Dirk Thomas Date: Tue, 11 Nov 2014 11:23:35 -0800 Subject: [PATCH 09/14] fix removal of QueuedConnection leading to wrong subscriber count (fix #526) --- clients/rospy/src/rospy/topics.py | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/clients/rospy/src/rospy/topics.py b/clients/rospy/src/rospy/topics.py index 5a42617337..6f1e13c863 100644 --- a/clients/rospy/src/rospy/topics.py +++ b/clients/rospy/src/rospy/topics.py @@ -351,8 +351,14 @@ def _remove_connection(self, connections, c): c.close() except: pass - if c in connections: - connections.remove(c) + # while c might be a rospy.impl.tcpros_base.TCPROSTransport instance + # connections might only contain the rospy.impl.tcpros_pubsub.QueuedConnection proxy + # therefore finding the "right" connection is more difficult then + # if c in connections + matching_connections = [ + conn for conn in connections if conn.fileno() == c.fileno()] + if matching_connections: + connections.remove(matching_connections[0]) def add_connection(self, c): """ From 270173f7077a4b6bcb77af411c83ce43beb509f5 Mon Sep 17 00:00:00 2001 From: Dirk Thomas Date: Tue, 11 Nov 2014 12:09:50 -0800 Subject: [PATCH 10/14] use fileno comparison only as an alternative and when available --- clients/rospy/src/rospy/topics.py | 15 +++++++++------ 1 file changed, 9 insertions(+), 6 deletions(-) diff --git a/clients/rospy/src/rospy/topics.py b/clients/rospy/src/rospy/topics.py index 6f1e13c863..63d5becdc8 100644 --- a/clients/rospy/src/rospy/topics.py +++ b/clients/rospy/src/rospy/topics.py @@ -353,12 +353,15 @@ def _remove_connection(self, connections, c): pass # while c might be a rospy.impl.tcpros_base.TCPROSTransport instance # connections might only contain the rospy.impl.tcpros_pubsub.QueuedConnection proxy - # therefore finding the "right" connection is more difficult then - # if c in connections - matching_connections = [ - conn for conn in connections if conn.fileno() == c.fileno()] - if matching_connections: - connections.remove(matching_connections[0]) + # finding the "right" connection is more difficult then + if c in connections: + connections.remove(c) + # therefore additionally check for fileno equality if available + elif c.fileno(): + matching_connections = [ + conn for conn in connections if conn.fileno() == c.fileno()] + if len(matching_connections) == 1: + connections.remove(matching_connections[0]) def add_connection(self, c): """ From 29265816d8774f87c12d90dd54dedf5f6815f8c8 Mon Sep 17 00:00:00 2001 From: Alexis Ballier Date: Fri, 24 Oct 2014 15:21:35 +0200 Subject: [PATCH 11/14] Fix exception at roscore startup if python has ipv6 disabled. Admittedly a python bug, but the workaround is already present in other parts of this file. --- tools/rosgraph/src/rosgraph/network.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/tools/rosgraph/src/rosgraph/network.py b/tools/rosgraph/src/rosgraph/network.py index cc8bc2b559..f1878237d3 100644 --- a/tools/rosgraph/src/rosgraph/network.py +++ b/tools/rosgraph/src/rosgraph/network.py @@ -167,7 +167,10 @@ def is_local_address(hostname): :returns True: if hostname maps to a local address, False otherwise. False conditions include invalid hostnames. """ try: - reverse_ips = [host[4][0] for host in socket.getaddrinfo(hostname, 0, 0, 0, socket.SOL_TCP)] + if use_ipv6(): + reverse_ips = [host[4][0] for host in socket.getaddrinfo(socket.gethostname(), 0, 0, 0, socket.SOL_TCP)] + else: + reverse_ips = [host[4][0] for host in socket.getaddrinfo(socket.gethostname(), 0, socket.AF_INET, 0, socket.SOL_TCP)] except socket.error: return False local_addresses = ['localhost'] + get_local_addresses() From f283fa94d0a10d6a207a4fe9eefc5b841fb85022 Mon Sep 17 00:00:00 2001 From: Alexis Ballier Date: Fri, 24 Oct 2014 15:51:38 +0200 Subject: [PATCH 12/14] Fix exception at roslaunch startup if python is build without ipv6 support. A bit different than rosgraph fix because we do not have use_ipv6 here. See e.g. http://bugs.python.org/issue16208#msg178764 --- tools/roslaunch/src/roslaunch/core.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/tools/roslaunch/src/roslaunch/core.py b/tools/roslaunch/src/roslaunch/core.py index d12090af58..6c51e6b8db 100644 --- a/tools/roslaunch/src/roslaunch/core.py +++ b/tools/roslaunch/src/roslaunch/core.py @@ -90,7 +90,8 @@ def is_machine_local(machine): :returns: True if machine is local and doesn't require remote login, ``bool`` """ try: - machine_ips = [host[4][0] for host in socket.getaddrinfo(machine.address, 0, 0, 0, socket.SOL_TCP)] + # If Python has ipv6 disabled but machine.address can be resolved somehow to an ipv6 address, then host[4][0] will be int + machine_ips = [host[4][0] for host in socket.getaddrinfo(machine.address, 0, 0, 0, socket.SOL_TCP) if isinstance(host[4][0], str)] except socket.gaierror: raise RLException("cannot resolve host address for machine [%s]"%machine.address) local_addresses = ['localhost'] + rosgraph.network.get_local_addresses() From 6fac86e39fefaad58db92389098901cdd1a7bf48 Mon Sep 17 00:00:00 2001 From: Tom Moore Date: Thu, 2 Oct 2014 21:15:24 -0400 Subject: [PATCH 13/14] Adding support for fixed-width floating-point and integer array values as per issue #400 --- tools/rostopic/package.xml | 1 + tools/rostopic/src/rostopic/__init__.py | 28 ++++++++++++++++++------- 2 files changed, 21 insertions(+), 8 deletions(-) diff --git a/tools/rostopic/package.xml b/tools/rostopic/package.xml index 63e7609f51..e6584b8794 100644 --- a/tools/rostopic/package.xml +++ b/tools/rostopic/package.xml @@ -23,6 +23,7 @@ rostest + genpy rosbag rospy diff --git a/tools/rostopic/src/rostopic/__init__.py b/tools/rostopic/src/rostopic/__init__.py index 41a930e1a7..d574a13070 100644 --- a/tools/rostopic/src/rostopic/__init__.py +++ b/tools/rostopic/src/rostopic/__init__.py @@ -500,7 +500,7 @@ def _sub_str_plot_fields(val, f, field_filter): return None -def _str_plot(val, time_offset=None, current_time=None, field_filter=None, type_information=None): +def _str_plot(val, time_offset=None, current_time=None, field_filter=None, type_information=None, fixed_numeric_width=None): """ Convert value to matlab/octave-friendly CSV string representation. @@ -592,7 +592,7 @@ class CallbackEcho(object): 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): + 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)`` @@ -600,6 +600,7 @@ def __init__(self, topic, msg_eval, plot=False, filter_fn=None, :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] @@ -607,6 +608,7 @@ def __init__(self, topic, msg_eval, plot=False, filter_fn=None, 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 @@ -639,11 +641,11 @@ def __init__(self, topic, msg_eval, plot=False, filter_fn=None, 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): + 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) + 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): """ @@ -696,12 +698,12 @@ def callback(self, data, callback_args, current_time=None): 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) + \ + 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) + \ + 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 @@ -1060,6 +1062,9 @@ def eval_fn(m): 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", @@ -1113,13 +1118,20 @@ def eval_fn(m): 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) + 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: From bb134395915633484bf84bd3f153e4243d391ef6 Mon Sep 17 00:00:00 2001 From: Dirk Thomas Date: Wed, 10 Dec 2014 12:18:31 -0800 Subject: [PATCH 14/14] fix regression of PR #515 --- tools/rosgraph/src/rosgraph/network.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/tools/rosgraph/src/rosgraph/network.py b/tools/rosgraph/src/rosgraph/network.py index f1878237d3..ff499914af 100644 --- a/tools/rosgraph/src/rosgraph/network.py +++ b/tools/rosgraph/src/rosgraph/network.py @@ -168,9 +168,9 @@ def is_local_address(hostname): """ try: if use_ipv6(): - reverse_ips = [host[4][0] for host in socket.getaddrinfo(socket.gethostname(), 0, 0, 0, socket.SOL_TCP)] + reverse_ips = [host[4][0] for host in socket.getaddrinfo(hostname, 0, 0, 0, socket.SOL_TCP)] else: - reverse_ips = [host[4][0] for host in socket.getaddrinfo(socket.gethostname(), 0, socket.AF_INET, 0, socket.SOL_TCP)] + reverse_ips = [host[4][0] for host in socket.getaddrinfo(hostname, 0, socket.AF_INET, 0, socket.SOL_TCP)] except socket.error: return False local_addresses = ['localhost'] + get_local_addresses()