element: sensor_threads.py Source File

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 import os
00025 from std_msgs.msg import Float64, UInt16, Bool
00026 from sensor_msgs.msg import Range
00027 
00028 """ Sensor class for the cmRobot Element """
00029 class Sensor():
00030     def __init__(self, controller, name, digital=True, type="Digital_IO", rate=10, pin=None, trigger_pin=None, output_pin=None):
00031         self.controller = controller
00032         self.name = name
00033         self.digital = digital
00034         self.pin = pin
00035         self.trigger_pin = trigger_pin
00036         self.output_pin = output_pin
00037         self.type = type
00038         self.rate = rate
00039         self.interval = 1.0 / rate
00040         self.value = None
00041         
00042         self.sonar_range_types = ['Ping', 'SRF04', 'SRF05', 'SRF08', 'SRF10', 'MaxEZ1']
00043         self.ir_range_types = ['GP2D12']
00044         self.min_range = {'Ping': 0.02, 'SRF04': 0.03, 'SRF05': 0.01, 'SRF08': 0.03, 'SRF10': 0.03, 'GP2D12': 0.1, 'MaxEZ1': 0.022}
00045         self.max_range =  {'Ping': 3.0, 'SRF04': 3.0, 'SRF05': 4.0, 'SRF08': 6.0, 'SRF10': 6.0, 'GP2D12': 0.8, 'MaxEZ1': 6.45}
00046 
00047         float_types = ['Voltage', 'PhidgetsCurrent', 'PhidgetsVoltage', 'PhidgetsTemperature']
00048         
00049         if self.type in self.sonar_range_types or self.type in self.ir_range_types:
00050             self.pub = rospy.Publisher("~sensor/" + name, Range)
00051             self.msg = Range()
00052             self.msg.min_range = self.min_range[self.type]
00053             self.msg.max_range = self.max_range[self.type]
00054             
00055             if self.type in self.sonar_range_types:
00056                 self.msg.radiation_type = Range.ULTRASOUND
00057             else:
00058                 self.msg.radiation_type = Range.INFRARED
00059                 
00060         elif self.type == "Digital_IO":
00061             self.pub = rospy.Publisher("~sensor/" + name, Bool)
00062             self.msg = Bool()
00063         elif self.type in float_types:
00064             self.pub = rospy.Publisher("~sensor/" + name, Float64)
00065             self.msg = Float64()
00066         else:
00067              self.pub = rospy.Publisher("~sensor/" + name, UInt16)
00068              self.msg = UInt16()
00069                         
00070     def poll(self):
00071         if self.type in self.sonar_range_types or self.type in self.ir_range_types:
00072             self.msg.header.stamp = rospy.Time.now()
00073             if self.type in self.sonar_range_types:
00074                 if self.type == "Ping":
00075                     try:
00076                         self.value = self.controller.get_Ping(self.pin)
00077                     except:
00078                         return
00079                 elif self.type == 'MaxEZ1':
00080                     try:
00081                         self.value = self.controller.get_MaxEZ1(self.trigger_pin, self.output_pin)
00082                     except:
00083                         return
00084 
00085             elif self.type == "GP2D12":
00086                 try:
00087                     self.value = self.controller.get_GP2D12(self.pin)
00088                 except:
00089                     return
00090 
00091             # The element returns ranges in cm so convert to meters
00092             try:
00093                 self.msg.range = self.value / 100.0
00094             except:
00095                 return
00096 
00097         elif self.digital:
00098             try:
00099                 self.msg.data = self.controller.get_io(self.pin)
00100             except:
00101                 return
00102 
00103         elif self.type == "Voltage":
00104             self.msg.data = self.controller.voltage()
00105         elif self.type == "PhidgetsCurrent":
00106             self.msg.data = self.controller.get_PhidgetsCurrent(self.pin)
00107         elif self.type == "PhidgetsVoltage":
00108             self.msg.data = self.controller.get_PhidgetsVoltage(self.pin)
00109         elif self.type == "PhidgetsTemperature":
00110             self.msg.data = self.controller.get_PhidgetsTemperature(self.pin)
00111         else:
00112             self.msg.data = self.controller.get_analog(self.pin)
00113             
00114         # For backward compatibility with the Serializer ROS package
00115         try:
00116             self.value = self.msg.data
00117         except:
00118             pass
00119             
00120         self.pub.publish(self.msg)
00121