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