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],
