imu.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 module handles IMU messages from the Arduino Mega 2560 mounted
00036 on BWI segbots, publishing them as ROS sensor_msgs/Imu.
00037 """
00038 
00039 # enable some python3 compatibility options:
00040 from __future__ import absolute_import, print_function, unicode_literals
00041 
00042 import re
00043 
00044 import rospy
00045 from sensor_msgs.msg import Imu
00046 from geometry_msgs.msg import Vector3
00047 
00048 
00049 def convert_accel(int_val):
00050     """ Convert acceleration component from device value to ROS value.
00051 
00052     :param int_val: Reading from device.
00053     :type int_val: str
00054     :returns: float value in meters per second squared.
00055 
00056     TODO: Figure out the actual conversion. The device readings are a
00057     signed A/D converter value based on a full range of 2g, which is
00058     about 19.6 m/s/s.
00059     """
00060     return (float(int_val) / 32768.0) * 19.6
00061 
00062 
00063 class ImuAttributes(Imu):
00064     """ Subclass of sensor_msgs/Imu, for filling in IMU attributes.
00065 
00066     The IMU orientation is ignored.  That information is not very
00067     useful, because the fixed frame of reference is not well-defined.
00068 
00069     The covariances of angular velocity and linear acceleration are
00070     presently unknown.  It would be helpful to figure out what they
00071     should be.
00072     """
00073     def __init__(self, frame_id):
00074         super(ImuAttributes, self).__init__(
00075             orientation_covariance=[-1.0, 0.0, 0.0,
00076                                     0.0, 0.0, 0.0,
00077                                     0.0, 0.0, 0.0],
00078             angular_velocity_covariance=[-1.0, 0.0, 0.0,
00079                                          0.0, 0.0, 0.0,
00080                                          0.0, 0.0, 0.0],
00081             linear_acceleration_covariance=[0.0, 0.0, 0.0,
00082                                             0.0, 0.0, 0.0,
00083                                             0.0, 0.0, 0.0])
00084         if frame_id:
00085             self.header.frame_id = frame_id
00086 
00087 
00088 class ImuMessages(object):
00089     """ ROS message translation for UTexas BWI segbot Arduino imu ranges. """
00090     def __init__(self):
00091         self.parser = re.compile(
00092             r'I(\d+) accel x: ([-+]?\d+) y: ([-+]?\d+) z: ([-+]?\d+)')
00093         """ Extracts IMU data from the Arduino serial message.
00094 
00095         :returns: List of IMU data strings reported, may be empty.
00096         """
00097         self.pubs = [rospy.Publisher('imu0', Imu, queue_size=1)]
00098         self.imus = [ImuAttributes('imu0')]
00099 
00100     def publish(self, serial_msg):
00101         """ Publish a ROS Range message for each reading.
00102 
00103         :param serial_msg: IMU message from Arduino.
00104         :type serial_msg: str
00105         """
00106         # Parse serial message line into a list of IMU data strings
00107         # containing integers.
00108         readings = self.parser.match(serial_msg)
00109         if not readings:                # no data available?
00110             rospy.logwarn('Invalid IMU message: ' + serial_msg)
00111             return
00112         now = rospy.Time.now()          # time when message received
00113         imu = int(readings.group(1))
00114 
00115         if imu >= len(self.imus):       # unknown IMU?
00116             rospy.logwarn('Invalid IMU number: ' + str(imu))
00117             return                      # skip this reading
00118 
00119         # Convert IMU acceleration components to ROS units
00120         x = convert_accel(readings.group(2))
00121         y = convert_accel(readings.group(3))
00122         z = convert_accel(readings.group(4))
00123         rospy.logdebug('IMU' + str(imu) +
00124                        ' accel: ' + str(x) + ', ' + str(y) + ', ' + str(z))
00125         msg = self.imus[imu]
00126         msg.linear_acceleration = Vector3(x, y, z)
00127         msg.header.stamp = now
00128 
00129         self.pubs[imu].publish(msg)
00130 
00131 
00132 imu = ImuMessages()                # make an ImuMessages instance
00133 handler = imu.publish              # declare message handler interface
00134 """ This interface is called once for each IMU message received. """


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