class Bridge():
def __init__(self):
serial_device = rospy.get_param("~device", "/dev/ttyAMA0")
self.comm = SerialComm(serial_device)
self.comm.connect()
self.srv_lock = Lock()
self.motor_sub = rospy.Subscriber("~motors", MotorPayload,
self.set_motors)
self.battery_srv = rospy.Service("~get_battery", GetBattery,
self.get_battery)
self.firmware_ver_srv = rospy.Service("~get_firmware_ver",
GetFirmwareVer,
self.get_firmware_ver)
def set_motors(self, data):
f = frame.motors(data.payload)
self.comm.send(f)
def get_battery(self, data):
with self.srv_lock:
self.comm.send(frame.battery())
status = self.comm.serial.read(1)
if not status:
success = False
rospy.logerr("Could not get battery status")
battery_status = 0
else:
success = True
battery_status = struct.unpack(">B", status)[0]
return GetBatteryResponse(success, battery_status)
def get_firmware_ver(self, data):
with self.srv_lock:
self.comm.send(frame.firmware_ver())
firmware_ver = self.comm.readline()
if not firmware_ver:
success = False
rospy.logerr("Could not get firmware version")
else:
success = True
return GetFirmwareVerResponse(success, firmware_ver)
class Bridge():
def __init__(self):
serial_device = rospy.get_param("~device", "/dev/ttyAMA0")
self.comm = SerialComm(serial_device)
self.comm.connect()
self.lock = Lock()
self.motor_sub = rospy.Subscriber("~motors", MotorPower,
self.set_motors)
self.servo_sub = rospy.Subscriber("~servo", ServoAngle, self.set_servo)
self.battery_srv = rospy.Service("~get_battery", GetBattery,
self.get_battery)
self.firmware_ver_srv = rospy.Service("~get_firmware_ver",
GetFirmwareVer,
self.get_firmware_ver)
def set_motors(self, data):
payload = []
for p in data.power:
p = max(min(p, 1.0), -1.0)
value = int(round(p * 0x7F))
if value < 0:
value = -value + 0x7F
payload.append(value)
with self.lock:
self.comm.serial.flushInput()
f = frame.motors(payload)
self.comm.send(f)
status = self.comm.readline()
if not status or not status == " OK \r\n":
rospy.logerr(
"Did not receive a valid response after motor command")
def set_servo(self, data):
if data.channel not in [1, 2, 3]:
rospy.logerr(("Wrong servo channel! Received {0}, "
"expected 1, 2 or 3".format(data.channel)))
return
value = int((data.angle / 180.0) * 3450 + 1300)
duty = struct.pack(">H", value)
f = frame.servo(data.channel, duty)
with self.lock:
self.comm.serial.flushInput()
self.comm.send(f)
status = self.comm.readline()
if not status or not status == " OK \r\n":
rospy.logerr(
"Did not receive a valid response after servo command")
def get_battery(self, data):
with self.lock:
self.comm.serial.flushInput()
self.comm.send(frame.battery())
#status = self.comm.readline()
status = self.comm.serial.read(4)
if not status: #or not status.endswith("\r\n"):
success = False
rospy.logerr("Could not get battery status")
battery_status = 0
else:
success = True
battery_status = struct.unpack("<f", status[:4])[0]
return GetBatteryResponse(success, battery_status)
def get_firmware_ver(self, data):
with self.lock:
self.comm.serial.flushInput()
self.comm.send(frame.firmware_ver())
firmware_ver = self.comm.readline()
if not firmware_ver or not firmware_ver.endswith("\r\n"):
success = False
rospy.logerr("Could not get firmware version")
else:
success = True
firmware_ver = firmware_ver[:-2]
return GetFirmwareVerResponse(success, firmware_ver)
