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()