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
