def main(args=None): parsed_args = parse_args() rclpy.init(args=args) topic = 'qos_deadline_chatter' deadline = Duration(seconds=parsed_args.deadline / 1000.0) qos_profile = QoSProfile(depth=10, deadline=deadline) subscription_callbacks = SubscriptionEventCallbacks( deadline=lambda event: get_logger('Listener').info(str(event))) listener = Listener(topic, qos_profile, event_callbacks=subscription_callbacks) publisher_callbacks = PublisherEventCallbacks( deadline=lambda event: get_logger('Talker').info(str(event))) talker = Talker(topic, qos_profile, event_callbacks=publisher_callbacks) publish_for_seconds = parsed_args.publish_for / 1000.0 pause_for_seconds = parsed_args.pause_for / 1000.0 pause_timer = talker.create_timer( # noqa: F841 publish_for_seconds, lambda: talker.pause_for(pause_for_seconds)) executor = SingleThreadedExecutor() executor.add_node(listener) executor.add_node(talker) try: executor.spin() except KeyboardInterrupt: pass except ExternalShutdownException: sys.exit(1) finally: rclpy.try_shutdown()
def __init__(self): super().__init__('base_info_handler') self._world_link = "/map" self._base_link = "/base_footprint" self._last_cmd = TRVCommand() try: self.declare_parameter(self.URDF_PARAM) except AttributeError: pass # ROS 2 prior to dashing does not have this method, so ignore AttributeErrors urdf_filename = self.get_parameter(self.URDF_PARAM).value get_logger(self.get_name()).info("Robot URDF: %s" % urdf_filename) self._odom = Odometry( header=Header(frame_id=self._world_link), child_frame_id=self._base_link, pose=PoseWithCovariance( pose=Pose(position=Point(x=0.0, y=0.0, z=0.0), orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0)))) self._transform = TransformStamped( header=Header(frame_id=self._world_link), child_frame_id=self._base_link) self.sub_robot_pose = self.create_subscription( BaseInfo, "/base_info", self.base_info_cb, qos_profile=qos_profile_sensor_data) # these bridge from standard geometry_msgs/Twist to drive_base_msgs/TRVCommand self.pub_cmd = self.create_publisher(TRVCommand, "/drive_cmd", qos_profile=QoSProfile(depth=1)) self.sub_joy = self.create_subscription( Joy, "/joy", self.joy_cb, qos_profile=QoSProfile(depth=1)) # standard odom topic. every message replaces the last, so we only need one self.pub_odom = self.create_publisher(Odometry, "/odom", qos_profile=QoSProfile(depth=1)) # tf topic. messages build a history, so give subscribers a chance to get them all # CHECKME: use transient local? self.pub_tf = self.create_publisher(TFMessage, "/tf", qos_profile=QoSProfile(depth=10)) # publish URDF "latched" -- doesn't seem to work, though if urdf_filename is not None: self.pub_urdf = self.create_publisher( String, "/robot_description", qos_profile=QoSProfile( depth=1, durability=QoSDurabilityPolicy. RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL)) self._urdf_msg = String(data=urdf_filename) self.pub_urdf.publish(self._urdf_msg) self.timer_desc = self.create_timer(1, self.description_timer_cb)
def main(args=None): parsed_args = parse_args() rclpy.init(args=args) topic = 'qos_liveliness_chatter' liveliness_lease_duration = Duration( seconds=parsed_args.liveliness_lease_duration / 1000.0) liveliness_policy = POLICY_MAP[parsed_args.policy] qos_profile = QoSProfile( depth=10, liveliness=liveliness_policy, liveliness_lease_duration=liveliness_lease_duration) subscription_callbacks = SubscriptionEventCallbacks( liveliness=lambda event: get_logger('Listener').info(str(event))) listener = Listener(topic, qos_profile, event_callbacks=subscription_callbacks) publisher_callbacks = PublisherEventCallbacks( liveliness=lambda event: get_logger('Talker').info(str(event))) talker = Talker(topic, qos_profile, event_callbacks=publisher_callbacks, assert_node_period=parsed_args.node_assert_period / 1000.0, assert_topic_period=parsed_args.topic_assert_period / 1000.0) executor = SingleThreadedExecutor() def kill_talker(): if liveliness_policy == QoSLivelinessPolicy.RMW_QOS_POLICY_LIVELINESS_AUTOMATIC: executor.remove_node(talker) talker.destroy_node() elif liveliness_policy == QoSLivelinessPolicy.RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_NODE: talker.stop() elif liveliness_policy == QoSLivelinessPolicy.RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC: talker.stop() kill_timer.cancel() if parsed_args.kill_publisher_after > 0: kill_timer = listener.create_timer( # noqa: F841 parsed_args.kill_publisher_after / 1000.0, kill_talker) executor.add_node(listener) executor.add_node(talker) executor.spin() rclpy.shutdown()
def __init__(self, mode=message_helpers.MSG_MODE, pkg_name='rqt_msg', ui_filename='messages.ui'): """ :param ui_filename: This Qt-based .ui file must have elements that are referred from this class. Otherwise unexpected errors are likely to happen. Best way to avoid that situation when you want to give your own .ui file is to implement all Qt components in rqt_msg/resource/message.ui file. """ super(MessagesWidget, self).__init__() self._logger = logging.get_logger("MessagesWidget") _, package_path = get_resource('packages', pkg_name) ui_file = os.path.join(package_path, 'share', pkg_name, 'resource', ui_filename) loadUi(ui_file, self, {'MessagesTreeView': MessagesTreeView}) self.setObjectName(ui_filename) self._mode = mode self._add_button.setIcon(QIcon.fromTheme('list-add')) self._add_button.clicked.connect(self._add_message) self._refresh_packages(mode) self._refresh_msgs(self._package_combo.itemText(0)) self._package_combo.currentIndexChanged[str].connect( self._refresh_msgs) self._messages_tree.mousePressEvent = self._handle_mouse_press self._browsers = []
def __init__(self, node_name, *, cli_args=None, namespace=None, use_global_arguments=True): self._handle = None self.publishers = [] self.subscriptions = [] self.clients = [] self.services = [] self.timers = [] self.guards = [] self._default_callback_group = MutuallyExclusiveCallbackGroup() namespace = namespace or '' if not ok(): raise NotInitializedException('cannot create node') try: self._handle = _rclpy.rclpy_create_node(node_name, namespace, cli_args, use_global_arguments) except ValueError: # these will raise more specific errors if the name or namespace is bad validate_node_name(node_name) # emulate what rcl_node_init() does to accept '' and relative namespaces if not namespace: namespace = '/' if not namespace.startswith('/'): namespace = '/' + namespace validate_namespace(namespace) # Should not get to this point raise RuntimeError('rclpy_create_node failed for unknown reason') self._logger = get_logger( _rclpy.rclpy_get_node_logger_name(self.handle))
def create_event_handlers( self, callback_group: CallbackGroup, publisher_handle: Handle, topic_name: str, ) -> List[QoSEventHandler]: with publisher_handle as publisher_capsule: logger = get_logger( _rclpy.rclpy_get_publisher_logger_name(publisher_capsule)) event_handlers = [] if self.deadline: event_handlers.append( QoSEventHandler(callback_group=callback_group, callback=self.deadline, event_type=QoSPublisherEventType. RCL_PUBLISHER_OFFERED_DEADLINE_MISSED, parent_handle=publisher_handle)) if self.liveliness: event_handlers.append( QoSEventHandler(callback_group=callback_group, callback=self.liveliness, event_type=QoSPublisherEventType. RCL_PUBLISHER_LIVELINESS_LOST, parent_handle=publisher_handle)) if self.incompatible_qos: event_handlers.append( QoSEventHandler(callback_group=callback_group, callback=self.incompatible_qos, event_type=QoSPublisherEventType. RCL_PUBLISHER_OFFERED_INCOMPATIBLE_QOS, parent_handle=publisher_handle)) elif self.use_default_callbacks: # Register default callback when not specified try: def _default_incompatible_qos_callback(event): policy_name = qos_policy_name_from_kind( event.last_policy_kind) logger.warn( "New subscription discovered on topic '{}', requesting incompatible QoS. " 'No messages will be sent to it. ' 'Last incompatible policy: {}'.format( topic_name, policy_name)) event_handlers.append( QoSEventHandler( callback_group=callback_group, callback=_default_incompatible_qos_callback, event_type=QoSPublisherEventType. RCL_PUBLISHER_OFFERED_INCOMPATIBLE_QOS, parent_handle=publisher_handle)) except UnsupportedEventTypeError: pass return event_handlers
def create_event_handlers( self, callback_group: CallbackGroup, subscription_handle: Handle, ) -> List[QoSEventHandler]: with subscription_handle as subscription_capsule: logger = get_logger( _rclpy.rclpy_get_subscription_logger_name( subscription_capsule)) event_handlers = [] if self.deadline: event_handlers.append( QoSEventHandler(callback_group=callback_group, callback=self.deadline, event_type=QoSSubscriptionEventType. RCL_SUBSCRIPTION_REQUESTED_DEADLINE_MISSED, parent_handle=subscription_handle)) if self.liveliness: event_handlers.append( QoSEventHandler(callback_group=callback_group, callback=self.liveliness, event_type=QoSSubscriptionEventType. RCL_SUBSCRIPTION_LIVELINESS_CHANGED, parent_handle=subscription_handle)) if self.incompatible_qos: event_handlers.append( QoSEventHandler(callback_group=callback_group, callback=self.incompatible_qos, event_type=QoSSubscriptionEventType. RCL_SUBSCRIPTION_REQUESTED_INCOMPATIBLE_QOS, parent_handle=subscription_handle)) elif self.use_default_callbacks: # Register default callback when not specified try: def _default_incompatible_qos_callback(event): policy_name = qos_policy_name_from_kind( event.last_policy_kind) logger.warn( 'New publisher discovered on this topic, offering incompatible QoS. ' 'No messages will be received from it. ' 'Last incompatible policy: {}'.format(policy_name)) event_type = QoSSubscriptionEventType.RCL_SUBSCRIPTION_REQUESTED_INCOMPATIBLE_QOS event_handlers.append( QoSEventHandler( callback_group=callback_group, callback=_default_incompatible_qos_callback, event_type=event_type, parent_handle=subscription_handle)) except UnsupportedEventTypeError: pass return event_handlers
def __init__( self, node_name, *, context=None, cli_args=None, namespace=None, use_global_arguments=True, start_parameter_services=True, initial_parameters=None ): self._handle = None self._context = get_default_context() if context is None else context self._parameters = {} self.publishers = [] self.subscriptions = [] self.clients = [] self.services = [] self.timers = [] self.guards = [] self.waitables = [] self._default_callback_group = MutuallyExclusiveCallbackGroup() self._parameters_callback = None namespace = namespace or '' if not self._context.ok(): raise NotInitializedException('cannot create node') try: self._handle = _rclpy.rclpy_create_node( node_name, namespace, self._context.handle, cli_args, use_global_arguments) except ValueError: # these will raise more specific errors if the name or namespace is bad validate_node_name(node_name) # emulate what rcl_node_init() does to accept '' and relative namespaces if not namespace: namespace = '/' if not namespace.startswith('/'): namespace = '/' + namespace validate_namespace(namespace) # Should not get to this point raise RuntimeError('rclpy_create_node failed for unknown reason') self._logger = get_logger(_rclpy.rclpy_get_node_logger_name(self.handle)) # Clock that has support for ROS time. # TODO(dhood): use sim time if parameter has been set on the node. self._clock = ROSClock() self._time_source = TimeSource(node=self) self._time_source.attach_clock(self._clock) self.__executor_weakref = None self._parameter_event_publisher = self.create_publisher( ParameterEvent, 'parameter_events', qos_profile=qos_profile_parameter_events) node_parameters = _rclpy.rclpy_get_node_parameters(Parameter, self.handle) # Combine parameters from params files with those from the node constructor and # use the set_parameters_atomically API so a parameter event is published. if initial_parameters is not None: node_parameters.update({p.name: p for p in initial_parameters}) self.set_parameters_atomically(node_parameters.values()) if start_parameter_services: self._parameter_service = ParameterService(self)
def _get_rosidl_class_helper(message_type, mode, logger=None): # noqa: C901 """ A helper function for common logic to be used by get_message_class and get_service_class. :param message_type: name of the message or service class in the form 'package_name/MessageName' or 'package_name/msg/MessageName' :type message_type: str :param mode: one of MSG_MODE, SRV_MODE or ACTION_MODE :type mode: str :param logger: The logger to be used for warnings and info :type logger: either rclpy.impl.rcutils_logger.RcutilsLogger or None :returns: The message or service class or None """ if logger is None: logger = logging.get_logger('_get_message_service_class_helper') if mode not in ROSIDL_FILTERS.keys(): logger.warn('invalid mode {}'.format(mode)) return None message_info = message_type.split('/') if len(message_info) not in (2, 3): logger.error('Malformed message_type: {}'.format(message_type)) return None if len(message_info) == 3 and message_info[1] != mode: logger.error('Malformed {} message_type: {}'.format(mode, message_type)) return None package = message_info[0] base_type = message_info[-1] try: _, resource_path = get_resource('rosidl_interfaces', package) except LookupError: return None python_pkg = None class_val = None try: # import the package python_pkg = importlib.import_module('%s.%s' % (package, mode)) except ImportError: logger.info('Failed to import class: {} as {}.{}'.format(message_type, package, mode)) return None try: class_val = getattr(python_pkg, base_type) return class_val except AttributeError: logger.info('Failed to load class: {}'.format(message_type)) return None
def __init__(self): super().__init__("test_node") self.create_subscription(std_msgs.Empty, "topic_tl", self.on_topic_tl, qos_profile=QoSProfile( depth=1, durability=QoSDurabilityPolicy. RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL, )) self.create_subscription( std_msgs.Empty, "topic_normal", self.on_topic_normal, qos_profile=QoSPresetProfiles.SERVICES_DEFAULT.value) self._buffer = tf2_ros.Buffer() self._listener = tf2_ros.TransformListener(self._buffer, self, spin_thread=True) get_logger(__name__).info("Initialized")
def __init__(self, node_name, *, cli_args=None, namespace=None, use_global_arguments=True, start_parameter_services=True): self._handle = None self._parameters = {} self.publishers = [] self.subscriptions = [] self.clients = [] self.services = [] self.timers = [] self.guards = [] self._default_callback_group = MutuallyExclusiveCallbackGroup() self._parameters_callback = None namespace = namespace or '' if not ok(): raise NotInitializedException('cannot create node') try: self._handle = _rclpy.rclpy_create_node(node_name, namespace, cli_args, use_global_arguments) except ValueError: # these will raise more specific errors if the name or namespace is bad validate_node_name(node_name) # emulate what rcl_node_init() does to accept '' and relative namespaces if not namespace: namespace = '/' if not namespace.startswith('/'): namespace = '/' + namespace validate_namespace(namespace) # Should not get to this point raise RuntimeError('rclpy_create_node failed for unknown reason') self._logger = get_logger( _rclpy.rclpy_get_node_logger_name(self.handle)) # Clock that has support for ROS time. # TODO(dhood): use sim time if parameter has been set on the node. self._clock = ROSClock() self._time_source = TimeSource(node=self) self._time_source.attach_clock(self._clock) self.__executor_weakref = None if start_parameter_services: self._parameter_service = ParameterService(self)
def _get_field_type(topic_names_and_types, target): # noqa: C901 """Testable helper function for get_field_type.""" logger = logging.get_logger('topic_helpers._get_field_type') delim = '/' tokenized_target = target.strip(delim).split(delim) for topic_name, types in topic_names_and_types: tokenized_topic_name = topic_name.strip(delim).split(delim) # If the target starts with the topic we have found a potential match if tokenized_target[:len(tokenized_topic_name )] == tokenized_topic_name: # If the target passed in was the topic address not the address of a field if tokenized_target == tokenized_topic_name: # If there is more than one type of topic on target if len(types) > 1: logger.warn( 'Ambiguous request. Multiple topic types found on: {}'. format(target)) return None, False # If the types array is empty then something weird has happend if len(types) == 0: logger.warn('No msg types found on: {}'.format(target)) return None, False # If there is only one msg type msg_class = get_message_class(types[0]) return msg_class, False else: # The topic must be a substring of the target # Get the address of the field in the messgage class field_address = target[len(topic_name):] # Iterate through the message types on the given topic and see if any match the # path that was provided for msg_type_str in types: try: msg_class = get_message_class(msg_type_str) field_type, is_array = get_slot_type( msg_class, field_address) return field_type, is_array except KeyError: pass logger.debug('faild to find field type: {}'.format(target)) return None, False
def get_action_class(action_type): """ Gets the action class from a string representation. :param action_type: the type of action in the form `action_pkg/Action` :type action_type: str :returns: None or the Class """ if action_type in _action_class_cache: return _action_class_cache[action_type] logger = logging.get_logger('get_action_class') class_val = _get_rosidl_class_helper(action_type, ACTION_MODE, logger) if class_val is not None: _action_class_cache[action_type] = class_val return class_val
def get_message_class(message_type): """ Gets the message class from a string representation. :param message_type: the type of message in the form `msg_pkg/Message` :type message_type: str :returns: None or the Class """ if message_type in _message_class_cache: return _message_class_cache[message_type] logger = logging.get_logger('get_message_class') class_val = _get_rosidl_class_helper(message_type, MSG_MODE, logger) if class_val is not None: _message_class_cache[message_type] = class_val return class_val
def get_service_class(srv_type): """ Gets the service class from a string representation. :param srv_type: the type of service in the form `package_name/ServiceName` or `package_name/srv/ServiceName` :type srv_type: str :returns: None or the Class """ if srv_type in _srv_class_cache: return _srv_class_cache[srv_type] logger = logging.get_logger('get_service_class') class_val = _get_rosidl_class_helper(srv_type, SRV_MODE, logger) if class_val is not None: _srv_class_cache[srv_type] = class_val return class_val
def get_logger(): return logging.get_logger('dynamic_stack_decider')
def on_topic_normal(self, msg): get_logger(__name__).info("Received on 'topic_normal'")
class LayoutUtil(object): _logger = logging.get_logger('LayoutUtil') @staticmethod def alternate_color(list_widgets, colors_alter=[Qt.white, Qt.gray]): """ Alternate the background color of the widgets that are ordered linearly, by the given list of colors. Originally intended for the elements of QHBoxLayout & QVBoxLayout. @type list_widgets: QtGui.QWidget[] @type colors_alter: QtCore.Qt.GlobalColor[] @param colors_alter: 1st element is used as initial/default color. @rtype: void @author: Isaac Saito """ colors_num = len(colors_alter) i_widget = 0 for w in list_widgets: w.setAutoFillBackground(True) p = w.palette() divisor = (i_widget + colors_num) % colors_num i_widget += 1 LayoutUtil._logger.debug( 'divisor={} i_widget={} colors_num={}'.format( divisor, i_widget, colors_num)) p.setColor(w.backgroundRole(), colors_alter[divisor]) w.setPalette(p) @staticmethod def clear_layout(layout): """ Clear all items in the given layout. Currently, only the instances of QWidgetItem get cleared (ie. QSpaceItem is ignored). Originally taken from http://stackoverflow.com/a/9375273/577001 :type layout: QLayout """ for i in reversed(range(layout.count())): item = layout.itemAt(i) if isinstance(item, QWidgetItem): # print "widget" + str(item) item.widget().close() # or # item.widget().setParent(None) elif isinstance(item, QSpacerItem): # print "spacer " + str(item) continue # no need to do extra stuff else: # print "layout " + str(item) LayoutUtil.clear_layout(item.layout()) # remove the item from layout layout.removeItem(item)
def publish_both(self): get_logger(__name__).info("Publishing") self.topic_tl_pub.publish(std_msgs.Empty()) self.topic_normal_pub.publish(std_msgs.Empty())
import itertools import numpy as np import rclpy from rclpy import logging from .candidate import CandidateFinder, Candidate from .color import ColorDetector from .field_boundary import FieldBoundaryDetector logger = logging.get_logger('vision_obstacle') class ObstacleDetector(CandidateFinder): """ The obstacle detection module is a :class:`bitbots_vision.vision_modules.candidate.CandidateFinder` that finds obstructions, e.g. robots. In order to perform its task it uses the normal or convex field_boundary of a :class:`bitbots_vision.vision_modules.field_boundary.FieldBoundaryDetector` depending on the method used. Given that the field boundary contains dents where objects obstruct the edge of the field and consists of a list of points, the obstacle detection module can find these objects by comparing the height of adjacent field boundary-points. Alternatively objects can be found by measuring the distance between the ordinary field boundary and the convex field boundary which is a slightly less efficient but more accurate method. """ def __init__(self, config, field_boundary_detector): """ Initialization of the ObstacleDetector. :param config: Configuration as defined in visionparams.yaml :param field_boundary_detector: locates the field_boundary """ # type: (dict, ColorDetector, ColorDetector, ColorDetector, FieldBoundaryDetector) -> None # Set used detectors self._field_boundary_detector = field_boundary_detector
class RqtRoscommUtil(object): _logger = logging.get_logger('RqtRoscommUtil') @staticmethod def load_parameters(config, caller_id): """ TODO(mlautman) Load parameters from node graph Load parameters onto the parameter server. Copied from ROSLaunchRunner. @type config: roslaunch.config.ROSLaunchConfig @raise RLException: """ # XMLRPC proxy for communicating with master, 'xmlrpclib.ServerProxy' # param_server = config.master.get() # param = None # try: # # multi-call style xmlrpc # # According to API doc, get_multi() returns # # multicall XMLRPC proxy for communicating with master, # # "xmlrpclib.MultiCall" # param_server_multi = config.master.get_multi() # # clear specified parameter namespaces # # 2468 unify clear params to prevent error # for param in roslaunch.launch._unify_clear_params(config.clear_params): # if param_server.hasParam(caller_id, param)[2]: # param_server_multi.deleteParam(caller_id, param) # r = param_server_multi() # for code, msg, _ in r: # if code != 1: # raise RLException("Failed to clear parameter {}: ".format(msg)) # except RLException: # raise # except Exception as e: # RqtRoscommUtil._logger.error( # "load_parameters: unable to set params (last param was [{}]): {}".format( # param, e)) # raise # re-raise as this is fatal # try: # # multi-call objects are not reusable # param_server_multi = config.master.get_multi() # for param in config.params.values(): # # suppressing this as it causes too much spam # # printlog("setting parameter [%s]"%param.key) # param_server_multi.setParam(caller_id, param.key, param.value) # r = param_server_multi() # for code, msg, _ in r: # if code != 1: # raise RLException("Failed to set parameter: %s" % (msg)) # except RLException: # raise # except Exception as e: # print( # "load_parameters: unable to set params (last param was [%s]): %s" % (param, e)) # raise # re-raise as this is fatal # RqtRoscommUtil._logger.debug("... load_parameters complete") RqtRoscommUtil._logger.error( 'load_parameters: not yet implemented in ROS2))') pass @staticmethod def iterate_packages(subdir): """ Iterator for packages that contain the given subdir. This method is generalizing rosmsg.iterate_packages. @param subdir: eg. 'launch', 'msg', 'srv', 'action' @type subdir: str @raise ValueError: """ if subdir is None or subdir == '': raise ValueError('Invalid package subdir = {}'.format(subdir)) packages_map = get_resources('packages') for package_name, package_path in packages_map.items(): package_path = os.path.join(package_path, 'share', package_name, subdir) RqtRoscommUtil._logger.debug('package:\t{} dir:\t{}'.format( package_name, package_path)) if os.path.isdir(package_path): yield package_name, package_path @staticmethod def list_files(package, subdir, file_extension='.launch'): """ Note: Mlautman 11/2/2018 This method is deprecated in ROS2 This functionality does not fit the ROS2 design paradigm of explicitly exporting resources to a shared location. #TODO: Come up with better name of the method. Taken from rosmsg. Lists files contained in the specified package @param package: package name, ``str`` @param file_extension: Defaults to '.launch', ``str`` :returns: list of msgs/srv in package, ``[str]`` """ RqtRoscommUtil._logger.error('list_files: not implemented in ROS2))') pass @staticmethod def _list_types(path, ext): """ Taken from rosmsg List all messages in the specified package :param package str: name of package to search :param include_depends bool: if True, will also list messages in package dependencies. :returns [str]: message type names """ types = RqtRoscommUtil._list_resources(path, RqtRoscommUtil._msg_filter(ext)) result = [x for x in types] # result = [x[:-len(ext)] for x in types] # Remove extension result.sort() return result @staticmethod def _list_resources(path, rfilter=os.path.isfile): """ Taken from rosmsg._list_resources List resources in a package directory within a particular subdirectory. This is useful for listing messages, services, etc... :param rfilter: resource filter function that returns true if filename is the desired resource type, ``fn(filename)->bool`` """ resources = [] if os.path.isdir(path): resources = [ f for f in os.listdir(path) if rfilter(os.path.join(path, f)) ] else: resources = [] return resources @staticmethod def _msg_filter(ext): """Taken from rosmsg._msg_filter""" def mfilter(f): """ Predicate for filtering directory list. matches message files :param f: filename, ``str`` """ return os.path.isfile(f) and f.endswith(ext) return mfilter
def main(args=None): # Argument parsing and usage parser = get_parser() parsed_args = parser.parse_args() # Configuration variables qos_policy_name = parsed_args.incompatible_qos_policy_name qos_profile_publisher = QoSProfile(depth=10) qos_profile_subscription = QoSProfile(depth=10) if qos_policy_name == 'durability': print('Durability incompatibility selected.\n' 'Incompatibility condition: publisher durability kind <' 'subscripition durability kind.\n' 'Setting publisher durability to: VOLATILE\n' 'Setting subscription durability to: TRANSIENT_LOCAL\n') qos_profile_publisher.durability = \ QoSDurabilityPolicy.RMW_QOS_POLICY_DURABILITY_VOLATILE qos_profile_subscription.durability = \ QoSDurabilityPolicy.RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL elif qos_policy_name == 'deadline': print( 'Deadline incompatibility selected.\n' 'Incompatibility condition: publisher deadline > subscription deadline.\n' 'Setting publisher durability to: 2 seconds\n' 'Setting subscription durability to: 1 second\n') qos_profile_publisher.deadline = Duration(seconds=2) qos_profile_subscription.deadline = Duration(seconds=1) elif qos_policy_name == 'liveliness_policy': print('Liveliness Policy incompatibility selected.\n' 'Incompatibility condition: publisher liveliness policy <' 'subscripition liveliness policy.\n' 'Setting publisher liveliness policy to: AUTOMATIC\n' 'Setting subscription liveliness policy to: MANUAL_BY_TOPIC\n') qos_profile_publisher.liveliness = \ QoSLivelinessPolicy.RMW_QOS_POLICY_LIVELINESS_AUTOMATIC qos_profile_subscription.liveliness = \ QoSLivelinessPolicy.RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC elif qos_policy_name == 'liveliness_lease_duration': print( 'Liveliness lease duration incompatibility selected.\n' 'Incompatibility condition: publisher liveliness lease duration >' 'subscription liveliness lease duration.\n' 'Setting publisher liveliness lease duration to: 2 seconds\n' 'Setting subscription liveliness lease duration to: 1 second\n') qos_profile_publisher.liveliness_lease_duration = Duration(seconds=2) qos_profile_subscription.liveliness_lease_duration = Duration( seconds=1) elif qos_policy_name == 'reliability': print( 'Reliability incompatibility selected.\n' 'Incompatibility condition: publisher reliability < subscripition reliability.\n' 'Setting publisher reliability to: BEST_EFFORT\n' 'Setting subscription reliability to: RELIABLE\n') qos_profile_publisher.reliability = \ QoSReliabilityPolicy.RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT qos_profile_subscription.reliability = \ QoSReliabilityPolicy.RMW_QOS_POLICY_RELIABILITY_RELIABLE else: print('{name} not recognised.'.format(name=qos_policy_name)) parser.print_help() return 0 # Initialization and configuration rclpy.init(args=args) topic = 'incompatible_qos_chatter' num_msgs = 5 publisher_callbacks = PublisherEventCallbacks( incompatible_qos=lambda event: get_logger('Talker').info(str(event))) subscription_callbacks = SubscriptionEventCallbacks( incompatible_qos=lambda event: get_logger('Listener').info(str(event))) try: talker = Talker(topic, qos_profile_publisher, event_callbacks=publisher_callbacks, publish_count=num_msgs) listener = Listener(topic, qos_profile_subscription, event_callbacks=subscription_callbacks) except UnsupportedEventTypeError as exc: print() print(exc, end='\n\n') print('Please try this demo using a different RMW implementation') return -1 executor = SingleThreadedExecutor() executor.add_node(listener) executor.add_node(talker) try: while talker.publish_count < num_msgs: executor.spin_once() except KeyboardInterrupt: pass rclpy.shutdown() return 0
import numpy as np import cv2 import rclpy import abc import math from operator import itemgetter from rclpy import logging from .color import ColorDetector logger = logging.get_logger('vision_field_boundary') class FieldBoundaryDetector(object): """ The abstract class :class:`.FieldBoundaryDetector` is used for detecting the field boundary in various ways. The task of such a detector is the localisation of the edges of the field in the image. It returns a list of points that form this so called field boundary. It requires the ColorDetector to find the green pixels that are used to identify the field in the picture. The pixels of the field boundary are found by traversing the picture column wise in steps of a given length. Because obstacles can obscure the edges of the field, sometimes the first green pixel from the top of the picture is found at the bottom of the respective obstacle. Therefore not all of the points are located in a straight line and the field boundary contains multiple dents. Additionally white field markings and green pixels in the field that are false negatives can create small dents too. Besides the normal field boundary, the :class:`.FieldBoundaryDetector` can also create a convex field boundary that forms a convex hull over the dents of the detected field boundary and is therefore completely straight (with the exception of the corners of the field). """ def __init__(self, config, field_color_detector): # type: (dict, ColorDetector) -> None """ Initialization of :class:`.FieldBoundaryDetector`.
def __init__(self, xacro_observer: XacroObserver, client: RobotDescriptionClient): self.xacro_observer = xacro_observer self.logger = roslog.get_logger('xacro_live') self.client = client self.future = Future()
import cv2 import os import abc import rclpy import numpy as np from rclpy import logging from math import exp from collections import defaultdict from .candidate import CandidateFinder, Candidate logger = logging.get_logger('vision_yolo') try: from pydarknet import Detector, Image except ImportError: logger.warning( "Not able to run Darknet YOLO! Its only executable under python3 with yolo34py or yolo34py-gpu installed." ) try: from openvino.inference_engine import IENetwork, IECore except ImportError: logger.warning( "Not able to run YOLO on the Intel NCS2 TPU! The OpenVINO SDK should be installed if you intend to run YOLO on the TPU" ) try: ie = IECore() except NameError: logger.warning( "Please install/source OpenVino environment to use the NCS2 YOLO Handler." ) try:
def __init__( self, node_name: str, *, context: Context = None, cli_args: List[str] = None, namespace: str = None, use_global_arguments: bool = True, start_parameter_services: bool = True, initial_parameters: List[Parameter] = None, allow_undeclared_parameters: bool = False, automatically_declare_initial_parameters: bool = True) -> None: """ Constructor. :param node_name: A name to give to this node. Validated by :func:`validate_node_name`. :param context: The context to be associated with, or ``None`` for the default global context. :param cli_args: A list of strings of command line args to be used only by this node. :param namespace: The namespace to which relative topic and service names will be prefixed. Validated by :func:`validate_namespace`. :param use_global_arguments: ``False`` if the node should ignore process-wide command line args. :param start_parameter_services: ``False`` if the node should not create parameter services. :param initial_parameters: A list of parameters to be set during node creation. :param allow_undeclared_parameters: True if undeclared parameters are allowed. This flag affects the behavior of parameter-related operations. :param automatically_declare_initial_parameters: True if initial parameters have to be declared upon node creation, false otherwise. """ self.__handle = None self._context = get_default_context() if context is None else context self._parameters: dict = {} self.__publishers: List[Publisher] = [] self.__subscriptions: List[Subscription] = [] self.__clients: List[Client] = [] self.__services: List[Service] = [] self.__timers: List[WallTimer] = [] self.__guards: List[GuardCondition] = [] self.__waitables: List[Waitable] = [] self._default_callback_group = MutuallyExclusiveCallbackGroup() self._parameters_callback = None self._allow_undeclared_parameters = allow_undeclared_parameters self._initial_parameters = {} self._descriptors = {} namespace = namespace or '' if not self._context.ok(): raise NotInitializedException('cannot create node') try: self.__handle = Handle( _rclpy.rclpy_create_node(node_name, namespace, self._context.handle, cli_args, use_global_arguments)) except ValueError: # these will raise more specific errors if the name or namespace is bad validate_node_name(node_name) # emulate what rcl_node_init() does to accept '' and relative namespaces if not namespace: namespace = '/' if not namespace.startswith('/'): namespace = '/' + namespace validate_namespace(namespace) # Should not get to this point raise RuntimeError('rclpy_create_node failed for unknown reason') with self.handle as capsule: self._logger = get_logger( _rclpy.rclpy_get_node_logger_name(capsule)) # Clock that has support for ROS time. self._clock = ROSClock() self._time_source = TimeSource(node=self) self._time_source.attach_clock(self._clock) self.__executor_weakref = None self._parameter_event_publisher = self.create_publisher( ParameterEvent, 'parameter_events', qos_profile=qos_profile_parameter_events) with self.handle as capsule: self._initial_parameters = _rclpy.rclpy_get_node_parameters( Parameter, capsule) # Combine parameters from params files with those from the node constructor and # use the set_parameters_atomically API so a parameter event is published. if initial_parameters is not None: self._initial_parameters.update( {p.name: p for p in initial_parameters}) if automatically_declare_initial_parameters: self._parameters.update(self._initial_parameters) self._descriptors.update( {p: ParameterDescriptor() for p in self._parameters}) if start_parameter_services: self._parameter_service = ParameterService(self)
# * Redistributions in binary form must reproduce the above # copyright notice, this list of conditions and the following # disclaimer in the documentation and/or other materials provided # with the distribution. # * Neither the name of Willow Garage, Inc. nor the names of its # contributors may be used to endorse or promote products derived # from this software without specific prior written permission. # # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. from rclpy.logging import get_logger logger = get_logger(__package__) debug = logger.debug error = logger.error fatal = logger.fatal info = logger.info warn = logger.warning
import abc import rclpy from rclpy import logging logger = logging.get_logger('vision_candidate') class Candidate: """ A :class:`.Candidate` is a representation of an arbitrary object in an image. It is very similar to bounding boxes but with an additional rating. This class provides several getters for different properties of the candidate. """ def __init__(self, x1=0, y1=0, width=0, height=0, rating=None): """ Initialization of :class:`.Candidate`. :param int x1: Horizontal part of the coordinate of the top left corner of the candidate :param int y1: Vertical part of the coordinate of the top left corner of the candidate :param int width: Horizontal size :param int height: Vertical size :param float rating: Confidence of the candidate """ self._x1 = x1 self._y1 = y1 self._width = width self._height = height self._rating = rating def get_width(self):
def __init__(self, root_file: str): self.xacro_tree = XacroTree(root_file) self.observer = Observer() self.logger = roslog.get_logger('xacro_live')
import rclpy import yaml from rclpy import logging from cv_bridge import CvBridge from vision_msgs.msg import BoundingBox2D, Pose2D, Point2D from humanoid_league_msgs.msg import Audio, GameState from soccer_vision_2d_msgs.msg import Ball, BallArray, FieldBoundary, Goalpost, GoalpostArray, Robot, RobotArray, MarkingArray, MarkingSegment """ This module provides some methods needed for the ros environment, e.g. methods to convert candidates to ROS messages or methods to modify the dynamic reconfigure objects. """ _cv_bridge = CvBridge() _game_state = None logger = logging.get_logger('bitbots_vision') general_parameters = [] def create_or_update_publisher(node, old_config, new_config, publisher_object, topic_key, data_class, qos_profile=1, callback_group=None): """ Creates or updates a publisher