driver.py
Go to the documentation of this file.
00001 #!/usr/bin/python
00002 import roslib; roslib.load_manifest('irobot_create_2_1')
00003 import rospy
00004 from time import sleep
00005 from irobot import Create
00006 from threading import Thread
00007 from math import sin,cos,pi
00008 from datetime import datetime
00009 
00010 from geometry_msgs.msg import Quaternion
00011 from geometry_msgs.msg import Twist
00012 from nav_msgs.msg import Odometry
00013 from tf.broadcaster import TransformBroadcaster
00014 
00015 from irobot_create_2_1.msg import SensorPacket
00016 from irobot_create_2_1.srv import *
00017 
00018 class CreateDriver:
00019         def __init__(self):
00020                 port = rospy.get_param('/brown/irobot_create_2_1/port', "/dev/ttyUSB0")
00021                 self.autodock = rospy.get_param('/brown/irobot_create_2_1/autodock', 0.0)
00022                 self.create = Create(port)
00023                 self.packetPub = rospy.Publisher('sensorPacket', SensorPacket)
00024                 self.odomPub = rospy.Publisher('odom',Odometry)
00025                 self.odomBroadcaster = TransformBroadcaster()
00026                 self.fields = ['wheeldropCaster','wheeldropLeft','wheeldropRight','bumpLeft','bumpRight','wall','cliffLeft','cliffFronLeft','cliffFrontRight','cliffRight','virtualWall','infraredByte','advance','play','distance','angle','chargingState','voltage','current','batteryTemperature','batteryCharge','batteryCapacity','wallSignal','cliffLeftSignal','cliffFrontLeftSignal','cliffFrontRightSignal','cliffRightSignal','homeBase','internalCharger','songNumber','songPlaying']
00027                 self.then = datetime.now() 
00028                 self.x = 0
00029                 self.y = 0
00030                 self.th = 0
00031                 self.create.update = self.sense
00032                 self.docking = False
00033 
00034         def start(self):
00035                 self.create.start()
00036                 self.then = datetime.now() 
00037 
00038         def stop(self):
00039                 self.create.stop()
00040 
00041         def sense(self):
00042                 now = datetime.now()
00043                 elapsed = now - self.then
00044                 self.then = now
00045                 elapsed = float(elapsed.seconds) + elapsed.microseconds/1000000.
00046                 d = self.create.d_distance / 1000.
00047                 th = self.create.d_angle*pi/180
00048                 dx = d / elapsed
00049                 dth = th / elapsed
00050 
00051                 if (d != 0):
00052                         x = cos(th)*d
00053                         y = -sin(th)*d
00054                         self.x = self.x + (cos(self.th)*x - sin(self.th)*y)
00055                         self.y = self.y + (sin(self.th)*x + cos(self.th)*y)
00056 
00057                 if (th != 0):
00058                         self.th = self.th + th
00059 
00060                 quaternion = Quaternion()
00061                 quaternion.x = 0.0 
00062                 quaternion.y = 0.0
00063                 quaternion.z = sin(self.th/2)
00064                 quaternion.w = cos(self.th/2)
00065 
00066                 self.odomBroadcaster.sendTransform(
00067                         (self.x, self.y, 0), 
00068                         (quaternion.x, quaternion.y, quaternion.z, quaternion.w),
00069                         rospy.Time.now(),
00070                         "base_link",
00071                         "odom"
00072                         )
00073 
00074                 odom = Odometry()
00075                 odom.header.stamp = rospy.Time.now()
00076                 odom.header.frame_id = "odom"
00077                 odom.pose.pose.position.x = self.x
00078                 odom.pose.pose.position.y = self.y
00079                 odom.pose.pose.position.z = 0
00080                 odom.pose.pose.orientation = quaternion
00081 
00082                 odom.child_frame_id = "base_link"
00083                 odom.twist.twist.linear.x = dx
00084                 odom.twist.twist.linear.y = 0
00085                 odom.twist.twist.angular.z = dth
00086 
00087                 self.odomPub.publish(odom)
00088 
00089                 packet = SensorPacket()
00090                 for field in self.fields:
00091                         packet.__setattr__(field,self.create.__getattr__(field))
00092                 self.packetPub.publish(packet)
00093 
00094                 charge_level = float(packet.batteryCharge) / float(packet.batteryCapacity)
00095                 if (self.docking and packet.homeBase and charge_level > 0.95):
00096                         self.docking = False
00097                         self.create.reset()
00098                         rospy.sleep(1)
00099                         self.create.start()
00100                 elif (not self.docking and charge_level < self.autodock):
00101                         self.create.demo(1)
00102                         self.docking = True
00103 
00104         def brake(self,req):
00105                 if (req.brake):
00106                         self.create.brake()
00107                 return BrakeResponse(True)
00108 
00109         def circle(self,req):
00110                 if (req.clear):
00111                         self.create.clear()
00112                 self.create.forwardTurn(req.speed,req.radius)
00113                 return CircleResponse(True)
00114 
00115         def demo(self,req):
00116                 self.create.demo(req.demo)
00117                 return DemoResponse(True)
00118 
00119         def leds(self,req):
00120                 self.create.leds(req.advance,req.play,req.color,req.intensity)
00121                 return LedsResponse(True)
00122 
00123         def tank(self,req):
00124                 if (req.clear):
00125                         self.create.clear()
00126                 self.create.tank(req.left,req.right)
00127                 return TankResponse(True)
00128 
00129         def turn(self,req):
00130                 if (req.clear):
00131                         self.create.clear()
00132                 self.create.turn(req.turn)
00133                 return TurnResponse(True)
00134 
00135         def dock(self,req):
00136                 self.create.demo(1)
00137                 self.docking = True
00138                 return DockResponse(True)
00139 
00140         def twist(self,req):
00141                 x = req.linear.x*1000.
00142                 th = req.angular.z
00143                 if (x == 0):
00144                         th = th*180/pi
00145                         speed = (8*pi*th)/9
00146                         self.create.left(int(speed))
00147                 elif (th == 0):
00148                         x = int(x)
00149                         self.create.tank(x,x)
00150                 else:
00151                         self.create.forwardTurn(int(x),int(x/th))
00152 
00153 if __name__ == '__main__':
00154         node = rospy.init_node('create')
00155         driver = CreateDriver()
00156         
00157         rospy.Service('brake',Brake,driver.brake)
00158         rospy.Service('circle',Circle,driver.circle)
00159         rospy.Service('demo',Demo,driver.demo)
00160         rospy.Service('leds',Leds,driver.leds)
00161         rospy.Service('tank',Tank,driver.tank)
00162         rospy.Service('turn',Turn,driver.turn)
00163         rospy.Service('dock',Dock,driver.dock)
00164 
00165         rospy.Subscriber("cmd_vel", Twist, driver.twist)
00166 
00167         sleep(1)
00168         driver.start()
00169         sleep(1)
00170 
00171         rospy.spin()
00172         driver.stop()


irobot_create_2_1
Author(s): Graylin Trevor Jay, Brian Thomas
autogenerated on Sun Oct 5 2014 22:39:11