sonar.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 sonar messages from the Arduino Mega 2560 mounted
00036 on BWI segbots, publishing them as ROS sensor_msgs/Range.
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 Range
00046 
00047 
00048 class SonarAttributes(Range):
00049     """ Subclass of sensor_msgs/Range, for filling in sonar attributes."""
00050     def __init__(self, frame_id,
00051                  field_of_view=0.4, min_range=0.01, max_range=2.0):
00052         super(SonarAttributes, self).__init__(
00053             radiation_type=Range.ULTRASOUND,
00054             field_of_view=field_of_view,
00055             min_range=min_range,
00056             max_range=max_range)
00057         if frame_id:
00058             self.header.frame_id = frame_id
00059 
00060 
00061 class SonarMessages(object):
00062     """ ROS message translation for UTexas BWI segbot Arduino sonar ranges. """
00063     def __init__(self):
00064         self.pub = rospy.Publisher('sonar', Range, queue_size=6)
00065         self.parser = re.compile(r'(\d+)=(\d+)cm')
00066         """ Extracts list of distances from the Arduino serial message.
00067 
00068         :returns: List of (sonar, distance) pairs of strings reported
00069             for the sonars, may be empty.
00070         """
00071 
00072         # Our version 2 segbots have three sonars.
00073         # TODO: define this configuration information using ROS parameters
00074         self.sonars = [
00075             SonarAttributes('sonar0_link'),
00076             SonarAttributes('sonar1_link'),
00077             SonarAttributes('sonar2_link')]
00078 
00079     def publish(self, serial_msg):
00080         """ Publish a ROS Range message for each reading.
00081 
00082         :param serial_msg: sonar message from Arduino.
00083         :type serial_msg: str
00084         """
00085         # Parse serial message line into a list of (sonar, distance)
00086         # pairs of strings reported for one or more sonars.  The
00087         # strings represent integers, with distances in centimeters.
00088         readings = self.parser.findall(serial_msg)
00089         if not readings:                # no data available?
00090             return
00091         now = rospy.Time.now()          # time when message received
00092         for sonar, distance in readings:
00093             sonar = int(sonar)
00094             if sonar >= len(self.sonars):  # unknown sonar?
00095                 continue                   # skip this reading
00096             msg = self.sonars[sonar]
00097             msg.header.stamp = now
00098             distance = int(distance)
00099             if distance == 0:           # nothing detected?
00100                 msg.range = float('+inf')  # follow REP-0117
00101             else:
00102                 # Convert distance from int centimeters to float meters.
00103                 msg.range = 0.01 * float(distance)
00104             self.pub.publish(msg)
00105 
00106 
00107 sonar = SonarMessages()                # make a SonarMessages instance
00108 handler = sonar.publish                # declare message handler interface
00109 """ This interface is called once for each sonar message received. """


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