class PortPzh:
def __init__(self, sub_addr, object_port):
self.dev = MsgDevice()
self.dev.open()
self.dev.sub_connect(sub_addr + ':' + object_port)
self.dev.sub_add_url("det.data", [-100] * 7) # -100表示信号丢失，-99表示程序断了。
def receive(self):
data = self.dev.sub_get("det.data")
# assert isinstance(data, list), "data should be a list"
assert len(
data
) == 7, "data should have 3*2 position and 1 target, totally 7 numbers"
ret = {
"Object 0":
[data[0], data[1]] if data[0] not in [-100, -99] else None,
"Object 1":
[data[2], data[3]] if data[2] not in [-100, -99] else None,
"Object 2":
[data[4], data[5]] if data[4] not in [-100, -99] else None,
"target":
"Object {}".format(int(data[6]))
if int(data[6]) in [0, 1, 2] else None,
"terminated":
all([d == -99 for d in data])
}
return ret
def main():
arduinoSer = serial.serial_for_url(arduinoUrl,
baudrate=115200,
do_not_open=True,
timeout=1)
dev = MsgDevice()
dev.open()
dev.pub_bind(msgPubBind)
dev.sub_connect(msgSubConnect)
dev.sub_add_url('motorSpeed')
dev.sub_add_url('maxPower')
for i in range(7):
dev.sub_add_url('rudder' + str(i) + 'Ang')
t = PeriodTimer(0.1)
t.start()
try:
try:
arduinoSer.open()
except serial.serialutil.SerialException, e:
arduinoSer.close()
print e, ", trying reconnet..."
while True:
with t:
# data = subFromVeristand(dev)
data = [140, 10, 130, 50, 50, 130, 50, 130, 50]
print data
if not arduinoSer._isOpen:
try:
arduinoSer.open()
except serial.serialutil.SerialException, e:
print e, ", trying reconnect..."
try:
if arduinoSer._isOpen:
arduinoSer.write(dataSend(data))
dataFromArduino = dataRead(arduinoSer)
print dataFromArduino
except serial.serialutil.SerialException, e:
arduinoSer.close()
try:
if dataFromArduino:
pubToVeristand(dev, dataFromArduino)
except UnboundLocalError:
pass
def main():
# arduinoSocket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
arduinoSer = serial.serial_for_url(
arduinoUrl, baudrate=115200, do_not_open=True, timeout=1)
dev = MsgDevice()
dev.open()
dev.pub_bind(msgPubBind)
dev.sub_connect(msgSubConnect)
dev.sub_add_url('motorSpeed')
dev.sub_add_url('rudderAng')
dev.sub_add_url('sailAng')
t = PeriodTimer(0.1)
t.start()
try:
try:
arduinoSer.open()
except serial.serialutil.SerialException, e:
arduinoSer.close()
print e, ", trying reconnet..."
while True:
with t:
data = subFromVeristand(dev)
# data = [100, 90, 0]
print "SEND: ", data
if not arduinoSer._isOpen:
try:
arduinoSer.open()
except serial.serialutil.SerialException, e:
print e, ", trying reconnect..."
try:
if arduinoSer._isOpen:
arduinoSer.write(dataSend(data))
dataFromArduino = dataRead(arduinoSer)
print "RECV: ", dataFromArduino
except serial.serialutil.SerialException, e:
arduinoSer.close()
try:
if dataFromArduino:
pubToVeristand(dev, dataFromArduino)
except UnboundLocalError:
pass
class Interface_rec(object):
def __init__(self, sub_addr, pose_port):
self.dev = MsgDevice()
self.dev.open()
self.dev.sub_connect(sub_addr + ':' + pose_port)
self.dev.sub_add_url('posx')
self.dev.sub_add_url('posy')
self.dev.sub_add_url('speed')
self.dev.sub_add_url('speed_tar')
self.dev.sub_add_url('yaw')
self.dev.sub_add_url('yaw_tar')
self.dev.sub_add_url('yawspd')
self.dev.sub_add_url('yawspd_tar')
def receive(self, *args):
data = []
for i in args:
data.append(self.dev.sub_get1(i))
return data
class Interface(object):
def __init__(self, sub_addr, ahrs_port, gnss_port, motor_port):
self.dev = MsgDevice()
self.dev.open()
self.dev.sub_connect(sub_addr + ':' + ahrs_port)
self.dev.sub_add_url('ahrs.roll')
self.dev.sub_add_url('ahrs.pitch')
self.dev.sub_add_url('ahrs.yaw')
self.dev.sub_add_url('ahrs.roll_speed')
self.dev.sub_add_url('ahrs.pitch_speed')
self.dev.sub_add_url('ahrs.yaw_speed')
self.dev.sub_add_url('ahrs.acce_x')
self.dev.sub_add_url('ahrs.acce_y')
self.dev.sub_add_url('ahrs.acce_z')
self.dev.sub_connect(sub_addr + ':' + gnss_port)
self.dev.sub_add_url('gps.time')
self.dev.sub_add_url('gps.posx')
self.dev.sub_add_url('gps.posy')
self.dev.sub_add_url('gps.posz')
self.dev.sub_add_url('gps.stdx')
self.dev.sub_add_url('gps.stdy')
self.dev.sub_add_url('gps.stdz')
self.dev.sub_add_url('gps.satn')
self.dev.sub_add_url('gps.hspeed')
self.dev.sub_add_url('gps.vspeed')
self.dev.sub_add_url('gps.track')
def receive(self, *args):
data = []
for i in args:
data.append(self.dev.sub_get1(i))
return data
class MIO_TRANS():
def __init__(self):
self.dev_connect_flag = 0
self.datashow_count = 0
self.logger = console_logger('MIO-5251')
self.dev_init()
self.ser_init()
self.Servo_init()
self.fst = struct.Struct('!4H')
def dev_init(self):
self.jsdev = MsgDevice()
self.dev = MsgDevice()
self.tmdev = MsgDevice()
self.jsdev.open()
self.dev.open()
self.tmdev.open()
try:
self.tmdev.sub_connect('tcp://127.0.0.1:55006')
self.logger.info('tmdev sub connect tcp://127.0.0.1:55006')
if Local_test is True:
self.dev.sub_connect(MSG_SUB_CONNECTS[0])
self.logger.info('dev sub connect ' + MSG_SUB_CONNECTS[0])
self.dev.pub_bind(MSG_PUB_BINDS)
self.logger.info('dev pub bind ' + MSG_PUB_BINDS)
self.jsdev.sub_connect(MSG_SUB_CONNECTS[2])
self.logger.info('jsdev sub connect ' + MSG_SUB_CONNECTS[2])
else:
self.dev.sub_connect(MSG_SUB_CONNECTS[1])
self.logger.info('dev sub connect ' + MSG_SUB_CONNECTS[1])
self.dev.pub_bind(MSG_PUB_BINDS)
self.logger.info('dev pub bind ' + MSG_PUB_BINDS)
self.jsdev.sub_connect(MSG_SUB_CONNECTS[3])
self.logger.info('jsdev sub connect ' + MSG_SUB_CONNECTS[3])
self.jsdev.sub_add_url('js.Autoctrl')
self.tmdev.sub_add_url('Transmitter_flag')
except (zmq.error.ZMQError):
self.dev.close()
self.jsdev.close()
self.logger.info('Address already in use')
raise
def ser_init(self):
if not Serial_on:
return
try:
self.Arduino_ser = serial.Serial(Arduino_port_addr,
baudrate=9600,
timeout=1)
self.logger.info(self.Arduino_ser.portstr + ' open successfully!')
self.Arduino_ser.flushInput()
self.Arduino_read = Arduino_READ(self.Arduino_ser, self.dev)
except (serial.serialutil.SerialException, OSError):
self.dev.close()
self.jsdev.close()
self.logger.info('could not open port: ' + Arduino_port_addr)
raise
def Servo_init(self):
self.motor = Servo('Motor_speed', 30, 0, self.dev, self.jsdev,
self.tmdev)
self.rudder = Servo('Rudder_ang', 40, 4, self.dev, self.jsdev,
self.tmdev)
self.mainsail = Servo('Mainsail_ang', 80, 0, self.dev, self.jsdev,
self.tmdev)
self.foresail = Servo('Foresail_ang', 70, 0, self.dev, self.jsdev,
self.tmdev)
def update(self):
self.MIO_pub_sub()
self.Arduino_update()
if not Data_show:
return
self.DataInfoShow()
def MIO_pub_sub(self):
self.js_Autoctrl = int(self.jsdev.sub_get1('js.Autoctrl'))
self.dev.pub_set1('js.Autoctrl', self.js_Autoctrl)
self.dev_connect_flag = not self.dev_connect_flag
self.dev.pub_set1('dev_connect_flag', self.dev_connect_flag)
self.Transmitter_flag = self.tmdev.sub_get1('Transmitter_flag')
self.motor.update(self.js_Autoctrl, self.Transmitter_flag)
self.rudder.update(self.js_Autoctrl, self.Transmitter_flag)
self.mainsail.update(self.js_Autoctrl, self.Transmitter_flag)
self.foresail.update(self.js_Autoctrl, self.Transmitter_flag)
global Data_show
if self.Transmitter_flag == 1:
Data_show = False
def Arduino_update(self):
if not Serial_on:
return
self.Arduino_read.update()
self.Arduino_write()
def Arduino_write(self):
header = '\xff\x01'
tmp = self.fst.pack(self.motor.c_data, self.rudder.c_data,
self.mainsail.c_data, self.foresail.c_data)
crc_code = struct.pack('!H', crc16(tmp))
tmp = header + tmp
tmp = tmp + crc_code
self.Arduino_ser.write(tmp)
def DataInfoShow(self):
self.datashow_count += 1
if self.datashow_count < 15:
return
self.datashow_count = 0
self.logger.info('''MIO received data from bank:
Autoctrl, Motor, Rudder, Mainsail, Foresail: ''' + str([
self.js_Autoctrl, self.motor.r_data, self.rudder.r_data,
self.mainsail.r_data, self.foresail.r_data
]))
if not Serial_on:
return
self.logger.info('''Arduino to MIO data:)
dogvane, mainsail, voltage, arduino_receive_flag: ''' + str([
self.Arduino_read.dogvane, self.Arduino_read.mainsail,
self.Arduino_read.voltage, self.Arduino_read.arduino_receive_flag
]))
self.logger.info('''MIO to Arduino data: )
motor_pwm, rudder_pwm, mainsail_pwm, foresail_pwm: ''' + str([
self.motor.c_data, self.rudder.c_data, self.mainsail.c_data,
self.foresail.c_data
]))
print
class Communicater(object):
def __init__(self, upload=True):
self.dev = MsgDevice()
self.dev.open()
self.dev.sub_connect('tcp://192.168.1.150:55003') # 下载端口
self.upload_flag = upload
if upload:
self.dev.pub_bind('tcp://0.0.0.0:55010') # 上传端口
self.dev.sub_add_url('')
self.dev_gnss = MsgDevice()
self.dev_gnss.open()
self.dev_gnss.sub_connect('tcp://192.168.1.150:55004')
self.dev_ahrs = MsgDevice()
self.dev_ahrs.open()
self.dev_ahrs.sub_connect('tcp://192.168.1.150:55005')
self.dev_volt = MsgDevice()
self.dev_volt.open()
self.dev_volt.sub_connect('tcp://192.168.1.150:55006')
self.data_down = []
self.data_up = [None, None]
self.file_name = './data/' + time.strftime("%Y-%m-%d__%H:%M",
time.localtime())
self.f = open(self.file_name + '.csv', 'w')
self.writer = csv.writer(self.f)
self.dev_gnss.sub_add_url('gps.posx'),
self.dev_gnss.sub_add_url('gps.posy'),
self.dev_gnss.sub_add_url('gps.posz'),
self.dev_gnss.sub_add_url('gps.hspeed'),
self.dev_gnss.sub_add_url('gps.vspeed'),
self.dev_gnss.sub_add_url('gps.track')
self.dev_ahrs.sub_add_url('ahrs.yaw'),
self.dev_ahrs.sub_add_url('ahrs.yaw_speed'),
self.dev_ahrs.sub_add_url('ahrs.acce_x'),
self.dev_ahrs.sub_add_url('ahrs.acce_y'),
self.dev_ahrs.sub_add_url('ahrs.acce_z'),
self.dev.sub_add_url('left.Motor_SpeedCalc')
self.dev.sub_add_url('right.Motor_SpeedCalc')
self.dev_volt.sub_add_url('voltage')
def download(self):
gps = [
self.dev_gnss.sub_get1('gps.posx'),
self.dev_gnss.sub_get1('gps.posy'),
self.dev_gnss.sub_get1('gps.posz'),
self.dev_gnss.sub_get1('gps.hspeed'),
self.dev_gnss.sub_get1('gps.vspeed'),
self.dev_gnss.sub_get1('gps.track')
]
ahrs = [
self.dev_ahrs.sub_get1('ahrs.yaw'),
self.dev_ahrs.sub_get1('ahrs.yaw_speed'),
self.dev_ahrs.sub_get1('ahrs.acce_x'),
self.dev_ahrs.sub_get1('ahrs.acce_y'),
self.dev_ahrs.sub_get1('ahrs.acce_z'),
]
l_m = self.dev.sub_get1('left.Motor_SpeedCalc')
r_m = self.dev.sub_get1('right.Motor_SpeedCalc')
volt = self.dev_volt.sub_get1('voltage')
return gps, ahrs, l_m, r_m, volt
def getNEData(self):
"""
:return: x,y,u,v,yaw,omega in NE sys.
"""
gps, ahrs, l_m, r_m, volt = self.download()
yaw = ahrs[0] - 5 * math.pi / 180
U = gps[3] * math.cos(gps[5]) # 北东系速度
V = gps[3] * math.sin(gps[5])
self.data_down = [gps[0], gps[1], U, V, yaw, ahrs[1]]
return {
'x': gps[0],
'y': gps[1],
'u': U,
'v': V,
'phi': yaw,
'alpha': ahrs[1],
'lm': l_m,
'rm': r_m
}
def upload(self, left, right):
if self.upload_flag:
self.dev.pub_set1('pro.left.speed', left)
self.dev.pub_set1('pro.right.speed', right)
self.data_up = [left, right]
print('uploaded', left, right)
else:
self.data_up = [-1, -1]
print('upload disabled')
def record(self):
self.writer.writerow(self.data_down + self.data_up)
print('Saved: ', self.data_down + self.data_up)
def __del__(self):
self.upload(0, 0)
self.dev.close()
self.f.close()
stopbits=1,
xonxoff=0))
master.set_timeout(1.0)
master.set_verbose(True)
#Connect to control program
dev_pro = MsgDevice()
dev_pro.open()
dev_pro.sub_connect('tcp://127.0.0.1:55002')
dev_pro.pub_bind('tcp://0.0.0.0:55003')
# Connect to joystick
dev_joy = MsgDevice()
dev_joy.open()
dev_joy.sub_connect('tcp://127.0.0.1:55001')
dev_joy.sub_add_url('js.autoctrl')
left_motor = Motor(dev_pro, dev_joy, 'left', 0x01, master)
right_motor = Motor(dev_pro, dev_joy, 'right', 0x02, master)
t = PeriodTimer(0.1)
t.start()
while True:
with t:
autoctrl = dev_joy.sub_get1('js.autoctrl')
dev_pro.pub_set1('autoctrl', autoctrl)
print('Autoctrl:', autoctrl)
left_motor.update(autoctrl)
right_motor.update(autoctrl)
print('rpm1', left_motor.Motor_SpeedCalc, 'rpm2',
right_motor.Motor_SpeedCalc)
class Interface(object):
def __init__(self, sub_addr, ahrs_port, gnss_port, ekf_port, motor_port):
self.dev = MsgDevice()
self.dev.open()
self.dev.sub_connect(sub_addr + ':' + ahrs_port)
self.dev.sub_add_url('ahrs.roll')
self.dev.sub_add_url('ahrs.pitch')
self.dev.sub_add_url('ahrs.yaw')
self.dev.sub_add_url('ahrs.roll_speed')
self.dev.sub_add_url('ahrs.pitch_speed')
self.dev.sub_add_url('ahrs.yaw_speed')
self.dev.sub_add_url('ahrs.acce_x')
self.dev.sub_add_url('ahrs.acce_y')
self.dev.sub_add_url('ahrs.acce_z')
self.dev.sub_connect(sub_addr + ':' + gnss_port)
self.dev.sub_add_url('gps.time')
self.dev.sub_add_url('gps.posx')
self.dev.sub_add_url('gps.posy')
self.dev.sub_add_url('gps.posz')
self.dev.sub_add_url('gps.stdx')
self.dev.sub_add_url('gps.stdy')
self.dev.sub_add_url('gps.stdz')
self.dev.sub_add_url('gps.satn')
self.dev.sub_add_url('gps.hspeed')
self.dev.sub_add_url('gps.vspeed')
self.dev.sub_add_url('gps.track')
self.dev.sub_connect(sub_addr + ':' + ekf_port)
self.dev.sub_add_url('USV150.state', default_values=[0, 0, 0, 0, 0, 0])
if motor_port is not None:
self.dev.pub_bind('tcp://0.0.0.0:' + motor_port)
def receive(self, *args):
data = []
for i in args:
data.append(self.dev.sub_get(i))
return data
def Motor_send(self, left_motor, right_motor):
self.dev.pub_set1('pro.left.speed', left_motor)
self.dev.pub_set1('pro.right.speed', right_motor)
class Interface(object):
def __init__(self, sub_addr, ahrs_port, gnss_port, motor_port):
self.dev = MsgDevice()
self.dev.open()
self.dev.sub_connect(sub_addr + ':' + ahrs_port)
self.dev.sub_add_url('ahrs.roll')
self.dev.sub_add_url('ahrs.pitch')
self.dev.sub_add_url('ahrs.yaw')
self.dev.sub_add_url('ahrs.roll_speed')
self.dev.sub_add_url('ahrs.pitch_speed')
self.dev.sub_add_url('ahrs.yaw_speed')
self.dev.sub_add_url('ahrs.acce_x')
self.dev.sub_add_url('ahrs.acce_y')
self.dev.sub_add_url('ahrs.acce_z')
self.dev.sub_connect(sub_addr + ':' + gnss_port)
self.dev.sub_add_url('gps.posx')
self.dev.sub_add_url('gps.posy')
self.dev.sub_add_url('gps.posz')
self.dev.sub_add_url('gps.stdx')
self.dev.sub_add_url('gps.stdy')
self.dev.sub_add_url('gps.stdz')
self.dev.sub_add_url('gps.hspeed')
self.dev.sub_add_url('gps.vspeed')
self.dev.sub_add_url('gps.track')
self.dev.pub_bind('tcp://0.0.0.0:' + motor_port)
def Sensor_receive(self, *args):
package = []
for i in args:
package.append(self.dev.sub_get1(i))
return package
def Motor_send(self, left_motor, right_motor, autoctrl):
self.dev.pub_set1('js.left.speed', left_motor)
self.dev.pub_set1('js.right.speed', right_motor)
self.dev.pub_set1('js.autoctrl', autoctrl)
class Interface_rec(object):
def __init__(self, sub_addr, gnss_port):
self.dev = MsgDevice()
self.dev.open()
self.dev.sub_connect(sub_addr + ':' + gnss_port)
self.dev.sub_add_url('gps.time')
self.dev.sub_add_url('gps.posx')
self.dev.sub_add_url('gps.posy')
self.dev.sub_add_url('gps.posz')
self.dev.sub_add_url('gps.stdx')
self.dev.sub_add_url('gps.stdy')
self.dev.sub_add_url('gps.stdz')
self.dev.sub_add_url('gps.satn')
self.dev.sub_add_url('gps.hspeed')
self.dev.sub_add_url('gps.vspeed')
self.dev.sub_add_url('gps.track')
def receive(self, *args):
data = []
for i in args:
data.append(self.dev.sub_get1(i))
return data
