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
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!")
target = None
periodt = PeriodTimer(0.1)
try:
while True:
with periodt:
with fps_timer:
if use_local_data:
lidar_data, extra_data = lidar_dataset[
cnt], extra_dataset[cnt]
cnt += 1
if cnt >= len(lidar_data):
break
else:
tmp_data = pull_lidar_dev.sub_get("vlp.image")
lidar_data, extra_data = np.asarray(
tmp_data[:30600]), np.asarray(tmp_data[30600:])
print(lidar_data.mean())
ret = map.update(lidar_data, extra_data)
object_d = detector.update(ret)
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,