From 5a15f99f4fab253514842ca0f31bc75f3fb13217 Mon Sep 17 00:00:00 2001 From: mitsuhiro Date: Fri, 17 Jun 2016 22:02:22 +0900 Subject: [PATCH] add scritps --- README.md | 20 +- launch/plen.launch | 7 + script/bleNode.py | 447 ++++++++++++++++++++++++++++++++++++++++++ script/controlNode.py | 80 ++++++++ script/gpioNode.py | 82 ++++++++ script/i2cNode.py | 93 +++++++++ script/serialNode.py | 72 +++++++ 7 files changed, 800 insertions(+), 1 deletion(-) create mode 100644 launch/plen.launch create mode 100644 script/bleNode.py create mode 100644 script/controlNode.py create mode 100644 script/gpioNode.py create mode 100644 script/i2cNode.py create mode 100644 script/serialNode.py diff --git a/README.md b/README.md index 6e3212e..bb37421 100644 --- a/README.md +++ b/README.md @@ -1 +1,19 @@ -# plen-Firmware_ROS +plen-Firmware_ROS +================================================================================ +##Copyright (c) 2016, +- [Mitsuhiro YABU](https://github.com/MitsuhiroYabu) +- [PLEN Project Company Inc.](https://plen.jp) + +##Build Enviroment +- Intel Edison +- ROS indigo +- Bluez 5.35 +- Python 2.7.9 +- mraa +- pySerial +- python-dbus +- python-gobject + +##License +- This scritps are released under the BSD License. +- "bleNode.py" is released under GNU General Public License (GPL). \ No newline at end of file diff --git a/launch/plen.launch b/launch/plen.launch new file mode 100644 index 0000000..01794cb --- /dev/null +++ b/launch/plen.launch @@ -0,0 +1,7 @@ + + + + + + + \ No newline at end of file diff --git a/script/bleNode.py b/script/bleNode.py new file mode 100644 index 0000000..77f205c --- /dev/null +++ b/script/bleNode.py @@ -0,0 +1,447 @@ +#!/usr/bin/env python + +import subprocess +import time + +ps = subprocess.Popen(['ps', 'aux'], stdout=subprocess.PIPE,) +grep = subprocess.Popen(['grep', 'bluetooth'], stdin=ps.stdout, stdout=subprocess.PIPE,) +end_of_pipe = grep.stdout + +moji_2 = [] +result = [] +detect = 0 +for line in end_of_pipe: + if line.find('bluetoothd') != -1: + moji_2 = line.split(' ') + detect=1 +val2 = 0 +print moji_2 +if detect == 1: + for val in range(0,len(moji_2)): + if moji_2[val-val2] == "": + moji_2.pop(val-val2) + val2=val2+1 + print moji_2 + kill = subprocess.Popen(['kill',moji_2[1]], stdout=subprocess.PIPE,) + end_of_pipe = kill.stdout + detect = 0 +time.sleep(1.0) +bluetoothd = subprocess.Popen(['bluetoothd','-nE'], stdout=subprocess.PIPE,) +end_of_pipe = bluetoothd.stdout +time.sleep(1.0) + +import dbus +import dbus.exceptions +import dbus.mainloop.glib +import dbus.service + +import array +import gobject + +from random import randint + +import rospy +from std_msgs.msg import String + +rospy.init_node('bleNode', anonymous=True) +pub = rospy.Publisher('BleToControl', String, queue_size = 10) + +mainloop = None + +BLUEZ_SERVICE_NAME = 'org.bluez' +GATT_MANAGER_IFACE = 'org.bluez.GattManager1' +DBUS_OM_IFACE = 'org.freedesktop.DBus.ObjectManager' +DBUS_PROP_IFACE = 'org.freedesktop.DBus.Properties' + +GATT_SERVICE_IFACE = 'org.bluez.GattService1' +GATT_CHRC_IFACE = 'org.bluez.GattCharacteristic1' +GATT_DESC_IFACE = 'org.bluez.GattDescriptor1' + +class InvalidArgsException(dbus.exceptions.DBusException): + _dbus_error_name = 'org.freedesktop.DBus.Error.InvalidArgs' + +class NotSupportedException(dbus.exceptions.DBusException): + _dbus_error_name = 'org.bluez.Error.NotSupported' + +class NotPermittedException(dbus.exceptions.DBusException): + _dbus_error_name = 'org.bluez.Error.NotPermitted' + +class InvalidValueLengthException(dbus.exceptions.DBusException): + _dbus_error_name = 'org.bluez.Error.InvalidValueLength' + +class FailedException(dbus.exceptions.DBusException): + _dbus_error_name = 'org.bluez.Error.Failed' + + +class Service(dbus.service.Object): + PATH_BASE = '/org/bluez/example/service' + + def __init__(self, bus, index, uuid, primary): + self.path = self.PATH_BASE + str(index) + self.bus = bus + self.uuid = uuid + self.primary = primary + self.characteristics = [] + dbus.service.Object.__init__(self, bus, self.path) + + def get_properties(self): + return { + GATT_SERVICE_IFACE: { + 'UUID': self.uuid, + 'Primary': self.primary, + 'Characteristics': dbus.Array( + self.get_characteristic_paths(), + signature='o') + } + } + + def get_path(self): + return dbus.ObjectPath(self.path) + + def add_characteristic(self, characteristic): + self.characteristics.append(characteristic) + + def get_characteristic_paths(self): + result = [] + for chrc in self.characteristics: + result.append(chrc.get_path()) + return result + + def get_characteristics(self): + return self.characteristics + + @dbus.service.method(DBUS_PROP_IFACE, + in_signature='s', + out_signature='a{sv}') + def GetAll(self, interface): + if interface != GATT_SERVICE_IFACE: + raise InvalidArgsException() + + return self.get_properties[GATT_SERVICE_IFACE] + + @dbus.service.method(DBUS_OM_IFACE, out_signature='a{oa{sa{sv}}}') + def GetManagedObjects(self): + response = {} + print('GetManagedObjects') + + response[self.get_path()] = self.get_properties() + chrcs = self.get_characteristics() + for chrc in chrcs: + response[chrc.get_path()] = chrc.get_properties() + descs = chrc.get_descriptors() + for desc in descs: + response[desc.get_path()] = desc.get_properties() + + return response + + +class Characteristic(dbus.service.Object): + def __init__(self, bus, index, uuid, flags, service): + self.path = service.path + '/char' + str(index) + self.bus = bus + self.uuid = uuid + self.service = service + self.flags = flags + self.descriptors = [] + dbus.service.Object.__init__(self, bus, self.path) + + def get_properties(self): + return { + GATT_CHRC_IFACE: { + 'Service': self.service.get_path(), + 'UUID': self.uuid, + 'Flags': self.flags, + 'Descriptors': dbus.Array( + self.get_descriptor_paths(), + signature='o') + } + } + + def get_path(self): + return dbus.ObjectPath(self.path) + + def add_descriptor(self, descriptor): + self.descriptors.append(descriptor) + + def get_descriptor_paths(self): + result = [] + for desc in self.descriptors: + result.append(desc.get_path()) + return result + + def get_descriptors(self): + return self.descriptors + + @dbus.service.method(DBUS_PROP_IFACE, + in_signature='s', + out_signature='a{sv}') + def GetAll(self, interface): + if interface != GATT_CHRC_IFACE: + raise InvalidArgsException() + + return self.get_properties[GATT_CHRC_IFACE] + + @dbus.service.method(GATT_CHRC_IFACE, out_signature='ay') + def ReadValue(self): + print('Default ReadValue called, returning error') + raise NotSupportedException() + + @dbus.service.method(GATT_CHRC_IFACE, in_signature='ay') + def WriteValue(self, value): + print('Default WriteValue called, returning error') + raise NotSupportedException() + + @dbus.service.method(GATT_CHRC_IFACE) + def StartNotify(self): + print('Default StartNotify called, returning error') + raise NotSupportedException() + + @dbus.service.method(GATT_CHRC_IFACE) + def StopNotify(self): + print('Default StopNotify called, returning error') + raise NotSupportedException() + + @dbus.service.signal(DBUS_PROP_IFACE, + signature='sa{sv}as') + def PropertiesChanged(self, interface, changed, invalidated): + pass + + +class Descriptor(dbus.service.Object): + def __init__(self, bus, index, uuid, flags, characteristic): + self.path = characteristic.path + '/desc' + str(index) + self.bus = bus + self.uuid = uuid + self.flags = flags + self.chrc = characteristic + dbus.service.Object.__init__(self, bus, self.path) + + def get_properties(self): + return { + GATT_DESC_IFACE: { + 'Characteristic': self.chrc.get_path(), + 'UUID': self.uuid, + 'Flags': self.flags, + } + } + + def get_path(self): + return dbus.ObjectPath(self.path) + + @dbus.service.method(DBUS_PROP_IFACE, + in_signature='s', + out_signature='a{sv}') + def GetAll(self, interface): + if interface != GATT_DESC_IFACE: + raise InvalidArgsException() + + return self.get_properties[GATT_CHRC_IFACE] + + @dbus.service.method(GATT_DESC_IFACE, out_signature='ay') + def ReadValue(self): + print ('Default ReadValue called, returning error') + raise NotSupportedException() + + @dbus.service.method(GATT_DESC_IFACE, in_signature='ay') + def WriteValue(self, value): + print('Default WriteValue called, returning error') + raise NotSupportedException() + +class TestService(Service): + + TEST_SVC_UUID = 'E1F40469-CFE1-43C1-838D-DDBC9DAFDDE6' + CH_UUID = 'F90E9CFE-7E05-44A5-9D75-F13644D6F645' + CH_UUID2 = 'CF70EE7F-2A26-4F62-931F-9087AB12552C' + + def __init__(self, bus, index): + Service.__init__(self, bus, index, self.TEST_SVC_UUID, True) + self.add_characteristic(TestCharacteristic(bus, 1, self, self.CH_UUID2,0,1,['read'])) + self.add_characteristic(TestCharacteristic(bus, 2, self, self.CH_UUID,0,1,['read', 'write','writable-auxiliaries'])) + self.add_characteristic(TestCharacteristic(bus, 3, self, self.CH_UUIDd,0,1,['read', 'write'])) + +class TestCharacteristic(Characteristic): + + def __init__(self, bus, index, service,TEST_CHRC_UUID,flag,flag2,p): + Characteristic.__init__( + self, bus, index, + TEST_CHRC_UUID, + p, + service) + self.value = [] + if flag == 1: + self.add_descriptor(TestDescriptor(bus, 0, self)) + if flag2 == 1: + self.add_descriptor(CharacteristicUserDescriptionDescriptor(bus, 1, self)) + + def ReadValue(self): + print('TestCharacteristic Read: ' + repr(self.value)) + print('TestCharacteristic Read value: ' + str(self.value)) + return self.value + + def WriteValue(self, value): + self.value = value + s = "".join(chr(b) for b in value) + print s + rospy.loginfo("controlNode %s", s) + + if ('#' in s or '$' in s or '<' in s or '>' in s): + LEDon = String() + LEDon.data = "gpio,w,act" + send(LEDon) + + message = String() + message.data = "serial,w," + s + send(message) + + def StartNotify(self): + print('callback:StartNotify') + +class TestDescriptor(Descriptor): + + def __init__(self, bus, index, characteristic): + Descriptor.__init__( + self, bus, index, + self.TEST_DESC_UUID, + ['read', 'write'], + characteristic) + + def ReadValue(self): + return [ + dbus.Byte('T'), dbus.Byte('e'), dbus.Byte('s'), dbus.Byte('t') + ] + + +class CharacteristicUserDescriptionDescriptor(Descriptor): + CUD_UUID = '2901' + + def __init__(self, bus, index, characteristic): + self.writable = 'writable-auxiliaries' in characteristic.flags + self.value = array.array('B', 'TX Data') + self.value = self.value.tolist() + Descriptor.__init__( + self, bus, index, + self.CUD_UUID, + ['read', 'write'], + characteristic) + + def ReadValue(self): + return self.value + + def WriteValue(self, value): + if not self.writable: + raise NotPermittedException() + self.value = value + +def property_changed(interface, changed, invalidated, path): + iface = interface[interface.rfind(".") + 1:] + for name, value in changed.iteritems(): + val = str(value) + if name == 'Connected': + if val == "1": + print("ON") + message = String() + message.data = "gpio,w,on" + send(message) + elif val == "0": + print("OFF") + message = String() + message.data = "gpio,w,off" + send(message) + advertise() + + else: + pass + elif name == 'Alias' or name == 'Name': + print("ON") + message = String() + message.data = "gpio,w,on" + send(message) + else: + pass + +def register_service_cb(): + print('GATT service registered') + + +def register_service_error_cb(error): + print('Failed to register service: ' + str(error)) + mainloop.quit() + + +def find_adapter(bus): + remote_om = dbus.Interface(bus.get_object(BLUEZ_SERVICE_NAME, '/'), + DBUS_OM_IFACE) + objects = remote_om.GetManagedObjects() + + for o, props in objects.iteritems(): + if props.has_key(GATT_MANAGER_IFACE): + return o + + return None + +def send(message): + pub.publish(message) + +def prepare_ble_cmd(): + pass + +def advertise(): + print('hciup') + hci_on = subprocess.Popen(['hciconfig','hci0','up'], stdout=subprocess.PIPE,) + + end_of_pipe = hci_on.stdout + time.sleep(0.1) + print('hcitool') + hcitool_cmd2 = subprocess.Popen(['hcitool', '-i', 'hci0', 'cmd', '0x08', '0x0006', '20', '00', '20', '00','00', '00', '00', '00', '00', '00', '00', '00', '00', '07','00'], stdout=subprocess.PIPE,) + + end_of_pipe = hcitool_cmd2.stdout + hcitool_cmd = subprocess.Popen(['hcitool','-i','hci0','cmd','0x08','0x0008','15','02','01','06','11','07','e6','dd','af','9d','bc','dd','8d','83','c1','43','e1','cf','69','04','f4','e1'], stdout=subprocess.PIPE,) + + end_of_pipe = hcitool_cmd.stdout + hcitool_cmd3 = subprocess.Popen(['hcitool', '-i', 'hci0', 'cmd', '0x08', '0x000a' ,'01'],stdout=subprocess.PIPE,) + end_of_pipe = hcitool_cmd3.stdout + +def main(): + global mainloop + dbus.mainloop.glib.DBusGMainLoop(set_as_default=True) + + bus = dbus.SystemBus() + + adapter = find_adapter(bus) + if not adapter: + print('GattManager1 interface not found') + return + + service_manager = dbus.Interface( + bus.get_object(BLUEZ_SERVICE_NAME, adapter), + GATT_MANAGER_IFACE) + + test_service = TestService(bus,0) + + mainloop = gobject.MainLoop(is_running=True) + + bus.add_signal_receiver(property_changed, bus_name="org.bluez", + dbus_interface="org.freedesktop.DBus.Properties", + signal_name="PropertiesChanged", + path_keyword="path") + + service_manager.RegisterService(test_service.get_path(), {}, reply_handler=register_service_cb, error_handler=register_service_error_cb) + advertise() + advertise() + try: + print "mainloop.run!" + mainloop.run() + + except (KeyboardInterrupt, SystemExit): + mainloop.quit() + print "mainloop.quit!" + +def mybleNode_shutdown(): + global mainloop + mainloop.quit() + print "shutdown now!" + +rospy.on_shutdown(mybleNode_shutdown) + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/script/controlNode.py b/script/controlNode.py new file mode 100644 index 0000000..7bf3f1e --- /dev/null +++ b/script/controlNode.py @@ -0,0 +1,80 @@ +#!/usr/bin/env python + +import rospy +from std_msgs.msg import String + +accelgyro = ['0','0','0','0','0','0'] + +rospy.init_node('ControlNode', anonymous=True) +gpiopub = rospy.Publisher('ControlToGpio', String, queue_size = 10) +serialpub = rospy.Publisher('ControlToSerial', String, queue_size = 30) +blepub = rospy.Publisher('ControlToBle', String, queue_size = 10) +i2cpub = rospy.Publisher('ControlToI2c', String, queue_size = 10) + +def send_message_to_gpioNode(message): + global gpiopub + gpiopub.publish(message.data) + +def send_message_to_serialNode(message): + global serialpub + serialpub.publish(message.data) + +def send_message_to_bleNode(message): + global blepub + blepub.publish(message.data) + +def send_message_to_i2cNode(message): + global i2cpub + i2cpub.publish(message.data) + +def publish_control(message): + tmp = message.data.split(",") + send = String() + + if tmp[0] == "gpio": + send.data = ",".join(tmp[1:3]) + send_message_to_gpioNode(send) + elif tmp[0] == "serial": + send.data = ",".join(tmp[1:3]) + send_message_to_serialNode(send) + elif tmp[0] == "ble": + send.data = ",".join(tmp[1:3]) + send_message_to_bleNode(send) + elif tmp[0] == "i2c": + send.data = ",".join(tmp[1:3]) + send_message_to_i2cNode(send) + elif tmp[0] == "data": + if tmp[1] == "w": + for val in range(0,6): + global accelgyro + accelgyro[val] = tmp[val+1] + elif tmp[1] == "r": + send.data = "data" + global accelgyro + for num in range(0,len(accelgyro)): + send.data += ','+accelgyro[num] + send_message_to_serialNode(send) + + +def callback_gpio(message): + rospy.loginfo("gpioNode %s", message.data) + publish_control(message) + +def callback_serial(message): + rospy.loginfo("serialNode %s", message.data) + publish_control(message) + +def callback_ble(message): + rospy.loginfo("bleNode %s", message.data) + publish_control(message) + +def callback_pc(message): + rospy.loginfo("pcNode %s", message.data) + publish_control(message) + +rospy.Subscriber('GpioToControl', String, callback_gpio) +rospy.Subscriber('SerialToControl', String, callback_serial) +rospy.Subscriber('BleToControl', String, callback_ble) +rospy.Subscriber('PcToControl', String, callback_pc) + +rospy.spin() diff --git a/script/gpioNode.py b/script/gpioNode.py new file mode 100644 index 0000000..b679f02 --- /dev/null +++ b/script/gpioNode.py @@ -0,0 +1,82 @@ +#!/usr/bin/env python +import rospy +import mraa +from std_msgs.msg import String + + +class PlenEye(object): + def __init__(self, pin, pwm_period_us=700): + self._pwm = mraa.Pwm(pin) + self._pwm.period_us(pwm_period_us) + self._pwm.enable(True) + + def on(self): + self._pwm_duty = 1.0 + self._inc_speed = 0.0 + + def off(self): + self._pwm_duty = 0.0 + self._inc_speed = 0.0 + + def act(self): + self._pwm_duty = 0.0 + self._inc_speed = 0.05 + + def update(self): + self._pwm.write(self._pwm_duty) + self._pwm_duty = max(0, min(1, self._pwm_duty + self._inc_speed)) + + +def left_eye(): + return PlenEye(pin=14) + + +def right_eye(): + return PlenEye(pin=20) + + +class Node(object): + ROSPY_RATE_HZ = 100 + + def __init__(self): + self.eyes = (left_eye(), right_eye()) + + rospy.init_node('gpioNode', anonymous=True) + self.publisher = rospy.Publisher('GpioToControl', String, queue_size=10) + self.subscriber = rospy.Subscriber('ControlToGpio', String, self.subscribe) + self.rospy_rate = rospy.Rate(self.ROSPY_RATE_HZ) + + def subscribe(self, message): + rospy.loginfo("GPIO:%s", message.data) + + message_data = message.data.split(",") + assert len(message_data) == 2 + + rw, mode = message_data + + if rw != 'w': + return + + for eye in self.eyes: + if mode == "on": + eye.on() + elif mode == "off": + eye.off() + elif mode == "act": + eye.act() + else: + assert False + + def start(self): + try: + while not rospy.is_shutdown(): + for eye in self.eyes: + eye.update() + self.rospy_rate.sleep() + finally: + for eye in self.eyes: + eye.off() + + +if __name__ == '__main__': + Node().start() diff --git a/script/i2cNode.py b/script/i2cNode.py new file mode 100644 index 0000000..7287463 --- /dev/null +++ b/script/i2cNode.py @@ -0,0 +1,93 @@ +#!/usr/bin/env python +# coding=utf-8 + +# 便利な関数やメソッド +# - 文字列やバイト列の連結は join() +# - リストやタプルの各要素に関数を適用するときは map() または 内包表記 +# - 1バイト整数のリストやタプルを bytes に変換するときは bytearray() して bytes() +# - bytes を数値列に変換するときは struct.unpack() +# - 連続した整数の用意は range() か xrange() + +# 推奨される書式 +# - 関数名は小文字とアンダースコア + +# そのほか +# - 暗黙に仮定している (分かりにくい) 条件があれば assert文 で明示しておく + + +import struct + +import mraa +import rospy +from std_msgs.msg import String + + +class Mpu(object): + I2C_PORT = 0 + I2C_ADDRESS = 0x68 + + def __init__(self): + self._i2c = mraa.I2c(Mpu.I2C_PORT) + self._i2c.address(Mpu.I2C_ADDRESS) + self._i2c.writeReg(0x6B, 0x00) + + def read_accelgyros(self): + start_address = 0x3B + data_nbytes = 12 + reg_addresses = xrange(start_address, start_address + data_nbytes) + + # map(f, xs) は (f(x) for x in xs) と等価 + i2c_data = map(self._i2c.readReg, reg_addresses) + + # '>' : ビッグエンディアン (上位バイトから先に送られてくる形式) + # 'h' : 2byte符号付き整数 + data_format = '>' + 'h' * (data_nbytes / struct.calcsize('h')) + assert struct.calcsize(data_format) == data_nbytes + accelgyros = struct.unpack(data_format, bytes(bytearray(i2c_data))) + + return accelgyros + + +class Node(object): + NAME = 'i2cNode' + PUBLISHER_NAME = 'I2cToControl' + SUBSCRIBER_NAME = 'ControlToI2c' + ROSPY_RATE_HZ = 10 + + def __init__(self): + self.mpu = Mpu() + + rospy.init_node(Node.NAME, anonymous=True) + self.publisher = rospy.Publisher(Node.PUBLISHER_NAME, String, queue_size=10) + rospy.Subscriber(Node.SUBSCRIBER_NAME, String, self.subscribe) + self.rospy_rate = rospy.Rate(Node.ROSPY_RATE_HZ) + + def subscribe(self, message): + rospy.loginfo('controlNode %s', message.data) + + message_data = message.data.split(',') + assert len(message_data) == 1 + + rw, = message_data + assert rw == 'r' + + self.publish_accelgyros() + + def publish_accelgyros(self): + accelgyro = self.mpu.read_accelgyros() + + response = String() + response.data = 'data,w,' + ','.join(map(str, accelgyro)) + self.publisher.publish(response) + + def start(self): + try: + while not rospy.is_shutdown(): + self.publish_accelgyros() + self.rospy_rate.sleep() + finally: + pass + + +if __name__ == '__main__': + Node().start() diff --git a/script/serialNode.py b/script/serialNode.py new file mode 100644 index 0000000..0847a36 --- /dev/null +++ b/script/serialNode.py @@ -0,0 +1,72 @@ +#!/usr/bin/env python +import struct + +import serial +import mraa +import rospy +from std_msgs.msg import String + + +class PlenSerial(serial.Serial): + def __init__(self, port='/dev/ttyMFD1', baudrate=115200, re_de_pin=36, **kwargs): + super(PlenSerial, self).__init__(port=port, baudrate=baudrate, **kwargs) + self.re_de = mraa.Gpio(re_de_pin) + + def write_with_re_de(self, text): + self.re_de.write(1) + try: + self.write(text) + self.flushOutput() + finally: + self.re_de.write(0) + + +class Node(object): + SLEEP_RATE_HZ = 50 + + def __init__(self): + self.serial = PlenSerial() + + rospy.init_node('serialNode', anonymous=True) + self.publisher = rospy.Publisher('SerialToControl', String, queue_size=10) + self.subscribers = ( + rospy.Subscriber('ControlToSerial', String, self.subscribe_request), + rospy.Subscriber('I2cToControl', String, self.subscribe_accelgyros), + ) + self.sleep_rate = rospy.Rate(self.SLEEP_RATE_HZ) + + def subscribe_request(self, message): + request, text = message.data.split(',') + if request == 'w': + self.serial.write_with_re_de(text) + else: + rospy.logwarn('unknown request "%s" ignored.', request) + + def subscribe_accelgyros(self, message): + data = message.data.split(',') + self.accelgyros = map(int, data[2:]) + + def write_accelgyros(self): + # ready? + if self.serial.inWaiting() <= 0: + return + data = self.serial.read() + if data != '>': + rospy.logwarn('invalid input') + + # write + fmt = '>{}h'.format(len(self.accelgyros)) + text = struct.pack(fmt, *self.accelgyros) + self.serial.write_with_re_de(text) + + def start(self): + try: + while not rospy.is_shutdown(): + self.write_accelgyros() + self.sleep_rate.sleep() + finally: + self.serial.close() + + +if __name__ == '__main__': + Node().start()