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