st.inc_clear_freqs() print('inc clear freqs') if jm.dec_freqs_track_rope(): st.dec_clear_freqs() print('dec clear freqs') await asyncio.sleep(0.001) async def main(): await asyncio.gather(recv_and_process(), ) if __name__ == '__main__': ### plugin inputs subs_socks = [] subs_socks.append( zmq_wrapper.subscribe([zmq_topics.topic_stereo_camera], zmq_topics.topic_camera_port)) subs_socks.append( zmq_wrapper.subscribe([ zmq_topics.topic_axes, zmq_topics.topic_button, zmq_topics.topic_hat ], zmq_topics.topic_joy_port)) ### plugin outputs sock_pub = zmq_wrapper.publisher(zmq_topics.topic_tracker_port) loop = asyncio.get_event_loop() result = loop.run_until_complete(main()) #asyncio.run(main())
import zmq import struct import cv2, os import numpy as np import pickle import zmq_wrapper as utils import ue4_zmq_topics import zmq_topics import config topic_stereo = ue4_zmq_topics.topic_unreal_stereo_camera % 0 topic_depth = ue4_zmq_topics.topic_unreal_depth % 0 zmq_sub = utils.subscribe([topic_stereo, topic_depth], ue4_zmq_topics.zmq_pub_unreal_proxy[1]) zmq_pub = utils.publisher(zmq_topics.topic_camera_port) pub_sonar = utils.publisher(zmq_topics.topic_sonar_port) cvshow = 0 #cvshow=False test = 1 print('start...') def listener(): cnt = 0 rgb = None while 1: while len(zmq.select([zmq_sub], [], [], 0.001)[0]) > 0: data = zmq_sub.recv_multipart()
sys.path.append('..') sys.path.append('../utils') import zmq_wrapper import zmq_topics import config import cv2 import socket import argparse subs_socks=[] subs_socks.append(zmq_wrapper.subscribe([zmq_topics.topic_stereo_camera], zmq_topics.topic_camera_port)) subs_socks.append(zmq_wrapper.subscribe([zmq_topics.topic_system_state], zmq_topics.topic_controller_port)) autoFocusPublisher = zmq_wrapper.publisher(zmq_topics.topic_autoFocus_port) if __name__=='__main__': doResize = False sx,sy=config.cam_res_rgbx,config.cam_res_rgby imgCnt = 0.0 tic = time.time() keep_running = True curFocusVal = -1 margin = 100 ##af = None
await asyncio.sleep(0.001) async def main(): await asyncio.gather(recv_and_process(), ) if __name__ == '__main__': ### plugin inputs subs_socks = [] subs_socks.append( zmq_wrapper.subscribe([zmq_topics.topic_stereo_camera], zmq_topics.topic_camera_port)) subs_socks.append( zmq_wrapper.subscribe([zmq_topics.topic_imu], zmq_topics.topic_imu_port)) subs_socks.append( zmq_wrapper.subscribe( [zmq_topics.topic_system_state, zmq_topics.topic_tracker_cmd], zmq_topics.topic_controller_port)) ### plugin outputs sock_pub = zmq_wrapper.publisher(zmq_topics.topic_tracker_port) thrusters_source = zmq_wrapper.push_source(zmq_topics.thrusters_sink_port) pub_sock = zmq_wrapper.publisher(zmq_topics.topic_imHoldPos_port) loop = asyncio.get_event_loop() result = loop.run_until_complete(main()) #asyncio.run(main())
# vim: tabstop=8 expandtab shiftwidth=4 softtabstop=4 import numpy as np import zmq import sys import asyncio import time import pickle sys.path.append('..') sys.path.append('../utils') import zmq_wrapper as utils import zmq_topics from joy_mix import Joy_map pub_sock = utils.publisher(zmq_topics.topic_controller_port) subs_socks = [] subs_socks.append( utils.subscribe([zmq_topics.topic_axes, zmq_topics.topic_button], zmq_topics.topic_joy_port)) subs_socks.append( utils.subscribe([zmq_topics.topic_imu], zmq_topics.topic_imu_port)) thruster_sink = utils.pull_sink(zmq_topics.thrusters_sink_port) subs_socks.append(thruster_sink) async def recv_and_process(): keep_running = True thruster_cmd = np.zeros(8) timer10hz = time.time() + 1 / 10.0 timer20hz = time.time() + 1 / 20.0 system_state = {'arm': False, 'mode': [], 'lights': 0} #lights 0-5
sys.path.append('../utils') sys.path.append('unreal_proxy') import zmq_wrapper as utils import zmq_topics import ue4_zmq_topics dill.settings['recurse'] = True lamb = dill.load(open('lambda.pkl', 'rb')) current_command = [0 for _ in range(8)] # 8 thrusters dt = 1 / 60.0 keep_running = True savetofile = False if savetofile: savefd = open(r'dynsim.txt', 'w') #pub_pos_sim = utils.publisher(zmq_topics.topic_sitl_position_report_port) pub_pos_sim = utils.publisher(ue4_zmq_topics.zmq_pub_drone_fdm[1]) pub_imu = utils.publisher(zmq_topics.topic_imu_port) pub_depth = utils.publisher(zmq_topics.topic_depth_port) subs_socks = [] subs_socks.append( utils.subscribe([zmq_topics.topic_thrusters_comand], zmq_topics.topic_thrusters_comand_port)) position_struct = {} def get_next_state(curr_q, curr_u, control, dt, lamb): forces = control u_dot_f = lamb(curr_q, curr_u, *forces).flatten() next_q = curr_q + curr_u * dt
import sys sys.path.append('../') sys.path.append('../utils') import zmq_topics import zmq_wrapper import zmq import time import pickle import requests import json GPS_STATIC_URL = 'http://192.168.2.94' pub_sock = zmq_wrapper.publisher(zmq_topics.topic_gps_port) subs_socks = [] subs_socks.append( zmq_wrapper.subscribe([zmq_topics.topic_depth], zmq_topics.topic_depth_port)) def get_data(url): try: r = requests.get(url) except requests.exceptions.RequestException as exc: print("Exception occured {}".format(exc)) return None if r.status_code != requests.codes.ok: print("Got error {}: {}".format(r.status_code, r.text)) return None
pygame.display.init() pygame.joystick.init() joystick = pygame.joystick.Joystick(0) joystick.init() name = joystick.get_name() #isxbox = 'Microsoft X-Box One pad' in name isxbox = 'X-Box' in name print("Joystick name: {}".format(name)) axes = joystick.get_numaxes() print("Number of axes: {}".format(axes)) n_buttons = joystick.get_numbuttons() clock = pygame.time.Clock() pub_sock = utils.publisher(zmq_topics.topic_joy_port) done = False cnt = 0 start_time = time.time() joy_log = open('joy.log', 'wb') hat = [0, 0] axes_vals = [] def pub(topic, data): pub_sock.send_multipart([topic, data]) pickle.dump([time.time() - start_time, topic, data], joy_log, -1) while not done:
# vim: tabstop=8 expandtab shiftwidth=4 softtabstop=4 import zmq import asyncio import time import pickle import sys sys.path.append('..') sys.path.append('../utils') import zmq_wrapper as utils import zmq_topics import hw_stats_tools pub_sock = utils.publisher(zmq_topics.topic_hw_stats_port) while 1: stats = hw_stats_tools.get_hw_stats() pub_sock.send_multipart([zmq_topics.topic_hw_stats,pickle.dumps((time.time(),stats))]) time.sleep(5)
# vim: tabstop=8 expandtab shiftwidth=4 softtabstop=4 import numpy as np import zmq import sys import asyncio import time import pickle import os sys.path.append('..') sys.path.append('../utils') import zmq_wrapper as utils import zmq_topics from joy_mix import Joy_map pub_sock = utils.publisher(zmq_topics.topic_controller_port) camPubSock = utils.publisher(zmq_topics.topic_cam_ctrl_port) subs_socks = [] subs_socks.append( utils.subscribe([zmq_topics.topic_axes, zmq_topics.topic_button], zmq_topics.topic_joy_port)) subs_socks.append( utils.subscribe([zmq_topics.topic_imu], zmq_topics.topic_imu_port)) subs_socks.append( utils.subscribe([ zmq_topics.topic_gui_controller, zmq_topics.topic_gui_diveModes, zmq_topics.topic_gui_focus_controller, zmq_topics.topic_gui_autoFocus, zmq_topics.topic_gui_start_stop_track, zmq_topics.topic_gui_toggle_auto_exp, zmq_topics.topic_gui_toggle_auto_gain, zmq_topics.topic_gui_inc_exp,
import time import numpy as np import traceback from ipdb import set_trace as st import gi gi.require_version('Gst', '1.0') from gi.repository import GObject, Gst sys.path.append('../') sys.path.append('../utils') import zmq_topics import zmq_wrapper as utils import pickle socket_pub = utils.publisher(zmq_topics.topic_camera_port) def get_dims(pipeline,name='dec'): pad = pipeline.get_by_name(name).pads[0] caps=pad.get_current_caps() struct=caps.get_structure(0) h=struct.get_int('height')[1] w=struct.get_int('width')[1] if h==0: print('Error: dim h is 0') return None return (h,w,3) def get_image_from_sink(sink,dims): sample=sink.emit('pull-sample')
cv2.namedWindow('low', 0) cv2.namedWindow('high', 0) ''' if __name__=='__main__': try: pubList = [] topicPubDict = {} revTopicPubDict = {} for topic in topicsList: if topic[1] in pubList: # port already exist print('reuse publisher port: %d %s'%(topic[1], topic[0]) ) topicPubDict[topic[0]] = topicPubDict[revTopicPubDict[topic[1]]] else: print('creats publisher on port: %d %s'%(topic[1], topic[0]) ) topicPubDict[topic[0]] = utils.publisher(topic[1]) revTopicPubDict[topic[1]] = topic[0] pubList.append(topic[1]) fpsTic = time.time() fpsCnt = 0.0 if recPath is not None: vidPath = os.path.join(recPath, 'video.bin') vidQPath = os.path.join(recPath, 'videoQ.bin') telemPath = os.path.join(recPath, 'telem.pkl') imgCnt = 0 imRaw = None highResEndFlag = False lowResEndFlag = False telemRecEndFlag = False
default=1.0) args = parser.parse_args() file_path_fmt = args.path + '/{}{:08d}' #.ppm' if args.cc is not None: c_mat = pickle.load(open(args.cc, 'rb')) * args.ccr + (1.0 - args.ccr) * np.eye(3) def apply_colorcc(img): return (img.reshape( (-1, 3)) @ c_mat.T).clip(0, 255).reshape(img.shape).astype('uint8') if args.pub_data: socket_pub = utils.publisher(topics.topic_local_route_port, '0.0.0.0') base_name = os.path.basename(args.path) imbuff = [None for i in range(50)] def get_first_and_last(pkl_file): fd = open(pkl_file, 'rb') start = -1 end = -1 while 1: try: ret = pickle.load(fd) #print('----',ret) except EOFError:
jm.update_buttons(data) #target_depth+=data[jm.ud] if topic==zmq_topics.topic_system_state: _,system_state=data await asyncio.sleep(0.001) async def main(): await asyncio.gather( recv_and_process(), ) if __name__=='__main__': ### plugin inputs subs_socks=[] #subs_socks.append(zmq_wrapper.subscribe([zmq_topics.topic_axes],zmq_topics.topic_joy_port)) subs_socks.append(zmq_wrapper.subscribe([zmq_topics.topic_axes,zmq_topics.topic_button],zmq_topics.topic_joy_port)) subs_socks.append(zmq_wrapper.subscribe([zmq_topics.topic_imu],zmq_topics.topic_imu_port)) subs_socks.append(zmq_wrapper.subscribe([zmq_topics.topic_system_state],zmq_topics.topic_controller_port)) ### plugin outputs thrusters_source = zmq_wrapper.push_source(zmq_topics.thrusters_sink_port) pub_sock = zmq_wrapper.publisher(zmq_topics.topic_att_hold_port) loop = asyncio.get_event_loop() result = loop.run_until_complete(main()) #asyncio.run(main())
# conda activate 3.6 #from vnpy import * print('done import') import time, sys import os, zmq sys.path.append('..') sys.path.append('../utils') import zmq_wrapper as utils print('done import 2') import zmq_topics import asyncio, pickle import serial from select import select pub_imu = utils.publisher(zmq_topics.topic_imu_port) ##### vnav commands (setup) #doc in https://www.vectornav.com/docs/default-source/documentation/vn-100-documentation/vn-100-user-manual-(um001).pdf?sfvrsn=b49fe6b9_32 vn_pause = b'$VNASY,0*XX' vn_resume = b'$VNASY,1*XX' vn_setoutput_quaternion = b'$VNWRG,06,2*XX' vn_mag_disturbance_1 = b'$VNKMD,1,1*XX' vn_mag_disturbance_0 = b'$VNKMD,0,0*XX' vn_acc_disturbance_1 = b'$VNKAD,1,1*XX' vn_acc_disturbance_0 = b'$VNKAD,0,0*XX' #Yaw, Pitch, Roll, Magnetic, Acceleration, and Angular Rate Measurements #header VNYMR vn_setoutput = b'$VNWRG,06,14*XX' #####
import serial import time import numpy as np import struct import sys import pickle import zmq sys.path.append('../utils') sys.path.append('../') import detect_usb import zmq_wrapper import zmq_topics import config pub_depth = zmq_wrapper.publisher(zmq_topics.topic_depth_port) pub_volt = zmq_wrapper.publisher(zmq_topics.topic_volt_port) ser = serial.Serial(detect_usb.devmap['PERI_USB'], 115200) time.sleep(2) BATT_AMP_OFFSET = 0.330 BATT_AMP_PERVOLT = 37.8788 ADC_VOLTAGE_MUL = 0.0046 BATT_VOLTAGE_MUL = 11 LIGHTS_MAX=5 print('connected to ', detect_usb.devmap['PERI_USB']) subs_socks=[] subs_socks.append(zmq_wrapper.subscribe([zmq_topics.topic_lights],zmq_topics.topic_controller_port)) subs_socks.append(zmq_wrapper.subscribe([zmq_topics.topic_record_state ],zmq_topics.topic_record_state_port)) #start triggering at config fps
import os,zmq sys.path.append('..') sys.path.append('../utils') import zmq_wrapper as utils print('done import 2') import zmq_topics import asyncio,pickle from brping import Ping1D import detect_usb dev=detect_usb.devmap['SONAR_USB'] myPing = Ping1D(dev, 115200) if myPing.initialize() is False: print("Failed to initialize Ping!") exit(1) pub_sonar = utils.publisher(zmq_topics.topic_sonar_port) cnt=0 while 1: time.sleep(0.1) data = myPing.get_distance() if cnt%10==0: print('sonar ',data) tic=time.time() tosend = pickle.dumps({'ts':tic, 'sonar':[data['distance'],data['confidence']]}) pub_sonar.send_multipart([zmq_topics.topic_sonar,tosend]) cnt+=1 #
subs_socks.append( utils.subscribe([zmq_topics.topic_hw_stats], zmq_topics.topic_hw_stats_port)) subs_socks.append( utils.subscribe([zmq_topics.topic_pos_hold_pid_fmt % i for i in range(3)], zmq_topics.topic_pos_hold_port)) subs_socks.append( utils.subscribe([ zmq_topics.topic_att_hold_yaw_pid, zmq_topics.topic_att_hold_pitch_pid, zmq_topics.topic_att_hold_roll_pid ], zmq_topics.topic_att_hold_port)) #socket_pub = utils.publisher(config.zmq_local_route) if args.pub_data: socket_pub = utils.publisher(zmq_topics.topic_local_route_port, '0.0.0.0') pub_record_state = utils.publisher(zmq_topics.topic_record_state_port) if __name__ == '__main__': if udp: imgSock = socket.socket( socket.AF_INET, # Internet socket.SOCK_DGRAM) # UDP imgSock.bind(('', config.udpPort)) else: init_gst_reader(1) sx, sy = config.cam_res_rgbx, config.cam_res_rgby data_file_fd = None #main_camera_fd=None message_dict = {}
from select import select current_command = [0 for _ in range(8)] # 8 thrusters keep_running = True subs_socks = [] subs_socks.append( utils.subscribe([zmq_topics.topic_thrusters_comand], zmq_topics.topic_thrusters_comand_port)) ser = serial.Serial(detect_usb.devmap['ESC_USB'], 115200) rov_type = int(os.environ.get('ROV_TYPE', '1')) if rov_type == 4: pub_depth = utils.publisher(zmq_topics.topic_depth_port) pub_volt = utils.publisher(zmq_topics.topic_volt_port) pub_motors = utils.publisher(zmq_topics.topic_motors_output_port) subs_socks.append( utils.subscribe([zmq_topics.topic_lights], zmq_topics.topic_controller_port)) subs_socks.append( utils.subscribe([zmq_topics.topic_focus], zmq_topics.topic_controller_port)) OP_MOTORS = 0x01 OP_LEDS = 0x02 OP_CAMSERVO = 0x03