arduino_driver_node.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 # Software License Agreement (BSD License)
00003 #
00004 # Copyright (C) 2015, Jack O'Quin
00005 # All rights reserved.
00006 #
00007 # Redistribution and use in source and binary forms, with or without
00008 # modification, are permitted provided that the following conditions
00009 # are met:
00010 #
00011 #  * Redistributions of source code must retain the above copyright
00012 #    notice, this list of conditions and the following disclaimer.
00013 #  * Redistributions in binary form must reproduce the above
00014 #    copyright notice, this list of conditions and the following
00015 #    disclaimer in the documentation and/or other materials provided
00016 #    with the distribution.
00017 #  * Neither the name of the author nor of other contributors may be
00018 #    used to endorse or promote products derived from this software
00019 #    without specific prior written permission.
00020 #
00021 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 # POSSIBILITY OF SUCH DAMAGE.
00033 
00034 """
00035 This is a ROS device driver node that reads and parses serial messages
00036 from the Arduino Mega 2560 mounted on the version 2 BWI segbots,
00037 passing each line of data to a Python module specific to that attached
00038 device.
00039 
00040 .. note::
00041 
00042    TODO: add diagnositics
00043 
00044 """
00045 
00046 # enable some python3 compatibility options:
00047 from __future__ import absolute_import, print_function, unicode_literals
00048 
00049 import importlib
00050 import rospy
00051 
00052 from diagnostic_msgs.msg import DiagnosticStatus
00053 from .diagnostics import Diagnostics
00054 from .serial import ArduinoDevice
00055 
00056 
00057 class ArduinoDriver(object):
00058     """ ROS driver node for UTexas BWI segbot Arduino messages. """
00059     def __init__(self, port='/dev/ttyACM0', baud=115200,
00060                  node_name='arduino_driver'):
00061         rospy.init_node(node_name)
00062         port = rospy.get_param('~port', port)
00063         baud = rospy.get_param('~baud', baud)
00064 
00065         self.diag = Diagnostics()
00066         """ Diagnostics collector and publisher. """
00067         self.status = DiagnosticStatus(name=node_name)
00068         """ Current diagnostic status for this driver. """
00069         self.set_status(
00070             level=DiagnosticStatus.OK,
00071             message='Arduino driver starting.')
00072 
00073         self.arduino = ArduinoDevice(port, baud)
00074         """ Arduino serial device connection. """
00075         rospy.on_shutdown(self.shutdown)
00076 
00077         # define the known message types
00078         self.msgs = {}
00079         """ Dictionary of message types and handlers. """
00080         self.add('I', 'segbot_sensors.imu')
00081         self.add('S', 'segbot_sensors.sonar')
00082         self.add('V', 'segbot_sensors.voltmeter')
00083 
00084     def add(self, type_char, handler):
00085         """ Define another Arduino message.
00086 
00087         :param type_char: Identifying first character of this message.
00088         :type type_char: str
00089         :param handler: Python module for handling those messages.
00090         :type handler: str
00091 
00092         Adds messages starting with type_char to arduino_msgs
00093 
00094         :raises: :exc:`.ValueError` if request already exists.
00095         :raises: :exc:`.ImportError` if unable to import handler.
00096         """
00097         if type_char in self.msgs:
00098             raise ValueError('Duplicate Arduino message type: ' + type_char)
00099         self.msgs[type_char] = importlib.import_module(handler)
00100 
00101     def set_status(self, level, message):
00102         """ Update diagnostic status. """
00103         self.status.level = level
00104         self.status.message = message
00105         self.diag.update(self.status)
00106 
00107     def shutdown(self):
00108         """ Called by rospy on shutdown. """
00109         self.arduino.close()
00110 
00111     def spin(self):
00112         """ Main driver loop. """
00113         slow_poll = rospy.Rate(1.0)     # slow poll frequency
00114         while not rospy.is_shutdown():
00115 
00116             if self.arduino.ok():       # device connected?
00117                 msg = self.arduino.read()
00118                 if len(msg) > 0:        # something read?
00119                     handler = self.msgs.get(msg[0:1])
00120                     if handler is not None:
00121                         handler.handler(msg)
00122                     else:
00123                         rospy.logwarn('unknown Arduino message: ' + msg)
00124                 self.set_status(
00125                     level=DiagnosticStatus.OK,
00126                     message='Arduino driver running.')
00127 
00128             elif self.arduino.open():   # open succeeded?
00129                 pass
00130 
00131             else:
00132                 slow_poll.sleep()
00133                 self.set_status(
00134                     level=DiagnosticStatus.ERROR,
00135                     message='Arduino not connected.')
00136 
00137 
00138 def main():
00139     """ Arduino driver node main entry point."""
00140     node = ArduinoDriver()
00141     node.spin()                         # run main driver loop
00142 
00143 if __name__ == '__main__':
00144     main()


segbot_sensors
Author(s): Piyush Khandelwal
autogenerated on Thu Jun 6 2019 21:37:13