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])
self.dev.pub_bind('tcp://0.0.0.0:'+motor_port)
def receive1(self, *args):
data = []
for i in args:
data.append(self.dev.sub_get1(i))
return data
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):
print ('left_motor, right_motor', left_motor, right_motor)
self.dev.pub_set1('pro.left.speed', left_motor)
self.dev.pub_set1('pro.right.speed', right_motor)
def pointSend(self, pose):
self.dev.pub_set('target.point', pose)
ret["target"] = target
# return ret
return pickle.dumps(ret)
def build_push_detection_dev(push_detection_dev):
push_detection_dev.open()
# push_detection_dev.pub_bind('tcp://0.0.0.0:55010') # 上传端口
push_detection_dev.sub_connect("tcp://192.168.0.8:55019")
push_detection_dev.sub_add_url("det.data", [-100] * 7)
if __name__ == "__main__":
push_detection_dev = MsgDevice()
build_push_detection_dev(push_detection_dev)
from msgdev import PeriodTimer
t = PeriodTimer(0.1)
try:
while True:
with t:
data = push_detection_dev.sub_get("det.data")
print(data)
# print(data, pickle.loads(data))
finally:
push_detection_dev.pub_set("data", pickle.dumps(None))
push_detection_dev.close()
logging.info("Everything Closed!")
avaliables = detector.availiable_obejct_keys
if pressed_key in avaliables:
target = avaliables[pressed_key]
elif (pressed_key == ord("q")) or (target
not in object_d.keys()):
target = None
if args.render:
pressed_key = v.draw(ret["map_n"],
objects=object_d,
target=target
or list(avaliables.values()))
if pressed_key == ESC:
break
push_data = build_push_data(detector.object_dict, target)
push_detection_dev.pub_set('det.data', push_data)
except Exception as e:
pass
finally:
# import cv2
# cv2.destroyWindow("Detection")
# cv2.destroyAllWindows()
v.close()
for _ in range(3):
push_detection_dev.pub_set("det.data", [-99] * 7)
time.sleep(5)
push_detection_dev.close()
logging.info("Everything Closed!")
