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
