element_node_threads.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 Queue
00031 import threading
00032 import os, signal, time
00033 
00034 class ElementROS():
00035     def __init__(self):
00036         rospy.init_node('element', log_level=rospy.DEBUG)
00037                 
00038         # Cleanup when termniating the node
00039         rospy.on_shutdown(self.shutdown)
00040         
00041         # The thread queue to use for sensor and base control
00042         self.thread_queue = Queue.Queue()
00043         
00044         #self.thread_queue.maxsize = 5
00045         
00046         # A lock for the queue
00047         self.queue_lock = threading.Lock()
00048         
00049         # Thread pool for running publishers
00050         self.thread_pool = list()
00051     
00052         # An empty object to cleanly stop the queue on exit
00053         self.STOP = object()
00054         
00055         self.port = rospy.get_param("~port", "/dev/ttyUSB0")
00056         self.baud = int(rospy.get_param("~baud", 19200))
00057         self.timeout = rospy.get_param("~timeout", 0.5)
00058         self.sensor_rate = int(rospy.get_param("~sensor_rate", 10))
00059         self.rate = int(rospy.get_param("~rate", 20))
00060         r = rospy.Rate(self.rate)
00061         
00062         self.units = rospy.get_param("~units", 0)
00063         
00064         now = rospy.Time.now()
00065         self.t_delta_sensors = rospy.Duration(1.0 / self.sensor_rate)
00066         self.t_next_sensors = now + self.t_delta_sensors
00067         
00068         self.publish_sensors = rospy.get_param("~publish_sensors", True)
00069         self.use_base_controller = rospy.get_param("~use_base_controller", False)
00070         self.pid_params = rospy.get_param("~pid_params", {})
00071         
00072         # ROS services for sensors and base control.
00073         rospy.Service('~SetServo', SetServo, self.SetServoHandler)
00074         rospy.Service('~GetAnalog', GetAnalog, self.GetAnalogHandler)
00075         rospy.Service('~Voltage', Voltage, self.VoltageHandler)
00076         rospy.Service('~Ping', Ping, self.PingHandler)
00077         rospy.Service('~MaxEZ1', MaxEZ1, self.MaxEZ1Handler)
00078         rospy.Service('~GP2D12', GP2D12, self.GP2D12Handler)
00079         rospy.Service('~PhidgetsTemperature', PhidgetsTemperature, self.PhidgetsTemperatureHandler)
00080         rospy.Service('~PhidgetsVoltage', PhidgetsVoltage, self.PhidgetsVoltageHandler)
00081         rospy.Service('~PhidgetsCurrent', PhidgetsCurrent, self.PhidgetsCurrentHandler)
00082         rospy.Service('~Rotate', Rotate, self.RotateHandler)
00083         rospy.Service('~TravelDistance', TravelDistance, self.TravelDistanceHandler)
00084         rospy.Service('~Speak', SP03, self.SP03Handler)
00085         
00086         # The SensorState publisher for backward compatibility
00087         self.sensorStatePub = rospy.Publisher('~sensors', SensorState)
00088         
00089         # Initialize the Twist message
00090         self.cmd_vel = Twist()
00091   
00092         # Publish the Twist message to the cmd_vel topic
00093         self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist)
00094            
00095         # Initialize and connect to the controller
00096         self.controller = Element(self.port, self.baud, self.timeout, self.units)
00097         self.controller.connect(self.use_base_controller)
00098         
00099         rospy.loginfo("Connected to Element on port " + self.port + " at " + str(self.baud) + " baud")
00100         
00101         self.heartbeat = 0
00102 
00103         # Initialize any sensors
00104         if self.publish_sensors:
00105             mySensors = Sensors(self.controller, self.thread_queue)
00106         
00107             for i in range(len(mySensors.sensors)):
00108                 self.thread_pool.append(threading.Timer(mySensors.sensors[i].interval, mySensors.poll_sensors, [self.thread_queue, mySensors.sensors[i]]))
00109                 self.thread_pool[i].daemon = True
00110                 self.thread_pool[i].start()
00111                 
00112         # Initialize the base controller if used
00113         if self.use_base_controller:
00114             myBaseController = BaseController(self.controller, self.thread_queue)
00115             base_thread = threading.Timer(myBaseController.interval, myBaseController.poll_odom, [self.thread_queue])
00116             self.thread_pool.append(base_thread)
00117             self.thread_pool[len(self.thread_pool) - 1].daemon = True
00118             self.thread_pool[len(self.thread_pool) - 1].start()
00119             #base_thread.start()
00120     
00121         # Set the thread queue in motion...
00122         while True:
00123          
00124             #if not self.thread_queue.empty() and not self.thread_queue.full():
00125             print "tick: ", self.heartbeat, self.thread_queue.qsize()
00126 
00127             #self.queue_lock.acquire()
00128             # Get the next poller
00129             poller = self.thread_queue.get()
00130             #self.queue_lock.release()
00131                         
00132             if poller is self.STOP:
00133                 self.thread_queue.task_done()
00134                 break
00135             
00136             # Run the poller
00137             poller()
00138             
00139             # Signal that we are finished
00140             #self.thread_queue.task_done()
00141             
00142             self.heartbeat += 1
00143             
00144             # For backwards compatibility...
00145             now = rospy.Time.now()
00146             
00147             if self.publish_sensors and now > self.t_next_sensors:
00148                 msg = SensorState()
00149                 msg.header.frame_id = 'base_link'
00150                 msg.header.stamp = now
00151                 for i in range(len(mySensors.sensors)):
00152                     msg.name.append(mySensors.sensors[i].name)
00153                     msg.value.append(mySensors.sensors[i].value)
00154                 try:
00155                     self.sensorStatePub.publish(msg)
00156                 except:
00157                     pass
00158                 
00159                 self.t_next_sensors = now + self.t_delta_sensors
00160             
00161             r.sleep()
00162                 
00163     def SetServoHandler(self, req):
00164         self.controller.servo(req.id, req.value)
00165         return SetServoResponse()
00166     
00167     def RotateHandler(self, req):
00168         self.controller.rotate(req.angle, req.velocity)
00169         return RotateResponse()
00170     
00171     def TravelDistanceHandler(self, req):
00172         self.controller.travel_distance(req.distance, req.velocity)
00173         return TravelDistanceResponse()
00174     
00175     def GetAnalogHandler(self, req):
00176         return GetAnalogResponse(self.controller.get_analog(req.pin))
00177     
00178     def PingHandler(self, req):
00179         try:
00180             range = self.controller.get_Ping(req.pin, req.cached)
00181             if self.units == 0:
00182                 range /= 100.
00183         except:
00184             rospy.loginfo("Bad sonar value on Ping " + str(req.pin) + ". Value was: " + str(range))
00185         return PingResponse(range)
00186     
00187     def MaxEZ1Handler(self, req):
00188         try:
00189             range = self.controller.get_MaxEZ1(req.trigger_pin, req.output_pin, req.cached)
00190             if self.units == 0:
00191                 range /= 100.
00192         except:
00193             rospy.loginfo("Bad sonar value on MaxEZ1 " + str(req.trigger_pin) + ". Value was: " + str(range))
00194         return MaxEZ1Response(range)
00195 
00196     def GP2D12Handler(self, req):
00197         try:
00198             range = self.controller.get_GP2D12(req.pin, req.cached)
00199             if self.units == 0:
00200                 range /= 100.
00201         except:
00202             rospy.loginfo("Bad IR value on GP2D12 " + str(req.pin) + ". Value was: " + str(range))
00203         return GP2D12Response(range)
00204                 
00205     def VoltageHandler(self, req):
00206         return VoltageResponse(self.controller.voltage(req.cached))
00207 
00208     def PhidgetsTemperatureHandler(self, req):
00209         return PhidgetsTemperatureResponse(self.controller.get_PhidgetsTemperature(req.pin, req.cached))
00210     
00211     def PhidgetsVoltageHandler(self, req):
00212         return PhidgetsVoltageResponse(self.controller.get_PhidgetsVoltage(req.pin, req.cached))
00213     
00214     def PhidgetsCurrentHandler(self, req):
00215         return PhidgetsCurrentResponse(self.controller.get_PhidgetsCurrent(req.pin, req.cached))
00216     
00217     def SP03Handler(self, req):
00218         return SP03Response(self.controller.sp03(req.msg, req.i2c_addr))
00219     
00220     def signal_handler(self, signum, frame):
00221         pass
00222         
00223     def shutdown(self):
00224         # Stop the robot
00225         try:
00226             rospy.loginfo("Stopping the robot...")
00227             self.cmd_vel_pub.publish(Twist())
00228             rospy.sleep(2)
00229         except:
00230             pass
00231         self.controller.shutdown = True
00232         rospy.loginfo("Shutting down Element Node...")
00233         rospy.loginfo("Closing threads, please wait...")
00234         for i in range(len(self.thread_pool)):
00235             self.thread_queue.put(self.STOP)
00236         #self.thread_queue.join()
00237         
00238 if __name__ == '__main__':
00239     myElement = ElementROS()


element
Author(s): Patrick Goebel
autogenerated on Mon Jan 6 2014 11:16:54