serializer: serializer_node.py Source File

Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 """
00004     ROS Node for the Robotics Connection Serializer(TM) microcontroller
00005     
00006     Created for the Pi Robot Project: http://www.pirobot.org
00007     Copyright (c) 2010 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('serializer')
00023 import rospy
00024 import serializer_driver as SerializerAPI
00025 from serializer.msg import SensorState
00026 from serializer.srv import *
00027 from base_controller import *
00028 import os
00029 
00030 class SerializerROS():
00031     def __init__(self):
00032         rospy.init_node('serializer')
00033         rospy.on_shutdown(self.shutdown)
00034         self.port = rospy.get_param("~port", "/dev/ttyUSB0")
00035         self.baud = int(rospy.get_param("~baud", 19200))
00036         self.timeout = rospy.get_param("~timeout", 0.5)
00037         self.publish_sensors = rospy.get_param("~publish_sensors", False)
00038         self.rate = int(rospy.get_param("~sensor_rate", 10))
00039         
00040         self.pid_params = dict()
00041         self.pid_params['use_base_controller'] = rospy.get_param("~use_base_controller", False)
00042         self.pid_params['units'] = rospy.get_param("~units", 0)
00043         self.pid_params['wheel_diameter'] = rospy.get_param("~wheel_diameter", "") 
00044         self.pid_params['wheel_track'] = rospy.get_param("~wheel_track", "")
00045         self.pid_params['encoder_type'] = rospy.get_param("~encoder_type", 1) 
00046         self.pid_params['encoder_resolution'] = rospy.get_param("~encoder_resolution", "") 
00047         self.pid_params['gear_reduction'] = rospy.get_param("~gear_reduction", 1.0)
00048         self.pid_params['motors_reversed'] = rospy.get_param("~motors_reversed", False)
00049         
00050         self.pid_params['init_pid'] = rospy.get_param("~init_pid", False)  
00051         self.pid_params['VPID_P'] = rospy.get_param("~VPID_P", "")
00052         self.pid_params['VPID_I'] = rospy.get_param("~VPID_I", "")
00053         self.pid_params['VPID_D']  = rospy.get_param("~VPID_D", "")
00054         self.pid_params['VPID_L'] = rospy.get_param("~VPID_L", "")
00055         self.pid_params['DPID_P'] = rospy.get_param("~DPID_P", "")
00056         self.pid_params['DPID_I'] = rospy.get_param("~DPID_I", "")
00057         self.pid_params['DPID_D'] = rospy.get_param("~DPID_D", "")
00058         self.pid_params['DPID_A'] = rospy.get_param("~DPID_A", "")
00059         self.pid_params['DPID_B'] = rospy.get_param("~DPID_B", "")
00060         
00061         # Check PID parameters if we are using the base controller.
00062         if self.pid_params['use_base_controller']:
00063             pid_error = False
00064             for param in self.pid_params:
00065                 if self.pid_params[param] == "":
00066                     rospy.logerr("*** PID Parameter " + param + " is missing. ***")
00067                     pid_error = True
00068             if pid_error:
00069                 rospy.signal_shutdown("Missing PID parameters.")
00070                 os._exit(1)
00071                 
00072         rospy.loginfo("Connected to Serializer on port " + self.port + " at " + str(self.baud) + " baud")
00073         rospy.loginfo("Publishing Serializer data at " + str(self.rate) + " Hz")
00074         
00075         if self.publish_sensors:
00076             try:
00077                 self.analog_sensors = rospy.get_param("~analog", dict({}))
00078             except:
00079                 pass
00080             try:
00081                 self.digital_sensors = rospy.get_param("~digital", dict({}))
00082             except:
00083                 pass
00084             try:
00085                 self.maxez1_sensors = rospy.get_param("~maxez1", dict({}))
00086             except:
00087                 pass
00088         
00089             rospy.loginfo("Publishing Sensors:")
00090             try:
00091                 for sensor, params in self.analog_sensors.iteritems():
00092                     rospy.loginfo(sensor + " " + str(params))
00093             except:
00094                 pass  
00095             try:      
00096                 for sensor, params in self.digital_sensors.iteritems():
00097                     rospy.loginfo(sensor + " " + str(params))
00098             except:
00099                 pass
00100             try:
00101                 for sensor, params in self.maxez1_sensors.iteritems():
00102                     rospy.loginfo(sensor + " " + str(params))
00103             except:
00104                 pass
00105             
00106             # The SensorState publisher
00107             self.sensorStatePub = rospy.Publisher('~sensors', SensorState)
00108         
00109         # The Serializer services.
00110         rospy.Service('~SetServo', SetServo, self.SetServoHandler)
00111         rospy.Service('~GetAnalog', GetAnalog, self.GetAnalogHandler)
00112         rospy.Service('~Voltage', Voltage, self.VoltageHandler)
00113         rospy.Service('~Ping', Ping, self.PingHandler)
00114         rospy.Service('~MaxEZ1', MaxEZ1, self.MaxEZ1Handler)
00115         rospy.Service('~GP2D12', GP2D12, self.GP2D12Handler)
00116         rospy.Service('~PhidgetsTemperature', PhidgetsTemperature, self.PhidgetsTemperatureHandler)
00117         rospy.Service('~PhidgetsVoltage', PhidgetsVoltage, self.PhidgetsVoltageHandler)
00118         rospy.Service('~PhidgetsCurrent', PhidgetsCurrent, self.PhidgetsCurrentHandler)
00119         rospy.Service('~Rotate', Rotate, self.RotateHandler)
00120         rospy.Service('~TravelDistance', TravelDistance, self.TravelDistanceHandler)
00121    
00122         rosRate = rospy.Rate(self.rate)
00123         
00124         # Initialize the Serializer driver
00125         self.mySerializer = SerializerAPI.Serializer(self.pid_params, self.port, self.baud, self.timeout)
00126         rospy.sleep(1)
00127         self.mySerializer.connect()
00128         rospy.sleep(1)
00129         
00130         # Create and start the base controller.
00131         if self.pid_params['use_base_controller']:
00132             rospy.loginfo("Starting Serialzier base controller...")
00133             self.base_controller = base_controller(self.mySerializer, "Serializer PID")
00134             self.base_controller.start()
00135             
00136         self.sensors = dict({})
00137         
00138         while not rospy.is_shutdown():
00139             if self.publish_sensors:
00140                 for sensor, params in self.analog_sensors.iteritems():
00141                     if params['type'] == "GP2D12":
00142                         self.sensors[sensor] = self.mySerializer.get_GP2D12(params['pin'])
00143                     elif params['type'] == "Voltage":
00144                         self.sensors[sensor] = self.mySerializer.voltage()
00145                     elif params['type'] == "PhidgetsCurrent":
00146                         self.sensors[sensor] = self.mySerializer.get_PhidgetsCurrent(params['pin'])
00147                     elif params['type'] == "PhidgetsVoltage":
00148                         self.sensors[sensor] = self.mySerializer.get_PhidgetsVoltage(params['pin'])
00149                     elif params['type'] == "PhidgetsTemperature":
00150                         self.sensors[sensor] = self.mySerializer.get_PhidgetsTemperature(params['pin'])
00151                     else:
00152                         self.sensors[sensor] = self.mySerializer.get_analog(params['pin'])
00153                     rospy.sleep(0.01)
00154                         
00155                 for sensor, params in self.digital_sensors.iteritems():
00156                     if params['type'] == "Ping":
00157                         self.sensors[sensor] = self.mySerializer.get_Ping(params['pin'])
00158                     else:
00159                         self.sensors[sensor] = self.mySerializer.get_io(params['pin'])
00160                     rospy.sleep(0.01)
00161                     
00162                 for sensor, params in self.maxez1_sensors.iteritems():
00163                     self.sensors[sensor] = self.mySerializer.get_MaxEZ1(params['trigger_pin'], params['output_pin'])
00164                     rospy.sleep(0.01)
00165 
00166                 self.msg = SensorState()
00167                 self.msg.name = list()
00168                 self.msg.value = list()
00169                            
00170                 for sensor, value in self.sensors.iteritems():
00171                     self.msg.name.append(sensor)
00172                     try:
00173                         self.msg.value.append(round(float(value), 1))
00174                     except:
00175                         try:
00176                             self.msg.value.append(float(value))
00177                         except:
00178                             self.msg.value.append(-999.0)
00179                
00180                 self.msg.header.frame_id = "sensors"
00181                 self.msg.header.stamp = rospy.Time.now()     
00182                 self.sensorStatePub.publish(self.msg)
00183                 rosRate.sleep()
00184             else:
00185                 rospy.spin()
00186                 
00187     def SetServoHandler(self, req):
00188         self.mySerializer.servo(req.id, req.value)
00189         return SetServoResponse()
00190     
00191     def RotateHandler(self, req):
00192         self.mySerializer.rotate(req.angle, req.velocity)
00193         return RotateResponse()
00194     
00195     def TravelDistanceHandler(self, req):
00196         self.mySerializer.travel_distance(req.distance, req.velocity)
00197         return TravelDistanceResponse()
00198     
00199     def GetAnalogHandler(self, req):
00200         return GetAnalogResponse(self.mySerializer.get_analog(req.pin))
00201     
00202     def PingHandler(self, req):
00203         try:
00204             sonar = self.mySerializer.get_Ping(req.pin, req.cached)
00205         except:
00206             rospy.logerror("Bad sonar value on Ping " + str(req.pin) + ". Value was: " + str(sonar))
00207         return PingResponse(sonar)
00208     
00209     def MaxEZ1Handler(self, req):
00210         try:
00211             sonar = self.mySerializer.get_MaxEZ1(req.trigger_pin, req.output_pin, req.cached)
00212         except:
00213             rospy.logerror("Bad sonar value on MaxEZ1 " + str(req.trigger_pin) + ". Value was: " + str(sonar))
00214         return MaxEZ1Response(sonar)
00215 
00216     def GP2D12Handler(self, req):
00217         return GP2D12Response(self.mySerializer.get_GP2D12(req.pin, req.cached))
00218     
00219     def VoltageHandler(self, req):
00220         return VoltageResponse(self.mySerializer.voltage(req.cached))
00221 
00222     def PhidgetsTemperatureHandler(self, req):
00223         return PhidgetsTemperatureResponse(self.mySerializer.get_PhidgetsTemperature(req.pin, req.cached))
00224     
00225     def PhidgetsVoltageHandler(self, req):
00226         return PhidgetsVoltageResponse(self.mySerializer.get_PhidgetsVoltage(req.pin, req.cached))
00227     
00228     def PhidgetsCurrentHandler(self, req):
00229         return PhidgetsCurrentResponse(self.mySerializer.get_PhidgetsCurrent(req.pin, req.cached))
00230         
00231     def shutdown(self):
00232         rospy.loginfo("Shutting down Serializer Node.")
00233         
00234 if __name__ == '__main__':
00235     try:
00236         mySerializer = SerializerROS()
00237     except rospy.ROSInterruptException:
00238         pass