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)
