def __init__(self):
self.serial_device = rospy.get_param("~device", "/dev/ttyAMA0")
self.battery_pub_rate = rospy.get_param("~battery_pub_rate", 1.0)
servo1_min_angle = rospy.get_param("~servo1_min_angle", -90)
servo1_max_angle = rospy.get_param("~servo1_max_angle", 90)
servo1_min_duty = rospy.get_param("~servo1_min_duty", 2400)
servo1_max_duty = rospy.get_param("~servo1_max_duty", 4800)
servo2_min_angle = rospy.get_param("~servo2_min_angle", -90)
servo2_max_angle = rospy.get_param("~servo2_max_angle", 90)
servo2_min_duty = rospy.get_param("~servo2_min_duty", 2400)
servo2_max_duty = rospy.get_param("~servo2_max_duty", 4800)
servo3_min_angle = rospy.get_param("~servo3_min_angle", -90)
servo3_max_angle = rospy.get_param("~servo3_max_angle", 90)
servo3_min_duty = rospy.get_param("~servo3_min_duty", 2400)
servo3_max_duty = rospy.get_param("~servo3_max_duty", 4800)
self.comm = SerialComm(self.serial_device)
reset_STM()
self.comm.connect()
self.motor_sub = rospy.Subscriber("~motors", Float32MultiArray,
self.set_motors)
self.servo1_sub = rospy.Subscriber(
"servo1/angle", Int16,
self.get_servo_callback(1, servo1_min_angle, servo1_max_angle,
servo1_min_duty, servo1_max_duty))
self.servo2_sub = rospy.Subscriber(
"servo2/angle", Int16,
self.get_servo_callback(2, servo2_min_angle, servo2_max_angle,
servo2_min_duty, servo2_max_duty))
self.servo3_sub = rospy.Subscriber(
"servo3/angle", Int16,
self.get_servo_callback(3, servo3_min_angle, servo3_max_angle,
servo3_min_duty, servo3_max_duty))
self.firmware_ver_pub = rospy.Publisher("~firmware_version",
String,
latch=True,
queue_size=1)
self.battery_pub = rospy.Publisher("battery", Float32, queue_size=1)
self.publish_firmware_ver()
self.publish_battery_thread = Thread(target=self.publish_battery_loop)
self.publish_battery_thread.daemon = True
self.publish_battery_thread.start()
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)
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 write(self, data, waitForResponse=True, timeout=1, byteCount=0):
'''
Write Data to PiArm
'''
self.log.debug('write: %s', data)
responseLines = SerialComm.write(self,
data,
waitForResponse=waitForResponse,
timeout=timeout,
byteCount=byteCount)
return responseLines
def write(self,
data,
waitForResponse=True,
writeTerm='\r',
timeout=5,
expectedResponse=None):
""" Write Data to pitalk Modem """
self.log.debug('write: %s', data)
responseLines = SerialComm.write(self,
data + writeTerm,
waitForResponse=waitForResponse,
timeout=timeout,
response=expectedResponse)
return responseLines
class WebcamBot(JabberBot):
def __init__(self,username,password):
JabberBot.__init__(self,username,password,debug=False)
self.serialComm=SerialComm()
@botcmd
def echo(self, mess, args):
"""Echoes the command verbatim."""
print "DBG", args
return "echo "+args
@botcmd
def pan(self, mess, angle):
"""Pans given angle. Negative left, positive right."""
print "pan"
return self.serialComm.sendAndWaitForReply("h %d"%int(angle))
@botcmd
def tilt(self, mess, angle):
"""Pans given angle. Negative down, positive up."""
print "TILT"
return self.serialComm.sendAndWaitForReply("v %d"%int(angle))
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)
def __init__(self,username,password):
JabberBot.__init__(self,username,password,debug=False)
self.serialComm=SerialComm()
class Bridge():
def __init__(self):
self.serial_device = rospy.get_param("~device", "/dev/ttyAMA0")
self.battery_pub_rate = rospy.get_param("~battery_pub_rate", 1.0)
servo1_min_angle = rospy.get_param("~servo1_min_angle", -90)
servo1_max_angle = rospy.get_param("~servo1_max_angle", 90)
servo1_min_duty = rospy.get_param("~servo1_min_duty", 2400)
servo1_max_duty = rospy.get_param("~servo1_max_duty", 4800)
servo2_min_angle = rospy.get_param("~servo2_min_angle", -90)
servo2_max_angle = rospy.get_param("~servo2_max_angle", 90)
servo2_min_duty = rospy.get_param("~servo2_min_duty", 2400)
servo2_max_duty = rospy.get_param("~servo2_max_duty", 4800)
servo3_min_angle = rospy.get_param("~servo3_min_angle", -90)
servo3_max_angle = rospy.get_param("~servo3_max_angle", 90)
servo3_min_duty = rospy.get_param("~servo3_min_duty", 2400)
servo3_max_duty = rospy.get_param("~servo3_max_duty", 4800)
self.comm = SerialComm(self.serial_device)
reset_STM()
self.comm.connect()
self.motor_sub = rospy.Subscriber("~motors", Float32MultiArray,
self.set_motors)
self.servo1_sub = rospy.Subscriber(
"servo1/angle", Int16,
self.get_servo_callback(1, servo1_min_angle, servo1_max_angle,
servo1_min_duty, servo1_max_duty))
self.servo2_sub = rospy.Subscriber(
"servo2/angle", Int16,
self.get_servo_callback(2, servo2_min_angle, servo2_max_angle,
servo2_min_duty, servo2_max_duty))
self.servo3_sub = rospy.Subscriber(
"servo3/angle", Int16,
self.get_servo_callback(3, servo3_min_angle, servo3_max_angle,
servo3_min_duty, servo3_max_duty))
self.firmware_ver_pub = rospy.Publisher("~firmware_version",
String,
latch=True,
queue_size=1)
self.battery_pub = rospy.Publisher("battery", Float32, queue_size=1)
self.publish_firmware_ver()
self.publish_battery_thread = Thread(target=self.publish_battery_loop)
self.publish_battery_thread.daemon = True
self.publish_battery_thread.start()
def set_motors(self, msg):
if len(msg.data) < 4:
rospy.logerr("Wrong array size in motor command")
return
payload = []
for p in msg.data:
value = power_to_motor_payload(p)
payload.append(value)
f = frame.motors(payload)
status = self.comm.proccess_command(f)
if not status or not status == " OK \r\n":
rospy.logerr(
"Did not receive a valid response after a motor command")
def get_servo_callback(self, channel, min_angle, max_angle, min_duty,
max_duty):
def set_servo(msg):
angle = msg.data
duty = servo_angle_to_duty(angle, min_angle, max_angle, min_duty,
max_duty)
f = frame.servo(channel, duty)
status = self.comm.proccess_command(f)
if not status or not status == " OK \r\n":
rospy.logerr(
"Did not receive a valid response after servo command")
return set_servo
def publish_firmware_ver(self):
firmware_ver = self.comm.proccess_command(frame.firmware_ver())
if not firmware_ver or not firmware_ver.endswith("\r\n"):
rospy.logerr("Could not get firmware version")
else:
firmware_ver = firmware_ver[:-2]
self.firmware_ver_pub.publish(firmware_ver)
def publish_battery(self):
status = self.comm.proccess_command(frame.battery())
if not status or not status.endswith("\r\n"):
rospy.logerr("Could not get battery status")
else:
battery_status = struct.unpack("<f", status[:4])[0]
self.battery_pub.publish(battery_status)
def publish_battery_loop(self):
rate = rospy.Rate(self.battery_pub_rate)
while not rospy.is_shutdown():
self.publish_battery()
rate.sleep()
def __init__(self):
SerialComm.__init__(self)
Commands.__init__(self, self.debug)
serial_interface = sys.argv[1]
image_file = sys.argv[2]
colorduinos_horizontal = int(sys.argv[3])
colorduinos_vertical = int(sys.argv[4])
colorduino_x = int(sys.argv[5])
colorduino_y = int(sys.argv[6])
except Exception:
if(len(sys.argv) != 7):
print "Usage: python main.py SERIAL_INTERFACE IMAGE_FILE COLORDUINOS_HORIZONTAL COLORDUINOS_VERTICAL COLORDUINO_X COLORDUINO_Y"
sys.exit()
im=ImageReader(sys.argv[2])
im.resize(colorduinos_horizontal*8,colorduinos_vertical*8)
im.load()
ser = SerialComm(serial_interface)
time.sleep(2)
for x in range(0,8):
for y in range(0,8):
rgb = im.getColorAt(colorduino_x*8 + x, colorduino_y*8 + y)
r = rgb[0]
g = rgb[1]
b = rgb[2]
print "x: %i, y: %i" % (int(colorduino_x)*8+x, int(colorduino_y*8)+y)
line = "%02X,%02X,%02X,%02X,%02X" % (y,x,r,g,b)
ser.writeln(line)
print line
time.sleep(7)
ser.close()