sensor.py

Go to the documentation of this file.

#!/usr/bin/env python

"""
    ROS Sensor Class for the cmRobot Element
    
    Created for the Pi Robot Project: http://www.pirobot.org
    Copyright (c) 2012 Patrick Goebel.  All rights reserved.

    This program is free software; you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation; either version 2 of the License, or
    (at your option) any later version.
    
    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details at:
    
    http://www.gnu.org/licenses/gpl.html
"""

import roslib; roslib.load_manifest('element')
import rospy
from std_msgs.msg import Float64, UInt16, Bool
from sensor_msgs.msg import Range

""" Sensor class for the cmRobot Element """
class Sensor():
    def __init__(self, controller, name, digital=True, type="Digital_IO", rate=10, pin=None, trigger_pin=None, output_pin=None):
        self.controller = controller
        self.name = name
        self.digital = digital
        self.pin = pin
        self.trigger_pin = trigger_pin
        self.output_pin = output_pin
        self.type = type
        self.rate = rate
        self.value = None
        
        self.t_delta = rospy.Duration(1.0 / self.rate)
        self.t_next = rospy.Time.now() + self.t_delta
        
        self.sonar_range_types = ['Ping', 'SRF04', 'SRF05', 'SRF08', 'SRF10', 'MaxEZ1']
        self.ir_range_types = ['GP2D12']
        self.min_range = {'Ping': 0.02, 'SRF04': 0.03, 'SRF05': 0.01, 'SRF08': 0.03, 'SRF10': 0.03, 'GP2D12': 0.1, 'MaxEZ1': 0.022}
        self.max_range =  {'Ping': 3.0, 'SRF04': 3.0, 'SRF05': 4.0, 'SRF08': 6.0, 'SRF10': 6.0, 'GP2D12': 0.8, 'MaxEZ1': 6.45}

        float_types = ['Voltage', 'PhidgetsCurrent', 'PhidgetsVoltage', 'PhidgetsTemperature']
        
        if self.type in self.sonar_range_types or self.type in self.ir_range_types:
            self.pub = rospy.Publisher("~sensor/" + name, Range)
            self.msg = Range()
            self.msg.min_range = self.min_range[self.type]
            self.msg.max_range = self.max_range[self.type]
            
            if self.type in self.sonar_range_types:
                self.msg.radiation_type = Range.ULTRASOUND
            else:
                self.msg.radiation_type = Range.INFRARED
                
        elif self.type == "Digital_IO":
            self.pub = rospy.Publisher("~sensor/" + name, Bool)
            self.msg = Bool()
        elif self.type in float_types:
            self.pub = rospy.Publisher("~sensor/" + name, Float64)
            self.msg = Float64()
        else:
             self.pub = rospy.Publisher("~sensor/" + name, UInt16)
             self.msg = UInt16()
                        
    def poll(self):
        now = rospy.Time.now()
        if now > self.t_next:
            if self.type in self.sonar_range_types or self.type in self.ir_range_types:
                self.msg.header.stamp = rospy.Time.now()
                if self.type in self.sonar_range_types:
                    if self.type == "Ping":
                        try:
                            self.value = self.controller.get_Ping(self.pin)
                        except:
                            return
                    elif self.type == 'MaxEZ1':
                        try:
                            self.value = self.controller.get_MaxEZ1(self.trigger_pin, self.output_pin)
                        except:
                            return
    
                elif self.type == "GP2D12":
                    try:
                        self.value = self.controller.get_GP2D12(self.pin)
                    except:
                        return
    
                # The element returns ranges in cm so convert to meters
                try:
                    self.msg.range = self.value / 100.0
                except:
                    return
    
            elif self.digital:
                try:
                    self.msg.data = self.controller.get_io(self.pin)
                except:
                    return
    
            elif self.type == "Voltage":
                self.msg.data = self.controller.voltage()
            elif self.type == "PhidgetsCurrent":
                self.msg.data = self.controller.get_PhidgetsCurrent(self.pin)
            elif self.type == "PhidgetsVoltage":
                self.msg.data = self.controller.get_PhidgetsVoltage(self.pin)
            elif self.type == "PhidgetsTemperature":
                self.msg.data = self.controller.get_PhidgetsTemperature(self.pin)
            else:
                self.msg.data = self.controller.get_analog(self.pin)
                
            # For backward compatibility with the Serializer ROS package
            try:
                self.value = self.msg.data
            except:
                pass
                
            self.pub.publish(self.msg)
            
            self.t_next = now + self.t_delta