def __init__(self, path_root, config_msg_type, status_msg_type):
self._path = path_root
self.config_mutex = Lock()
self.state_mutex = Lock()
self.cmd_times = []
self.ports = dict()
self.signals = dict()
self.state = None
self.config = None
self.state_changed = intera_dataflow.Signal()
self.config_changed = intera_dataflow.Signal()
self._config_sub = rospy.Subscriber(self._path + "/config",
config_msg_type,
self.handle_config)
self._state_sub = rospy.Subscriber(self._path + "/state",
status_msg_type, self.handle_state)
self._command_pub = rospy.Publisher(self._path + "/command",
IOComponentCommand,
queue_size=10)
# Wait for the state to be populated
intera_dataflow.wait_for(lambda: not self.state is None,
timeout=5.0,
timeout_msg=("Failed to get state."))
rospy.logdebug("Making new IOInterface on %s" % (self._path, ))
def __init__(self, component_id):
"""
Constructor.
@param component_id: unique id of the digital component
"""
self._id = component_id
self._component_type = 'digital_io'
self._is_output = False
self._state = None
self.state_changed = intera_dataflow.Signal()
type_ns = '/robot/' + self._component_type
topic_base = type_ns + '/' + self._id
self._sub_state = rospy.Subscriber(topic_base + '/state',
DigitalIOState, self._on_io_state)
intera_dataflow.wait_for(
lambda: self._state != None,
timeout=2.0,
timeout_msg="Failed to get current digital_io state from %s" \
% (topic_base,),
)
# check if output-capable before creating publisher
if self._is_output:
self._pub_output = rospy.Publisher(type_ns + '/command',
DigitalOutputCommand,
queue_size=10)
def __init__(self, path_root, config_msg_type, status_msg_type):
self._path = path_root
self.config_mutex = Lock()
self.state_mutex = Lock()
self.cmd_times = []
self.ports = dict()
self.signals = dict()
self.config = config_msg_type()
self.state = status_msg_type()
self.config_changed = intera_dataflow.Signal()
self.state_changed = intera_dataflow.Signal()
self._config_sub = rospy.Subscriber(self._path + "/config",
config_msg_type,
self.handle_config)
self._state_sub = rospy.Subscriber(self._path + "/state",
status_msg_type, self.handle_state)
self._command_pub = rospy.Publisher(self._path + "/command",
IOComponentCommand,
queue_size=10)
# Wait for the config to be populated
intera_dataflow.wait_for(
lambda: self.config is not None and self.is_config_valid(),
timeout=5.0,
timeout_msg=("Failed to get config at: {}.".format(self._path +
"/config")))
# Wait for the state to be populated too (optional)
is_init = intera_dataflow.wait_for(
lambda: self.state is not None and self.is_state_valid(),
timeout=5.0,
raise_on_error=False)
if not is_init:
rospy.loginfo(
"Did not receive initial state at: {}."
" Device may not be activated yet.".format(self._path +
"/state"))
rospy.logdebug("Making new IOInterface on %s" % (self._path, ))
def register_callback(self, callback_function, signal_name, poll_rate=10):
"""
Registers a supplied callback to a change in state of supplied
signal_name's value. Spawns a thread that will call the callback with
the updated value.
@type: function
@param: function handle for callback function
@type: str
@param: the signal name (button or wheel) to poll for value change
@type: int
@param: the rate at which to poll for a value change (in a separate
thread)
@rtype: str
@return: callback_id retuned if the callback was registered, and an
empty string if the requested signal_name does not exist in the
Navigator
"""
if signal_name in self.list_signal_names():
callback_id = uuid.uuid4()
self._callback_items[callback_id] = intera_dataflow.Signal()
def signal_spinner():
old_state = self.get_signal_value(signal_name)
r = rospy.Rate(poll_rate)
while not rospy.is_shutdown():
new_state = self.get_signal_value(signal_name)
if new_state != old_state:
self._callback_items[callback_id](new_state)
old_state = new_state
r.sleep()
self._callback_items[callback_id].connect(callback_function)
t = threading.Thread(target=signal_spinner)
t.daemon = True
t.start()
self._threads[callback_id] = t
self._callback_functions[callback_id] = callback_function
return callback_id
else:
return str()
