4 Control GPIO pins via ROS. Made to be as generic as possible, allowing the same node to be 5 used in multiple configurations/devices. 7 @author cst <chris thierauf, christopher.thierauf@tufts.edu> 10 @copyright Christopher Thierauf 2020. 12 The copyright holder uses this copyright to release the code in accordance with the license of 13 this repository, which is a Free and Open Source license: you may use, modify, and share this 14 code in any way you see fit as described by the terms of the license. You are not legally 15 obligated to provide attribution, but it would be greatly appreciated. 27 from std_msgs.msg
import Header
28 from gpio_control.msg
import InputState, OutputState
37 'beaglebone-experimental',
47 _DEFAULT_RATE_VAL = 10
51 _IMPORTED_PIGPIO =
False 52 _IMPORTED_ADAFRUIT_BBIO =
False 53 _IMPORTED_ONION_GPIO =
False 54 _IMPORTED_GPIO_API =
False 59 _IMPORTED_PIGPIO =
True 60 _IMPORTED_GPIO_API =
True 65 import Adafruit_BBIO.GPIO
as BBGPIO
67 _IMPORTED_ADAFRUIT_BBIO =
True 68 _IMPORTED_GPIO_API =
True 75 _IMPORTED_ONION_GPIO =
True 76 _IMPORTED_GPIO_API =
True 84 _num2words1 = {1:
'One', 2:
'Two', 3:
'Three', 4:
'Four', 5:
'Five',
85 6:
'Six', 7:
'Seven', 8:
'Eight', 9:
'Nine', 10:
'Ten',
86 11:
'Eleven', 12:
'Twelve', 13:
'Thirteen', 14:
'Fourteen',
87 15:
'Fifteen', 16:
'Sixteen', 17:
'Seventeen', 18:
'Eighteen', 19:
'Nineteen'}
88 _num2words2 = [
'Twenty',
'Thirty',
'Forty',
'Fifty',
'Sixty',
'Seventy',
'Eighty',
'Ninety']
93 return _num2words1[num]
95 tens, remainder = divmod(num, 10)
96 return _num2words2[tens-2] + _num2words1[remainder]
if remainder
else _num2words2[tens-2]
98 return _num2word(int(num/10)) +
"_thousand" 103 Class to provide consistent function calls to different pin outputs. 105 Takes in three functions as arguments, which allows us to call 'configure', 'set_low', 106 and 'set_high' on this class later, despite it being any actual implementation of gpio control 107 Also provide the option of an 'additional_shutdown' to run on close() if necessary. 109 We're using functional programming here and elsewhere. If you aren't familiar with that, give 110 that topic a quick read: https://docs.python.org/3/howto/functional.html. 111 If you're reading this because you'd like to contribute (thanks!) but are feeling daunted, 112 please feel free to open an issue on the tracker so I can work with you on that. 115 def __init__(self, configured_pinbus_object, pin, set_high_=None, set_low_=None):
117 if configured_pinbus_object
is not None:
124 """ Pull the pin low (0v), but don't try to run None """ 128 """ Pull the pin high (typically 3.3v or 5v), but don't try to run None """ 132 """ Make sure we're cleaning up while we shut down here """ 138 Class to provide consistent function calls to different pin inputs. 140 As before, takes in functions to be called later. 143 def __init__(self, configured_pinbus_object, pin, get_=None):
145 if configured_pinbus_object
is not None:
151 """ Get the current state of the GPIO pin. Returns true/false to indicate high/low """ 157 Configure the node as an input. Takes in a pin to access, and a string which is what was 158 passed in at the command line. 162 if device
in (
'pi',
'jetson'):
164 return_input.get_func =
lambda: return_input.configured_pinbus_object.read(int(pin))
167 if device ==
'beaglebone':
168 BBGPIO.setup(pin, BBGPIO.IN)
171 if device ==
'onion':
173 return_input.configured_pinbus_object.setInputDirection()
174 return_input.get_func = return_input.configured_pinbus_object.getValue
177 if device ==
'generic':
179 os.system(
'echo ' + str(pin) +
' > /sys/class/gpio/export')
180 os.system(
'echo in > /sys/class/gpio/gpio' + str(pin) +
'/direction')
184 lambda: int(subprocess.check_output(
185 [
'cat',
'/sys/class/gpio/gpio' + str(pin) +
'/value']
189 if device ==
'simulated':
190 return _GenericInputPin(
None, pin, get_=(
lambda: random.choice([
True,
False])))
192 raise RuntimeError(
'Device was invalid: ' + device)
197 Configure the node as an output. Takes in a pin to access, and a string which is what was 198 passed in at the command line. 200 if device
in (
'pi',
'jetson'):
203 return_output.set_low_func = (
204 lambda: return_output.configured_pinbus_object.write(int(pin), pigpio.LOW)
206 return_output.set_high_func = (
207 lambda: return_output.configured_pinbus_object.write(int(pin), pigpio.HIGH)
211 if device ==
'beaglebone':
213 lambda: BBGPIO.setup(pin, BBGPIO.OUT),
214 lambda: BBGPIO.output(pin, BBGPIO.LOW),
215 lambda: BBGPIO.output(pin, BBGPIO.HIGH),
218 if device ==
'onion':
220 return_output.configured_pinbus_object.setOutputDirection()
221 return_output.set_low_func =
lambda: return_output.configured_pinbus_object.setValue(0)
222 return_output.set_high_func =
lambda: return_output.configured_pinbus_object.setValue(1)
225 if device ==
'generic':
227 os.system(
'echo ' + str(pin) +
' > /sys/class/gpio/export')
228 os.system(
'echo out > /sys/class/gpio/gpio' + str(pin) +
'/direction')
232 set_low_=
lambda: os.system(
233 'echo 0 > /sys/class/gpio/gpio' + str(pin) +
'/value'),
234 set_high_=
lambda: os.system(
235 'echo 1 > /sys/class/gpio/gpio' + str(pin) +
'/value')
238 if device ==
'simulated':
239 return _GenericOutputPin(
None, pin, set_high_=(
lambda: print(
"[simulated] high!")),
240 set_low_=(
lambda: print(
"[simulated] low!")))
242 raise RuntimeError(
'Device was invalid: ' + device)
247 Convert input to a valid ROS name (alphabetic). This is necessary because ROS best practice is 248 to have topic names be only alphabetic (hyphens/slashes optional). Most GPIO pins will have 249 numbers in them, which can be an issue. 253 if isinstance(input_string, int)
or input_string.isnumeric():
254 return _num2word(int(input_string)).lower()
261 for character
in input_string:
262 if character.isalpha():
266 output_string += character.lower()
268 elif character.isnumeric():
272 output_string +=
_num2word(character).lower()
283 Configure a GPIO bus for the specific hardware we're dealing with. Return an object of the 284 appropriate hardware type, if the specific implementation requires that. 287 if device
in (
'pi',
'jetson'):
288 if not _IMPORTED_PIGPIO:
289 rospy.logfatal(
"You want to control your device with pigpio, but it didn't import " 290 "properly. Node will exit.")
295 if device ==
'beaglebone':
296 rospy.loginfo(
"This implementation is currently in beta. If you encounter bugs, please " 298 if not _IMPORTED_ADAFRUIT_BBIO:
299 rospy.logfatal(
"You want to control your device using the Adafruit BeagleBone Black " 300 "GPIO library, but it didn't import properly. Is it installed? Node " 304 if device ==
'onion':
305 rospy.loginfo(
"This implementation is currently in beta. If you encounter bugs, please " 307 if not _IMPORTED_ONION_GPIO:
308 rospy.logfatal(
"You want to control your device using the Onion Omega " 309 "GPIO library, but it didn't import properly. Is it installed? Node " 313 if device ==
'generic':
314 rospy.logwarn(
'You are using the generic GPIO controller, which operates using the Linux ' 315 'FHS to control GPIO states. It should not be considered as stable or safe ' 316 'as non-generic options.')
323 If the device in question requires cleanup functions, give them a run. 325 if device ==
'beaglebone':
326 return BBGPIO.cleanup()
328 if device ==
'generic':
329 unexporter = open(
'/sys/class/gpio/unexport',
'w')
338 Generic control of a GPIO device. Wraps the setup and then provides a spinner to run. 349 if device
not in VALID_DEVICES:
350 rospy.logerr(
"I don't know that device (" + device +
"). Valid devices: " +
351 str(VALID_DEVICES) +
"\nExiting.")
354 if device
not in [
'generic',
'simulated']
and not _IMPORTED_GPIO_API:
355 rospy.logfatal(
"You're trying to use a device-specific API, but we weren't able to " 356 "import the appropriate Python library. Please make sure that you have " 357 "the right library installed properly:\n " 358 "\t* Raspberry Pi: pigpio \n" 359 "\t* Nvidia Jetson: pigpio\n" 360 "\t* Beaglebone Black: Adafruit_BBIO\n" 361 "\t* Onion Omega: onionGpio\n" 362 "Cannot recover, exiting.")
366 """ Add a pin to perform input IO operations. """ 374 """ Add a pin to perform output IO operations. """ 377 def subscriber_callback(msg):
380 Called every time a message comes in telling us to change a pin state. 383 output_pin_obj.set_high_func()
385 output_pin_obj.set_low_func()
387 rospy.logerr(
"Not sure how to deal with " + str(msg))
394 def spin(self, rate_val: int =
None):
395 """ Wrapping the spinner function. """ 399 rate_val = _DEFAULT_RATE_VAL
400 rate = rospy.Rate(rate_val)
401 while not rospy.is_shutdown():
402 for pin_obj
in self._generic_pin_objects.values():
403 if pin_obj.type ==
'input':
407 header.stamp = rospy.Time.now()
410 if (isinstance(val, int)
and val == 1)
or (isinstance(val, bool)
and val):
412 elif (isinstance(val, int)
and val == 0)
or (isinstance(val, bool)
and not val):
415 rospy.logerr(
"Not sure how to deal with " + str(val))
419 InputState(header, val, str(pin_obj.pin))
422 rospy.logfatal_once(
"KeyError, you tried getting " +
423 str(pin_obj.pin) +
" but only " +
424 str(self._publishers.keys()) +
431 If using this code as an import, provide a simple function to set the pin. 433 if pin
not in self._generic_pin_objects.keys():
434 rospy.logerr(
"The pin you requested (" + str(pin) +
435 ") isn't in the list of ones we know about: " +
436 str(self._generic_pin_objects.keys()))
439 raise EnvironmentError(
"This pin is not an output! Can't set the state.")
448 If using this code as an import, provide a simple function to get the state of the pin. 450 if pin
not in self._generic_pin_objects.keys():
451 rospy.logerr(
"The pin you requested (" + str(pin) +
452 ") isn't in the list of ones we know about: " +
453 str(self._generic_pin_objects.keys()))
456 raise EnvironmentError(
"This pin is not an output! Can't set the state.")
def add_output_pin(self, pin)
def __init__(self, configured_pinbus_object, pin, set_high_=None, set_low_=None)
def configure_bus(device)
def _to_valid_ros_topic_name(input_string)
def configure_cleanup(device)
def add_input_pin(self, pin)