00001
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
00039 rospy.on_shutdown(self.shutdown)
00040
00041
00042 self.thread_queue = Queue.Queue()
00043
00044
00045
00046
00047 self.queue_lock = threading.Lock()
00048
00049
00050 self.thread_pool = list()
00051
00052
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
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
00087 self.sensorStatePub = rospy.Publisher('~sensors', SensorState)
00088
00089
00090 self.cmd_vel = Twist()
00091
00092
00093 self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist)
00094
00095
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
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
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
00120
00121
00122 while True:
00123
00124
00125 print "tick: ", self.heartbeat, self.thread_queue.qsize()
00126
00127
00128
00129 poller = self.thread_queue.get()
00130
00131
00132 if poller is self.STOP:
00133 self.thread_queue.task_done()
00134 break
00135
00136
00137 poller()
00138
00139
00140
00141
00142 self.heartbeat += 1
00143
00144
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
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
00237
00238 if __name__ == '__main__':
00239 myElement = ElementROS()