00001
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()
