def __init__(self): port = rospy.get_param("/brown/irobot_create_2_1/port", "/dev/ttyUSB0") self.roomba = Roomba(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", "cliffFrontLeft", "cliffFrontRight", "cliffRight", "virtualWall", "overCurrentLeft", "overCurrentRight", "overCurrentMainBrush", "overCurrentSideBrush", "dirtDetectorLeft", "dirtDetectorRight", "virtualWall", "infraredByte", "button", "distance", "angle", "chargingState", "voltage", "current", "batteryTemperature", "batteryCharge", "batteryCapacity", ] self.then = datetime.now() self.x = 0 self.y = 0 self.th = 0 self.roomba.update = self.sense
class RoombaDriver: def __init__(self): port = rospy.get_param("/brown/irobot_create_2_1/port", "/dev/ttyUSB0") self.roomba = Roomba(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", "cliffFrontLeft", "cliffFrontRight", "cliffRight", "virtualWall", "overCurrentLeft", "overCurrentRight", "overCurrentMainBrush", "overCurrentSideBrush", "dirtDetectorLeft", "dirtDetectorRight", "virtualWall", "infraredByte", "button", "distance", "angle", "chargingState", "voltage", "current", "batteryTemperature", "batteryCharge", "batteryCapacity", ] self.then = datetime.now() self.x = 0 self.y = 0 self.th = 0 self.roomba.update = self.sense def start(self): self.roomba.start() self.then = datetime.now() def stop(self): self.roomba.stop() def sense(self): now = datetime.now() elapsed = now - self.then self.then = now elapsed = float(elapsed.seconds) + elapsed.microseconds / 1000000.0 d = self.roomba.d_distance / 1000.0 th = self.roomba.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.roomba.__getattr__(field)) self.packetPub.publish(packet) def brake(self, req): if req.brake: self.roomba.brake() return BrakeResponse(True) def demo(self, req): self.roomba.demo(req.demo) return DemoResponse(True) def leds(self, req): self.roomba.leds(req.advance, req.play, req.color, req.intensity) return LedsResponse(True) def twist(self, req): x = req.linear.x * 1000 omega = req.angular.z self.roomba.driveTwist(x, omega)