element_node.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 """
00004     ROS Node for the cmRobot Element microcontroller
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 element_driver import Element
00025 from base_controller import BaseController
00026 from sensors import Sensors
00027 from element.srv import *
00028 from element.msg import *
00029 from geometry_msgs.msg import Twist
00030 import os, signal, time
00031 import thread
00032 
00033 class ElementROS():
00034     def __init__(self):
00035         rospy.init_node('element', log_level=rospy.DEBUG)
00036                 
00037         # Cleanup when termniating the node
00038         rospy.on_shutdown(self.shutdown)
00039         
00040         self.port = rospy.get_param("~port", "/dev/ttyUSB0")
00041         self.baud = int(rospy.get_param("~baud", 19200))
00042         self.timeout = rospy.get_param("~timeout", 0.5)
00043         self.sensor_rate = int(rospy.get_param("~sensor_rate", 10))
00044         self.rate = int(rospy.get_param("~rate", 50))
00045         r = rospy.Rate(self.rate)
00046         
00047         self.units = rospy.get_param("~units", 0)
00048         
00049         now = rospy.Time.now()
00050         self.t_delta_sensors = rospy.Duration(1.0 / self.sensor_rate)
00051         self.t_next_sensors = now + self.t_delta_sensors
00052         
00053         self.publish_sensors = rospy.get_param("~publish_sensors", True)
00054         self.use_base_controller = rospy.get_param("~use_base_controller", False)
00055         self.pid_params = rospy.get_param("~pid_params", {})
00056         
00057         # ROS services for sensors and base control.
00058         rospy.Service('~SetServo', SetServo, self.SetServoHandler)
00059         rospy.Service('~GetAnalog', GetAnalog, self.GetAnalogHandler)
00060         rospy.Service('~Voltage', Voltage, self.VoltageHandler)
00061         rospy.Service('~Ping', Ping, self.PingHandler)
00062         rospy.Service('~MaxEZ1', MaxEZ1, self.MaxEZ1Handler)
00063         rospy.Service('~GP2D12', GP2D12, self.GP2D12Handler)
00064         rospy.Service('~PhidgetsTemperature', PhidgetsTemperature, self.PhidgetsTemperatureHandler)
00065         rospy.Service('~PhidgetsVoltage', PhidgetsVoltage, self.PhidgetsVoltageHandler)
00066         rospy.Service('~PhidgetsCurrent', PhidgetsCurrent, self.PhidgetsCurrentHandler)
00067         rospy.Service('~Rotate', Rotate, self.RotateHandler)
00068         rospy.Service('~TravelDistance', TravelDistance, self.TravelDistanceHandler)
00069         rospy.Service('~Speak', SP03, self.SP03Handler)
00070         
00071         # The SensorState publisher for backward compatibility
00072         self.sensorStatePub = rospy.Publisher('~sensors', SensorState)
00073         
00074         # Initialize the Twist message
00075         self.cmd_vel = Twist()
00076   
00077         # A cmd_vel publisher so we can stop the robot when shutting down
00078         self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist)
00079            
00080         # Initialize and connect to the controller
00081         self.controller = Element(self.port, self.baud, self.timeout, self.units)
00082         self.controller.connect(self.use_base_controller)
00083         
00084         rospy.loginfo("Connected to Element on port " + self.port + " at " + str(self.baud) + " baud")
00085 
00086         mutex = thread.allocate_lock()
00087 
00088         # Initialize any sensors
00089         if self.publish_sensors:
00090             mySensors = Sensors(self.controller)
00091                 
00092         # Initialize the base controller if used
00093         if self.use_base_controller:
00094             myBaseController = BaseController(self.controller)
00095     
00096         # Start polling the sensors and base controller
00097         while not rospy.is_shutdown():
00098             if self.publish_sensors:
00099                 for sensor in mySensors.sensors:
00100                     mutex.acquire()
00101                     sensor.poll()
00102                     mutex.release()
00103                     
00104             if self.use_base_controller:
00105                 mutex.acquire()
00106                 myBaseController.poll()
00107                 mutex.release()
00108             
00109             # For backwards compatibility with the old Serializer ROS package...
00110             now = rospy.Time.now()
00111             
00112             if self.publish_sensors and now > self.t_next_sensors:
00113                 msg = SensorState()
00114                 msg.header.frame_id = 'base_link'
00115                 msg.header.stamp = now
00116                 for i in range(len(mySensors.sensors)):
00117                     msg.name.append(mySensors.sensors[i].name)
00118                     msg.value.append(mySensors.sensors[i].value)
00119                 try:
00120                     self.sensorStatePub.publish(msg)
00121                 except:
00122                     pass
00123                 
00124                 self.t_next_sensors = now + self.t_delta_sensors
00125             
00126             r.sleep()
00127                 
00128     def SetServoHandler(self, req):
00129         self.controller.servo(req.id, req.value)
00130         return SetServoResponse()
00131     
00132     def RotateHandler(self, req):
00133         self.controller.rotate(req.angle, req.velocity)
00134         return RotateResponse()
00135     
00136     def TravelDistanceHandler(self, req):
00137         self.controller.travel_distance(req.distance, req.velocity)
00138         return TravelDistanceResponse()
00139     
00140     def GetAnalogHandler(self, req):
00141         return GetAnalogResponse(self.controller.get_analog(req.pin))
00142     
00143     def PingHandler(self, req):
00144         try:
00145             range = self.controller.get_Ping(req.pin, req.cached)
00146             if self.units == 0:
00147                 range /= 100.
00148         except:
00149             rospy.loginfo("Bad sonar value on Ping " + str(req.pin) + ". Value was: " + str(range))
00150         return PingResponse(range)
00151     
00152     def MaxEZ1Handler(self, req):
00153         try:
00154             range = self.controller.get_MaxEZ1(req.trigger_pin, req.output_pin, req.cached)
00155             if self.units == 0:
00156                 range /= 100.
00157         except:
00158             rospy.loginfo("Bad sonar value on MaxEZ1 " + str(req.trigger_pin) + ". Value was: " + str(range))
00159         return MaxEZ1Response(range)
00160 
00161     def GP2D12Handler(self, req):
00162         try:
00163             range = self.controller.get_GP2D12(req.pin, req.cached)
00164             if self.units == 0:
00165                 range /= 100.
00166         except:
00167             rospy.loginfo("Bad IR value on GP2D12 " + str(req.pin) + ". Value was: " + str(range))
00168         return GP2D12Response(range)
00169                 
00170     def VoltageHandler(self, req):
00171         return VoltageResponse(self.controller.voltage(req.cached))
00172 
00173     def PhidgetsTemperatureHandler(self, req):
00174         return PhidgetsTemperatureResponse(self.controller.get_PhidgetsTemperature(req.pin, req.cached))
00175     
00176     def PhidgetsVoltageHandler(self, req):
00177         return PhidgetsVoltageResponse(self.controller.get_PhidgetsVoltage(req.pin, req.cached))
00178     
00179     def PhidgetsCurrentHandler(self, req):
00180         return PhidgetsCurrentResponse(self.controller.get_PhidgetsCurrent(req.pin, req.cached))
00181     
00182     def SP03Handler(self, req):
00183         return SP03Response(self.controller.sp03(req.msg, req.i2c_addr))
00184     
00185     def signal_handler(self, signum, frame):
00186         pass
00187         
00188     def shutdown(self):
00189         # Stop the robot
00190         try:
00191             rospy.loginfo("Stopping the robot...")
00192             self.cmd_vel_pub.publish(Twist())
00193             rospy.sleep(2)
00194         except:
00195             pass
00196         rospy.loginfo("Shutting down Element Node...")
00197         
00198 if __name__ == '__main__':
00199     myElement = ElementROS()


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