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_imu], zmq_topics.topic_imu_port)) subs_socks.append( zmq_wrapper.subscribe([zmq_topics.topic_depth], zmq_topics.topic_depth_port)) subs_socks.append( zmq_wrapper.subscribe([zmq_topics.topic_system_state], zmq_topics.topic_controller_port)) subs_socks.append( zmq_wrapper.subscribe([zmq_topics.topic_gui_depthAtt], zmq_topics.topic_gui_port)) ### plugin outputs thrusters_source = zmq_wrapper.push_source(zmq_topics.thrusters_sink_port)
[zmq_topics.topic_imu, zmq_topics.topic_imu_port], [zmq_topics.topic_stereo_camera, zmq_topics.topic_camera_port], [zmq_topics.topic_system_state, zmq_topics.topic_controller_port], [zmq_topics.topic_att_hold_roll_pid, zmq_topics.topic_att_hold_port], [zmq_topics.topic_att_hold_pitch_pid, zmq_topics.topic_att_hold_port], [zmq_topics.topic_att_hold_yaw_pid, zmq_topics.topic_att_hold_port], [zmq_topics.topic_depth_hold_pid, zmq_topics.topic_depth_hold_port], [zmq_topics.topic_motors_output, zmq_topics.topic_motors_output_port] ] subs_socks = [] mpsDict = {} for topic in topicsList: mpsDict[topic[0]] = mps.MPS(topic[0]) subs_socks.append(utils.subscribe([topic[0]], topic[1])) rov_type = int(os.environ.get('ROV_TYPE', '1')) doRec = False telemFile = None videoFile = None videoQFile = None recordsBasePath = "../records/" def initRec(): global telemFile, videoFile, videoQFile recName = time.strftime("%Y%m%d_%H%M%S", time.localtime()) recPath = os.path.join(recordsBasePath, recName)
import numpy as np import cv2,struct,pickle import queue import zmq_topics import zmq_wrapper as utils import bayer import config import camCalib import threading parser = argparse.ArgumentParser() parser.add_argument("--cvshow",help="show opencv mode", action='store_true') args = parser.parse_args() # subs_socks=[] subs_socks.append( utils.subscribe([ zmq_topics.topic_record_state ],zmq_topics.topic_record_state_port)) subs_socks.append(utils.subscribe([zmq_topics.topic_system_state],zmq_topics.topic_controller_port)) socket_pub = utils.publisher(zmq_topics.topic_camera_port) socket_calib_pub = utils.publisher(zmq_topics.topic_camera_calib_port) calibrator = camCalib.Calibrator() class TriggerType: SOFTWARE = 1 HARDWARE = 2 CHOSEN_TRIGGER = TriggerType.HARDWARE #CHOSEN_TRIGGER = TriggerType.SOFTWARE SLEEP_DURATION = 200 # amount of time for main thread to sleep for (in milliseconds) until _NUM_IMAGES have been saved
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 asyncio import time import pickle import mixer sys.path.append('..') sys.path.append('../utils') import zmq_wrapper import zmq_topics import image_enc_dec import config import gst subs_socks = [] subs_socks.append( zmq_wrapper.subscribe([zmq_topics.topic_stereo_camera], zmq_topics.topic_camera_port)) keep_running = True async def recv_and_process(): global current_command while keep_running: socks = zmq.select(subs_socks, [], [], 0.005)[0] for sock in socks: ret = sock.recv_multipart() if ret[0] == zmq_topics.topic_stereo_camera: frame_cnt, shape = pickle.loads(ret[1]) if frame_cnt % config.send_modulo == 0: imgl = np.frombuffer(ret[2], 'uint8').reshape(shape).copy() image_enc_dec.encode(imgl, frame_cnt) togst = [imgl]
import zmq_topics import zmq_wrapper as utils import config parser = argparse.ArgumentParser() parser.add_argument("--ip", help="main ground control ip addr", default='127.0.0.1') parser.add_argument("--scale", help="map size deafult 20", default=20.0, type=float) args = parser.parse_args() subs_socks = [] subs_socks.append( utils.subscribe([zmq_topics.topic_tracker], zmq_topics.topic_tracker_port)) subs_socks.append( utils.subscribe([zmq_topics.topic_imu], zmq_topics.topic_imu_port)) subs_socks.append( utils.subscribe([zmq_topics.topic_sonar], zmq_topics.topic_sonar_port)) ##### map radious im meters rad = float(args.scale) ##############move to utils..################### def roty(a): R_y = \ np.array([\ [np.cos(a), 0, np.sin(a) ],
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)) subs_socks.append( zmq_wrapper.subscribe([zmq_topics.topic_tracker], zmq_topics.topic_tracker_port)) ### plugin outputs thrusters_source = zmq_wrapper.push_source(zmq_topics.thrusters_sink_port) pub_sock = zmq_wrapper.publisher(zmq_topics.topic_pos_hold_port)
parser.add_argument("--ip", help="main ground control ip addr", default='127.0.0.1') parser.add_argument("--scale", help="map size deafult 20", default=20.0, type=float) parser.add_argument("--camera_pitch", help="cameras added pitch", default=0.0, type=float) args = parser.parse_args() subs_socks = [] subs_socks.append( utils.subscribe([zmq_topics.topic_tracker], zmq_topics.topic_tracker_port)) subs_socks.append( utils.subscribe([zmq_topics.topic_imu], zmq_topics.topic_imu_port)) subs_socks.append( utils.subscribe([zmq_topics.topic_sonar], zmq_topics.topic_sonar_port)) subs_socks.append(utils.subscribe([b''], zmq_topics.topic_local_route_port)) ##### map radious im meters rad = float(args.scale) ##############move to utils..################### def roty(a): R_y = \ np.array([\
import pickle import argparse import numpy as np import sys sys.path.append('../') sys.path.append('../utils') import zmq_wrapper parser = argparse.ArgumentParser() parser.add_argument("--topic",help="which topic") parser.add_argument("--port",help="which port",type=int,default=9995) parser.add_argument("--type",help="plot type: pid or vector",default='pid') args = parser.parse_args() subs_socks=[] subs_socks.append(zmq_wrapper.subscribe([args.topic.encode() ], args.port) ) class CycArr(): def __init__(self,size=20000): self.buf=[] self.size=size def add(self,arr): self.buf.append(arr) if len(self.buf)>self.size: self.buf.pop(0) def get_data(self,labels): data = np.zeros((len(self.buf),len(labels))) for i,d in enumerate(self.buf): for j,l in enumerate(labels):
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, zmq_topics.topic_gui_dec_exp, zmq_topics.topic_gui_exposureVal ], zmq_topics.topic_gui_port)) subs_socks.append( utils.subscribe([zmq_topics.topic_autoFocus], zmq_topics.topic_autoFocus_port)) subs_socks.append(
from time import sleep import numpy as np import cv2, struct, pickle import queue import zmq_topics import zmq_wrapper as utils import bayer import config parser = argparse.ArgumentParser() parser.add_argument("--cvshow", help="show opencv mode", action='store_true') args = parser.parse_args() # subs_socks = [] subs_socks.append( utils.subscribe([zmq_topics.topic_record_state], zmq_topics.topic_record_state_port)) socket_pub = utils.publisher(zmq_topics.topic_camera_port) class TriggerType: SOFTWARE = 1 HARDWARE = 2 CHOSEN_TRIGGER = TriggerType.HARDWARE #CHOSEN_TRIGGER = TriggerType.SOFTWARE SLEEP_DURATION = 200 # amount of time for main thread to sleep for (in milliseconds) until _NUM_IMAGES have been saved record_state = False
thrusters_source.send_pyobj( ['joy', time.time(), thruster_joy_cmd]) if topic == zmq_topics.topic_button: jm.update_buttons(data) if topic == zmq_topics.topic_imu: yaw, pitch, roll = data['yaw'], data['pitch'], data['roll'] 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_button], zmq_topics.topic_joy_port)) subs_socks.append( zmq_wrapper.subscribe([zmq_topics.topic_imu], zmq_topics.topic_imu_port)) ### plugin outputs thrusters_source = zmq_wrapper.push_source(zmq_topics.thrusters_sink_port) loop = asyncio.get_event_loop() result = loop.run_until_complete(main()) #asyncio.run(main())
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 thrusters_dict = {} last_axes_joy_message_time = 0 #keep alive time jm = Joy_map()
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_imu], zmq_topics.topic_imu_port)) subs_socks.append( zmq_wrapper.subscribe([zmq_topics.topic_sonar], zmq_topics.topic_sonar_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_sonar_hold_port) loop = asyncio.get_event_loop()
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 return r.json()
sys.path.append('..') sys.path.append('../utils') 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:
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 next_u = curr_u + u_dot_f * dt return next_q, next_u async def pubposition(): curr_q = np.zeros(6) curr_u = np.zeros(6)
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())
import pickle import cv2 import os 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
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 ser.write(b'\x01') ser.write(b'%c'%(config.fps+10)) #ser.flush() print('trigger sent') cur_lights_cmd = 0 prev_rec_state = 0 while True: socks=zmq.select(subs_socks,[],[],0.005)[0] for sock in socks: ret=sock.recv_multipart()
print('--->', trackerInitiated, data['trackPnt']) 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_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()
parser = argparse.ArgumentParser() parser.add_argument("--data_path", help="path for data", default='../../data') parser.add_argument("--pub_data", help="publish data", action='store_true') parser.add_argument("--udp", help="get image using udp", action='store_true') args = parser.parse_args() udp = args.udp resize_viewer = 'RESIZE_VIEWER' in os.environ if resize_viewer: resize_width = int(os.environ['RESIZE_VIEWER']) subs_socks = [] subs_socks.append( utils.subscribe( [zmq_topics.topic_thrusters_comand, zmq_topics.topic_system_state], zmq_topics.topic_controller_port)) subs_socks.append( utils.subscribe([zmq_topics.topic_button, zmq_topics.topic_hat], 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_depth], zmq_topics.topic_depth_port)) subs_socks.append( utils.subscribe([zmq_topics.topic_depth_hold_pid], zmq_topics.topic_depth_hold_port)) subs_socks.append( utils.subscribe([zmq_topics.topic_sonar], zmq_topics.topic_sonar_port)) subs_socks.append( utils.subscribe([zmq_topics.topic_stereo_camera_ts],
sys.path.append('..') sys.path.append('../utils') import zmq_wrapper as utils import zmq_topics import detect_usb import config 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],