Esempio n. 1
0
class CreateDriver:
    def __init__(self):
        port = rospy.get_param('/brown/irobot_create_2_1/port', "/dev/ttyUSB0")
        self.create = Create(port)
        self.packetPub = rospy.Publisher('sensorPacket',
                                         SensorPacket,
                                         queue_size=10)
        self.odomPub = rospy.Publisher('odom', Odometry, queue_size=10)
        self.odomBroadcaster = TransformBroadcaster()
        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'
        ]
        self.then = datetime.now()
        self.x = 0
        self.y = 0
        self.th = 0
        self.create.update = self.sense

    def start(self):
        self.create.start()
        self.then = datetime.now()

    def stop(self):
        self.create.stop()

    def sense(self):
        now = datetime.now()
        elapsed = now - self.then
        self.then = now
        elapsed = float(elapsed.seconds) + elapsed.microseconds / 1000000.
        d = self.create.d_distance / 1000.
        th = self.create.d_angle * pi / 180
        dx = d / elapsed
        dth = th / elapsed

        if (d != 0):
            x = cos(th) * d
            y = -sin(th) * d
            self.x = self.x + (cos(self.th) * x - sin(self.th) * y)
            self.y = self.y + (sin(self.th) * x + cos(self.th) * y)

        if (th != 0):
            self.th = self.th + th

        quaternion = Quaternion()
        quaternion.x = 0.0
        quaternion.y = 0.0
        quaternion.z = sin(self.th / 2)
        quaternion.w = cos(self.th / 2)

        self.odomBroadcaster.sendTransform(
            (self.x, self.y, 0),
            (quaternion.x, quaternion.y, quaternion.z, quaternion.w),
            rospy.Time.now(), "base_link", "odom")

        odom = Odometry()
        odom.header.stamp = rospy.Time.now()
        odom.header.frame_id = "odom"
        odom.pose.pose.position.x = self.x
        odom.pose.pose.position.y = self.y
        odom.pose.pose.position.z = 0
        odom.pose.pose.orientation = quaternion

        odom.child_frame_id = "base_link"
        odom.twist.twist.linear.x = dx
        odom.twist.twist.linear.y = 0
        odom.twist.twist.angular.z = dth

        self.odomPub.publish(odom)

        packet = SensorPacket()
        for field in self.fields:
            packet.__setattr__(field, self.create.__getattr__(field))
        self.packetPub.publish(packet)

    def brake(self, req):
        if (req.brake):
            self.create.brake()
        return BrakeResponse(True)

    def circle(self, req):
        if (req.clear):
            self.create.clear()
        self.create.forwardTurn(req.speed, req.radius)
        return CircleResponse(True)

    def demo(self, req):
        self.create.demo(req.demo)
        return DemoResponse(True)

    def leds(self, req):
        self.create.leds(req.advance, req.play, req.color, req.intensity)
        return LedsResponse(True)

    def tank(self, req):
        if (req.clear):
            self.create.clear()
        self.create.tank(req.left, req.right)
        return TankResponse(True)

    def turn(self, req):
        if (req.clear):
            self.create.clear()
        self.create.turn(req.turn)
        return TurnResponse(True)

    def twist(self, req):
        x = req.linear.x * 1000.
        th = req.angular.z
        if (x == 0):
            th = th * 180 / pi
            speed = (8 * pi * th) / 9
            self.create.left(int(speed))
        elif (th == 0):
            x = int(x)
            self.create.tank(x, x)
        else:
            self.create.forwardTurn(int(x), int(x / th))
Esempio n. 2
0
class CreateDriver:
    def __init__(self):
        port = rospy.get_param('~port', "/dev/ttyUSB0")
        self.create = Create(port)
        self.packetPub = rospy.Publisher('sensorPacket', SensorPacket)
        self.odomPub = rospy.Publisher('odom', Odometry)
        self.odomBroadcaster = TransformBroadcaster()
        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', 'x', 'y', 'theta', 'chargeLevel'
        ]
        self.then = datetime.now()
        self.x = 0
        self.y = 0
        self.th = 0
        self.create.update = self.sense
        self.inDock = False
        self.chargeLevel = 0

    def start(self):
        self.create.start()
        self.then = datetime.now()

    def stop(self):
        self.create.stop()

    def sense(self):
        now = datetime.now()
        elapsed = now - self.then
        self.then = now
        elapsed = float(elapsed.seconds) + elapsed.microseconds / 1000000.
        d = self.create.d_distance / 1000.
        th = self.create.d_angle * pi / 180
        dx = d / elapsed
        dth = th / elapsed

        if (d != 0):
            x = cos(th) * d
            y = -sin(th) * d
            self.x = self.x + (cos(self.th) * x - sin(self.th) * y)
            self.y = self.y + (sin(self.th) * x + cos(self.th) * y)

        if (th != 0):
            self.th = self.th + th

        quaternion = Quaternion()
        quaternion.x = 0.0
        quaternion.y = 0.0
        quaternion.z = sin(self.th / 2)
        quaternion.w = cos(self.th / 2)

        self.odomBroadcaster.sendTransform(
            (self.x, self.y, 0),
            (quaternion.x, quaternion.y, quaternion.z, quaternion.w),
            rospy.Time.now(), "base_link", "odom")

        odom = Odometry()
        odom.header.stamp = rospy.Time.now()
        odom.header.frame_id = "odom"
        odom.pose.pose.position.x = self.x
        odom.pose.pose.position.y = self.y
        odom.pose.pose.position.z = 0
        odom.pose.pose.orientation = quaternion

        odom.child_frame_id = "base_link"
        odom.twist.twist.linear.x = dx
        odom.twist.twist.linear.y = 0
        odom.twist.twist.angular.z = dth

        self.odomPub.publish(odom)

        packet = SensorPacket()
        for field in self.fields[:-4]:
            packet.__setattr__(field, self.create.__getattr__(field))

        self.chargeLevel = float(packet.batteryCharge) / float(
            packet.batteryCapacity)
        packet.__setattr__('x', self.x)
        packet.__setattr__('y', self.y)
        packet.__setattr__('theta', self.th)
        packet.__setattr__('chargeLevel', self.chargeLevel)
        self.packetPub.publish(packet)

        if packet.homeBase:
            self.inDock = True
        else:
            self.inDock = False

    def circle(self, req):
        self.create.forwardTurn(req.speed, req.radius)
        return CircleResponse(True)

    def demo(self, req):
        self.create.demo(req.demo)
        return DemoResponse(True)

    def leds(self, req):
        self.create.leds(req.advance, req.play, req.color, req.intensity)
        return LedsResponse(True)

    def tank(self, req):
        self.create.tank(req.left, req.right)
        return TankResponse(True)

    def turn(self, req):
        if (req.clear):
            self.create.clear()
        self.create.turn(req.turn)
        return TurnResponse(True)

    def dock(self, req):
        """ req.charge = 0: dock and charge 
                       = 1: wake up from charging
                       = 2: dock and wake up immediately """

        if req.charge == 2:
            self.create.restart()
            return DockResponse(True)

        self.create.brake()
        self.create.demo(1)
        while (not self.inDock):
            pass

        if req.charge == 0:
            rospy.sleep(3)
            self.create.restart()
        return DockResponse(True)

    """ Added summer 2012 """

    def opCode(self, opCode):
        self.create.send(opCode)

    def twist(self, req):
        x = req.linear.x * 1000.
        th = req.angular.z
        print(x, th)
        if (x == 0):
            th = th * 180 / pi
            speed = (8 * pi * th) / 9
            self.create.left(int(speed))
        elif (th == 0):
            x = int(x)
            self.create.tank(x, x)
        else:
            self.create.forwardTurn(int(x), int(x / th))

    def song(self, req):
        self.create.playFullSong(req.notes, req.durations)
        return SongResponse(True)
Esempio n. 3
0
class CreateDriver:
	def __init__(self):
		port = rospy.get_param('/irobot/serial_port', "/dev/ttyAMA0")
		self.autodock = rospy.get_param('/irobot/autodock', 0.0)
		self.create = Create(port)
		self.packetPub = rospy.Publisher('sensorPacket', SensorPacket)
		self.odomPub = rospy.Publisher('odom',Odometry)
		self.odomBroadcaster = TransformBroadcaster()
		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']
		self.then = datetime.now() 
		self.x = 0
		self.y = 0
		self.th = 0
		self.create.update = self.sense
		self.docking = False

	def start(self):
		self.create.start()
		self.then = datetime.now() 

	def stop(self):
		self.create.stop()

	def sense(self):
		now = datetime.now()
		elapsed = now - self.then
		self.then = now
		elapsed = float(elapsed.seconds) + elapsed.microseconds/1000000.
		d = self.create.d_distance / 1000.
		th = self.create.d_angle*pi/180
		dx = d / elapsed
		dth = th / elapsed

		if (d != 0):
			x = cos(th)*d
			y = -sin(th)*d
			self.x = self.x + (cos(self.th)*x - sin(self.th)*y)
			self.y = self.y + (sin(self.th)*x + cos(self.th)*y)

		if (th != 0):
			self.th = self.th + th

		quaternion = Quaternion()
		quaternion.x = 0.0 
		quaternion.y = 0.0
		quaternion.z = sin(self.th/2)
		quaternion.w = cos(self.th/2)

		self.odomBroadcaster.sendTransform(
			(self.x, self.y, 0), 
			(quaternion.x, quaternion.y, quaternion.z, quaternion.w),
			rospy.Time.now(),
			"base_link",
			"odom"
			)

		odom = Odometry()
		odom.header.stamp = rospy.Time.now()
		odom.header.frame_id = "odom"
		odom.pose.pose.position.x = self.x
		odom.pose.pose.position.y = self.y
		odom.pose.pose.position.z = 0
		odom.pose.pose.orientation = quaternion

		odom.child_frame_id = "base_link"
		odom.twist.twist.linear.x = dx
		odom.twist.twist.linear.y = 0
		odom.twist.twist.angular.z = dth

		self.odomPub.publish(odom)

		packet = SensorPacket()
		for field in self.fields:
			packet.__setattr__(field,self.create.__getattr__(field))
		self.packetPub.publish(packet)

		charge_level = float(packet.batteryCharge) / float(packet.batteryCapacity)
		if (self.docking and packet.homeBase and charge_level > 0.95):
			self.docking = False
			self.create.reset()
			rospy.sleep(1)
			self.create.start()
		elif (not self.docking and charge_level < self.autodock):
			self.create.demo(1)
			self.docking = True

	def brake(self,req):
		if (req.brake):
			self.create.brake()
		return BrakeResponse(True)

	def circle(self,req):
		if (req.clear):
			self.create.clear()
		self.create.forwardTurn(req.speed,req.radius)
		return CircleResponse(True)

	def demo(self,req):
		self.create.demo(req.demo)
		return DemoResponse(True)

	def leds(self,req):
		self.create.leds(req.advance,req.play,req.color,req.intensity)
		return LedsResponse(True)

	def tank(self,req):
		if (req.clear):
			self.create.clear()
		self.create.tank(req.left,req.right)
		return TankResponse(True)

	def turn(self,req):
		if (req.clear):
			self.create.clear()
		self.create.turn(req.turn)
		return TurnResponse(True)

	def dock(self,req):
		self.create.demo(1)
		self.docking = True
		return DockResponse(True)

	def twist(self,req):
		x = req.linear.x*1000.
		th = req.angular.z
		if (x == 0):
			th = th*180/pi
			speed = (8*pi*th)/9
			self.create.left(int(speed))
		elif (th == 0):
			x = int(x)
			self.create.tank(x,x)
		else:
			self.create.forwardTurn(int(x),int(x/th))
Esempio n. 4
0
class discreteCreateDriver:
    def __init__(self):
        print("init")
        port = rospy.get_param('/brown/irobot_create_2_1/port', "/dev/rfcomm0")
        self.create = Create(port)

        self.bump = False
        self.hz = 30
        self.rate = 1. / self.hz
        self.speed = 200
        self.turn = 50
        self.distance = 85
        self.threshold = .5
        self.angle = 18
        self.lock = Lock()
        self.packetPub = rospy.Publisher('sensorPacket', SensorPacket)

        self.odomPub = rospy.Publisher('odom', Odometry)
        self.odomBroadcaster = TransformBroadcaster()
        self.then = datetime.now()
        self.x = 0
        self.y = 0
        self.th = 0
        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'
        ]

        self.create.update = self.sense

    def start(self):
        self.create.start()
        self.then = datetime.now()
        self.norms = [
            self.create.cliffLeftSignal, self.create.cliffFrontLeftSignal,
            self.create.cliffFrontRightSignal, self.create.cliffRightSignal
        ]

    def stop(self):
        self.create.stop()

    def sense(self):
        print("sensing\n")
        now = datetime.now()
        elapsed = now - self.then
        self.then = now
        elapsed = float(elapsed.seconds) + elapsed.microseconds / 1000000.
        d = self.create.d_distance / 1000.
        th = self.create.d_angle * pi / 180
        dx = d / elapsed
        dth = th / elapsed

        if (d != 0):
            x = cos(th) * d
            y = -sin(th) * d
            self.x = self.x + (cos(self.th) * x - sin(self.th) * y)
            self.y = self.y + (sin(self.th) * x + cos(self.th) * y)

        if (th != 0):
            self.th = self.th + th

        quaternion = Quaternion()
        quaternion.x = 0.0
        quaternion.y = 0.0
        quaternion.z = sin(self.th / 2)
        quaternion.w = cos(self.th / 2)

        self.odomBroadcaster.sendTransform(
            (self.x, self.y, 0),
            (quaternion.x, quaternion.y, quaternion.z, quaternion.w),
            rospy.Time.now(), "base_link", "odom")

        odom = Odometry()
        odom.header.frame_id = "odom"
        odom.pose.pose.position.x = self.x
        odom.pose.pose.position.y = self.y
        odom.pose.pose.position.z = 0
        odom.pose.pose.orientation = quaternion

        odom.child_frame_id = "base_link"
        odom.twist.twist.linear.x = dx
        odom.twist.twist.linear.y = 0
        odom.twist.twist.angular.z = dth

        self.odomPub.publish(odom)
        packet = SensorPacket()
        for field in self.fields:
            packet.__setattr__(field, self.create.__getattr__(field))
        self.packetPub.publish(packet)

    def collision(self):
        #global create, norms, threshold
        sensors = [
            self.create.cliffLeftSignal, self.create.cliffFrontLeftSignal,
            self.create.cliffFrontRightSignal, self.create.cliffRightSignal
        ]
        # the first paren after the colon should eventually be an abs, we removed it so that the dirty (formerly taped spots) could be
        # properly ignored        h <-- abs under the h
        if (len(
                filter(
                    lambda x:
                    (float(x[0]) - float(x[1])) / float(x[1]) > self.threshold,
                    zip(sensors, self.norms))) or self.create.bumpLeft
                or self.create.bumpRight):
            return True
        else:
            return False

    def act(self, action):
        #global bump, rate, lock, speed, turn, create, distance, angle, norms, threshold

        if (not self.lock.acquire(0)):
            return

        move = action.action

        if (move == 1):
            self.create.clear()
            while (self.create.distance != 0):
                sleep(self.rate)
            self.create.tank(self.speed, self.speed)
            while (self.create.distance < self.distance):
                #print map(lambda x: abs(float(x[0])-float(x[1]))/float(x[0]),zip(sensors,norms))
                if (self.collision()):
                    self.create.tank(-self.speed, -self.speed)
                    while (self.create.distance > 0):
                        sleep(self.rate)
                    break
                sleep(self.rate)

        elif (move == 0 or move == 2):
            self.create.clear()
            while (self.create.angle != 0):
                sleep(self.rate)

            toPos = 1
            if (move == 0):
                toPos = -toPos
            self.create.turn(self.turn * toPos)

            if (not self.collision()):
                while (self.create.angle * toPos * -1 < self.angle):
                    if (self.collision()):
                        break
                    sleep(self.rate)
            else:
                while (self.collision()):
                    sleep(self.rate)

        self.create.brake()

        sleep(self.rate * 10)

        self.lock.release()

        return ActResponse(True)
Esempio n. 5
0
class discreteCreateDriver:
   def __init__(self):
       print("init")
       port = rospy.get_param('/brown/irobot_create_2_1/port', "/dev/rfcomm0")
       self.create=Create(port)

       self.bump=False
       self.hz=30
       self.rate = 1. / self.hz
       self.speed = 200
       self.turn = 50
       self.distance = 85
       self.threshold = .5
       self.angle = 18
       self.lock = Lock()
       self.packetPub = rospy.Publisher('sensorPacket', SensorPacket)

       self.odomPub=rospy.Publisher('odom', Odometry)
       self.odomBroadcaster=TransformBroadcaster()
       self.then=datetime.now()
       self.x=0
       self.y=0
       self.th=0
       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']
       
       self.create.update = self.sense

   def start(self):
       self.create.start()
       self.then=datetime.now()
       self.norms = [self.create.cliffLeftSignal, self.create.cliffFrontLeftSignal, self.create.cliffFrontRightSignal, self.create.cliffRightSignal]

   def stop(self):
       self.create.stop()
       
   def sense(self):
       print("sensing\n")	   
       now = datetime.now()
       elapsed = now - self.then
       self.then = now
       elapsed = float(elapsed.seconds) + elapsed.microseconds/1000000.
       d = self.create.d_distance / 1000.
       th = self.create.d_angle*pi/180
       dx = d / elapsed
       dth = th / elapsed

       if (d != 0):
	       x = cos(th)*d
	       y = -sin(th)*d
	       self.x = self.x + (cos(self.th)*x - sin(self.th)*y)
	       self.y = self.y + (sin(self.th)*x + cos(self.th)*y)

       if (th != 0):
	       self.th = self.th + th

       quaternion = Quaternion()
       quaternion.x = 0.0 
       quaternion.y = 0.0
       quaternion.z = sin(self.th/2)
       quaternion.w = cos(self.th/2)
       
       self.odomBroadcaster.sendTransform(
	       (self.x, self.y, 0), 
	       (quaternion.x, quaternion.y, quaternion.z, quaternion.w),
	       rospy.Time.now(),
	       "base_link",
	       "odom"
	       )

       odom = Odometry()
       odom.header.frame_id = "odom"
       odom.pose.pose.position.x = self.x
       odom.pose.pose.position.y = self.y
       odom.pose.pose.position.z = 0
       odom.pose.pose.orientation = quaternion
       
       odom.child_frame_id = "base_link"
       odom.twist.twist.linear.x = dx
       odom.twist.twist.linear.y = 0
       odom.twist.twist.angular.z = dth
       
       self.odomPub.publish(odom)
       packet = SensorPacket()
       for field in self.fields:
	       packet.__setattr__(field,self.create.__getattr__(field))
       self.packetPub.publish(packet)


   def collision(self):
       #global create, norms, threshold
       sensors = [self.create.cliffLeftSignal, self.create.cliffFrontLeftSignal, self.create.cliffFrontRightSignal, self.create.cliffRightSignal]
       # the first paren after the colon should eventually be an abs, we removed it so that the dirty (formerly taped spots) could be 
       # properly ignored        h <-- abs under the h
       if (len(filter(lambda x : (float(x[0])-float(x[1]))/float(x[1]) > self.threshold,zip(sensors,self.norms))) or self.create.bumpLeft or self.create.bumpRight):
	       return True
       else:
	       return  False

   def act(self, action):
       #global bump, rate, lock, speed, turn, create, distance, angle, norms, threshold
	   
	   if (not self.lock.acquire(0)):
		   return
	   
	   move = action.action
	   
	   if (move == 1):
		   self.create.clear()
		   while(self.create.distance != 0):
			   sleep(self.rate)
		   self.create.tank(self.speed,self.speed)
		   while(self.create.distance < self.distance):
			#print map(lambda x: abs(float(x[0])-float(x[1]))/float(x[0]),zip(sensors,norms))
			   if (self.collision()):
				   self.create.tank(-self.speed,-self.speed)
				   while(self.create.distance > 0):
					   sleep(self.rate)
				   break
			   sleep(self.rate)

	   elif (move == 0 or move == 2):
		   self.create.clear()
		   while(self.create.angle != 0):
			   sleep(self.rate)

		   toPos = 1
		   if (move == 0):
			   toPos = -toPos
		   self.create.turn(self.turn*toPos)

		   if (not self.collision()):
			   while(self.create.angle*toPos*-1 < self.angle):
				   if (self.collision()):
					   break
				   sleep(self.rate)
		   else:
			   while(self.collision()):
				   sleep(self.rate)

	   self.create.brake()

	   sleep(self.rate*10)

	   self.lock.release()
	
	   return ActResponse(True)
Esempio n. 6
0
class CreateDriver:
	def __init__(self):
		# If /create/use_host_name is True, the topic names used by
		# this driver will be /hostname/cmd_vel, /hostname/odom, etc.
		self.hn = HName(usename = rospy.get_param('/create/use_host_name', False))
		port = rospy.get_param('/brown/irobot_create_2_1/port', "/dev/ttyUSB0")
		self.create = Create(port)
		self.packetPub = rospy.Publisher(self.hn.topic('sensorPacket'), SensorPacket)
		self.odomPub = rospy.Publisher(self.hn.topic('odom'),Odometry)
		self.odomBroadcaster = TransformBroadcaster()
		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']
		self.then = datetime.now()
		self.x = 0
		self.y = 0
		self.th = 0
		self.idealTh = 0
		self.create.update = self.sense

		# SW
		self.create.storeSong(2,67,32,74,32,72,11,71,11,69,11,79,32,74,32,72,11,71,11,69,11,79,32,74,32,72,11,71,11,72,11,69,32)
		# CE3K
		self.create.storeSong(3,74,16,81,24,83,24,79,24,67,32,74,40)
		# HN
		self.create.storeSong(4,74,64,78,20,75,20,74,20,78,56,81,17,79,17,78,17,79,48,82,14,81,14,79,14,78,28,75,32,74,40)
		# Js
		self.create.storeSong(5,42,32,43,32,42,32,43,32,42,32,43,32,42,32,43,32)
		# ROLA
		self.create.storeSong(6,64,24,65,8,67,16,72,40,62,24,64,8,65,48,67,24,69,8,71,16,77,40,69,24,71,10,72,24,74,24,76,48)
		# JJMD
		self.create.storeSong(7,79,12,81,12,83,12,86,12,84,12,84,12,88,12,86,12,86,12,91,12,90,12,91,12,86,12,83,12,79,40)
		# SaaHC
		self.create.storeSong(8,84,25,79,18,79,12,81,25,79,40,83,28,84,12)
		# Angry men
		self.create.storeSong(9,64,24,62,12,60,24,62,12,64,24,65,12,67,36,64,12,62,12,60,12,59,24,57,12,59,24,60,12,55,48)
		# IGR
		self.create.storeSong(10,77,24,79,24,82,24,84,40)
#,84,24,82,24,79,24,77,40,77,24,79,24,82,24,84,18,87,18,84,10,86,32,84,42)

		self.create.storeSong(11,50,10,51,10)
		self.create.storeSong(12,51,10,50,10)
		self.create.storeSong(13,52,10,51,10,50,10)
		self.create.storeSong(14,53,10,54,10,55,10)
		self.create.storeSong(15,54,10,54,10)


	def start(self,req = True):
		if (req == True or req.start):
			self.create.start()
			self.then = datetime.now()
		return StartResponse(True)

	def stop(self,req = True):
		if (req == True or req.stop):
			self.create.stop()
		return StopResponse(True)

	def sense(self):
		now = datetime.now()
		elapsed = now - self.then
		self.then = now
		elapsed = float(elapsed.seconds) + elapsed.microseconds/1000000.
		d = self.create.d_distance / 1000.
		th = self.create.d_angle*pi/180
		dx = d / elapsed
		dth = th / elapsed

		if (abs(self.idealTh) < .5):
			dth = self.idealTh

		if (d != 0):
			x = cos(th)*d
			y = sin(th)*d  # TODO: Question this negative sign?
			self.x = self.x + (cos(self.th)*x - sin(self.th)*y)
			self.y = self.y + (sin(self.th)*x + cos(self.th)*y)

		if (th != 0):
			self.th = self.th + th

		quaternion = Quaternion()
		quaternion.x = 0.0
		quaternion.y = 0.0
		quaternion.z = sin(self.th/2)
		quaternion.w = cos(self.th/2)

		self.odomBroadcaster.sendTransform(
			(self.x, self.y, 0),
			(quaternion.x, quaternion.y, quaternion.z, quaternion.w),
			rospy.Time.now(),
			"base_link",
			"odom"
			)

		odom = Odometry()
		odom.header.stamp = rospy.Time.now()
		odom.header.frame_id = "odom"
		odom.pose.pose.position.x = self.x
		odom.pose.pose.position.y = self.y
		odom.pose.pose.position.z = 0
		odom.pose.pose.orientation = quaternion

		odom.child_frame_id = "base_link"
		odom.twist.twist.linear.x = dx
		odom.twist.twist.linear.y = 0
		odom.twist.twist.angular.z = dth

		self.odomPub.publish(odom)

		packet = SensorPacket()
		for field in self.fields:
			packet.__setattr__(field,self.create.__getattr__(field))
		self.packetPub.publish(packet)

	def brake(self,req):
		if (req.brake):
			self.create.brake()
		return BrakeResponse(True)

	def reset(self,req):
		if (req.reset):
			self.create.reset()
		return ResetResponse(True)

	def circle(self,req):
		if (req.clear):
			self.create.clear()
		self.create.forwardTurn(req.speed,req.radius)
		return CircleResponse(True)

	def demo(self,req):
		self.create.demo(req.demo)
		return DemoResponse(True)

	def leds(self,req):
		self.create.leds(req.advance,req.play,req.color,req.intensity)
		return LedsResponse(True)

	def loadsong(self,req):
		self.create.storeSong(req.songNumber, req.song)
		return SongResponse(True)

	def playsong(self,req):
		self.create.playSong(req.songNumber)
		return PlaySongResponse(True)

	def tank(self,req):
		if (req.clear):
			self.create.clear()
		self.create.tank(req.left,req.right)
		return TankResponse(True)

	def turn(self,req):
		if (req.clear):
			self.create.clear()
		self.create.turn(req.turn)
		return TurnResponse(True)

	def twist(self,req):
		x = req.linear.x * 1000. # ie, linear speed passed in as m/s, internally mm/s
		th = req.angular.z # angular speed in rad/s
		if (th == 0):
			x = int(x)
			self.create.tank(x,x)
		else:
			distanceBetweenWheels = 260 # mm (CONSTANT)
			wheelDistanceFromCenter = distanceBetweenWheels / 2
			turnRadius = x/th # from perspective of robot center
			lwSpeed=int((turnRadius-wheelDistanceFromCenter)*th)
			rwSpeed=int((turnRadius+wheelDistanceFromCenter)*th)
			self.create.tank(lwSpeed,rwSpeed)