sensor.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 """
00004     ROS Sensor Class for the cmRobot Element
00005     
00006     Created for the Pi Robot Project: http://www.pirobot.org
00007     Copyright (c) 2012 Patrick Goebel.  All rights reserved.
00008 
00009     This program is free software; you can redistribute it and/or modify
00010     it under the terms of the GNU General Public License as published by
00011     the Free Software Foundation; either version 2 of the License, or
00012     (at your option) any later version.
00013     
00014     This program is distributed in the hope that it will be useful,
00015     but WITHOUT ANY WARRANTY; without even the implied warranty of
00016     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00017     GNU General Public License for more details at:
00018     
00019     http://www.gnu.org/licenses/gpl.html
00020 """
00021 
00022 import roslib; roslib.load_manifest('element')
00023 import rospy
00024 from std_msgs.msg import Float64, UInt16, Bool
00025 from sensor_msgs.msg import Range
00026 
00027 """ Sensor class for the cmRobot Element """
00028 class Sensor():
00029     def __init__(self, controller, name, digital=True, type="Digital_IO", rate=10, pin=None, trigger_pin=None, output_pin=None):
00030         self.controller = controller
00031         self.name = name
00032         self.digital = digital
00033         self.pin = pin
00034         self.trigger_pin = trigger_pin
00035         self.output_pin = output_pin
00036         self.type = type
00037         self.rate = rate
00038         self.value = None
00039         
00040         self.t_delta = rospy.Duration(1.0 / self.rate)
00041         self.t_next = rospy.Time.now() + self.t_delta
00042         
00043         self.sonar_range_types = ['Ping', 'SRF04', 'SRF05', 'SRF08', 'SRF10', 'MaxEZ1']
00044         self.ir_range_types = ['GP2D12']
00045         self.min_range = {'Ping': 0.02, 'SRF04': 0.03, 'SRF05': 0.01, 'SRF08': 0.03, 'SRF10': 0.03, 'GP2D12': 0.1, 'MaxEZ1': 0.022}
00046         self.max_range =  {'Ping': 3.0, 'SRF04': 3.0, 'SRF05': 4.0, 'SRF08': 6.0, 'SRF10': 6.0, 'GP2D12': 0.8, 'MaxEZ1': 6.45}
00047 
00048         float_types = ['Voltage', 'PhidgetsCurrent', 'PhidgetsVoltage', 'PhidgetsTemperature']
00049         
00050         if self.type in self.sonar_range_types or self.type in self.ir_range_types:
00051             self.pub = rospy.Publisher("~sensor/" + name, Range)
00052             self.msg = Range()
00053             self.msg.min_range = self.min_range[self.type]
00054             self.msg.max_range = self.max_range[self.type]
00055             
00056             if self.type in self.sonar_range_types:
00057                 self.msg.radiation_type = Range.ULTRASOUND
00058             else:
00059                 self.msg.radiation_type = Range.INFRARED
00060                 
00061         elif self.type == "Digital_IO":
00062             self.pub = rospy.Publisher("~sensor/" + name, Bool)
00063             self.msg = Bool()
00064         elif self.type in float_types:
00065             self.pub = rospy.Publisher("~sensor/" + name, Float64)
00066             self.msg = Float64()
00067         else:
00068              self.pub = rospy.Publisher("~sensor/" + name, UInt16)
00069              self.msg = UInt16()
00070                         
00071     def poll(self):
00072         now = rospy.Time.now()
00073         if now > self.t_next:
00074             if self.type in self.sonar_range_types or self.type in self.ir_range_types:
00075                 self.msg.header.stamp = rospy.Time.now()
00076                 if self.type in self.sonar_range_types:
00077                     if self.type == "Ping":
00078                         try:
00079                             self.value = self.controller.get_Ping(self.pin)
00080                         except:
00081                             return
00082                     elif self.type == 'MaxEZ1':
00083                         try:
00084                             self.value = self.controller.get_MaxEZ1(self.trigger_pin, self.output_pin)
00085                         except:
00086                             return
00087     
00088                 elif self.type == "GP2D12":
00089                     try:
00090                         self.value = self.controller.get_GP2D12(self.pin)
00091                     except:
00092                         return
00093     
00094                 # The element returns ranges in cm so convert to meters
00095                 try:
00096                     self.msg.range = self.value / 100.0
00097                 except:
00098                     return
00099     
00100             elif self.digital:
00101                 try:
00102                     self.msg.data = self.controller.get_io(self.pin)
00103                 except:
00104                     return
00105     
00106             elif self.type == "Voltage":
00107                 self.msg.data = self.controller.voltage()
00108             elif self.type == "PhidgetsCurrent":
00109                 self.msg.data = self.controller.get_PhidgetsCurrent(self.pin)
00110             elif self.type == "PhidgetsVoltage":
00111                 self.msg.data = self.controller.get_PhidgetsVoltage(self.pin)
00112             elif self.type == "PhidgetsTemperature":
00113                 self.msg.data = self.controller.get_PhidgetsTemperature(self.pin)
00114             else:
00115                 self.msg.data = self.controller.get_analog(self.pin)
00116                 
00117             # For backward compatibility with the Serializer ROS package
00118             try:
00119                 self.value = self.msg.data
00120             except:
00121                 pass
00122                 
00123             self.pub.publish(self.msg)
00124             
00125             self.t_next = now + self.t_delta
00126             
00127         


element
Author(s): Patrick Goebel
autogenerated on Sun Oct 5 2014 23:44:54