def resubmit(self, indices_or_msg_ids=None, subheader=None, block=None):
"""Resubmit one or more tasks.
in-flight tasks may not be resubmitted.
Parameters
----------
indices_or_msg_ids : integer history index, str msg_id, or list of either
The indices or msg_ids of indices to be retrieved
block : bool
Whether to wait for the result to be done
Returns
-------
AsyncHubResult
A subclass of AsyncResult that retrieves results from the Hub
"""
block = self.block if block is None else block
if indices_or_msg_ids is None:
indices_or_msg_ids = -1
if not isinstance(indices_or_msg_ids, (list,tuple)):
indices_or_msg_ids = [indices_or_msg_ids]
theids = []
for id in indices_or_msg_ids:
if isinstance(id, int):
id = self.history[id]
if not isinstance(id, str):
raise TypeError("indices must be str or int, not %r"%id)
theids.append(id)
for msg_id in theids:
self.outstanding.discard(msg_id)
if msg_id in self.history:
self.history.remove(msg_id)
self.results.pop(msg_id, None)
self.metadata.pop(msg_id, None)
content = dict(msg_ids = theids)
self.session.send(self._query_socket, 'resubmit_request', content)
zmq.select([self._query_socket], [], [])
idents,msg = self.session.recv(self._query_socket, zmq.NOBLOCK)
if self.debug:
pprint(msg)
content = msg['content']
if content['status'] != 'ok':
raise self._unwrap_exception(content)
ar = AsyncHubResult(self, msg_ids=theids)
if block:
ar.wait()
return ar
def test_timeout(self):
"""make sure select timeout has the right units (seconds)."""
s1, s2 = self.create_bound_pair(zmq.PAIR, zmq.PAIR)
tic = time.time()
r,w,x = zmq.select([s1,s2],[],[],.005)
toc = time.time()
self.assertTrue(toc-tic < 1)
self.assertTrue(toc-tic > 0.001)
tic = time.time()
r,w,x = zmq.select([s1,s2],[],[],.25)
toc = time.time()
self.assertTrue(toc-tic < 1)
self.assertTrue(toc-tic > 0.1)
def last_msg(self):
r = [self.s]
msg = None
while r:
r, w, x = zmq.select([self.s], [], [], 0.0)
if r:
msg = self.s.recv()
r, w, x = zmq.select([self.s], [], [], 0.05)
if r:
msg = self.s.recv()
if msg is not None:
self._last = decode_utf8_json(msg)
return self._last
def read_write_nontty(socket):
EOF_reached = False
try:
r, w, x = select.select([sys.stdin], [sys.stdout], [], 0.0)
except select.error:
pass
zr,zw,zx = zmq.select([socket],[socket],[], timeout = 0.0)
if (sys.stdin in r) and (socket in zw):
x = sys.stdin.readline()
if not EOF_reached:
socket.send_json({'p':x})
if x == '':
EOF_reached = True
if (socket in zr) and (sys.stdout in w):
x = socket.recv_json()
try:
plain = x['p']
sys.stdout.write(plain)
while True:
r, w, x = select.select([], [sys.stdout], [], 0.0)
if sys.stdout in w:
sys.stdout.flush()
break
except KeyError:
if 'ctrl' in x:
ctrlmsg = x['ctrl']
if 'terminated' in ctrlmsg:
sys.stdout.write('\r\nexiting... \r\n')
socket.send_json({'ctrl':'terminated'})
return 1
return 0
def mdp_request(socket, service, msg, timeout=None):
"""Synchronous MDP request.
This function sends a request to the given service and
waits for a reply.
If timeout is set and no reply received in the given time
the function will return `None`.
:param socket: zmq REQ socket to use.
:type socket: zmq.Socket
:param service: service id to send the msg to.
:type service: str
:param msg: list of message parts to send.
:type msg: list of str
:param timeout: time to wait for answer in seconds.
:type timeout: float
:rtype list of str:
"""
if not timeout or timeout < 0.0:
timeout = None
if type(msg) in (bytes, unicode):
msg = [msg]
to_send = [PROTO_VERSION, service]
to_send.extend(msg)
socket.send_multipart(to_send)
ret = None
rlist, _, _ = select([socket], [], [], timeout)
if rlist and rlist[0] == socket:
ret = socket.recv_multipart()
ret.pop(0) # remove service from reply
return ret
def backend_send(self, *args, backend='echo'):
sock = getattr(self, backend)
self.assertEqual(([], [sock], []),
zmq.select([], [sock], [], timeout=self.timeout))
sock.send_multipart([
a if isinstance(a, bytes) else a.encode('utf-8')
for a in args], zmq.NOBLOCK)
def send_json(self, msg):
"""
Send a JSON-encoded message. If the connection is not established yet,
establish it before sending.
@raises ZMQReqConnection.NoConnection:
if the connection could not be established.
"""
with self.__zmq_lock:
if self._socket is None:
self.__connect()
if self._socket is not None:
sockets = [self._socket]
# Is the socket immediately available for writing?
rlist, wlist, xlist = zmq.select(rlist=sockets,
wlist=sockets,
xlist=sockets,
timeout=0)
try:
if self._socket in wlist:
self._socket.send_json(msg)
else:
raise ZMQReqConnection.NoConnection()
except (zmq.ZMQError, ZMQReqConnection.NoConnection) as e:
logger.error('ZMQ error on send: %s', e)
self.__disconnect()
raise ZMQReqConnection.NoConnection(
'No connection to Trusted Host')
def backend_recv(self, backend='echo'):
sock = getattr(self, backend)
if (([sock], [], []) !=
zmq.select([sock], [], [], timeout=self.timeout)):
raise TimeoutError()
val = sock.recv_multipart(zmq.NOBLOCK)
return val
def chat_room(kernel,output_box,socket_address):
context = zmq.Context()
subscribe_socket = context.socket(zmq.SUB)
subscribe_socket.connect(socket_address)
subscribe_socket.setsockopt(zmq.SUBSCRIBE, '')
print '>>> welcome to the zmq chatroom'
while True:
# print 'loop'
zr,zw,zx = zmq.select([subscribe_socket],[],[],timeout = 0.0)
if subscribe_socket in zr:
# print 'ok got message'
message = subscribe_socket.recv_json()
if 'plain_message' in message:
plain_message = message['plain_message']
output_box.value += '{}: {}\n'.format(plain_message['nickname'],plain_message['message'])
elif 'object_transfer' in message:
# print 'aqcuiring lock on shared state'
ipythonchat_state.shared_state_lock.acquire()
#ok now we no, nobody else is manipulating the shared_state object
sender = message['object_transfer']['sender']
import pickle
from_sender = pickle.loads(message['object_transfer']['payload'])
output_box.value += '<< got data object from {}>>\n'.format(sender)
ipythonchat_state.shared_state = from_sender
ipythonchat_state.shared_state_lock.release()
# print 'released lock, shared state is: ', ipythonchat_state.shared_state
kernel.do_one_iteration()
time.sleep(0.01)
def run(self):
context = zmq.Context().instance()
sub_socket = context.socket(zmq.SUB)
sub_socket.connect("tcp://" + self.server_address + ":34234")
sub_socket.setsockopt(zmq.SUBSCRIBE, "")
listener = context.socket(zmq.PAIR)
print(str(id(self)))
listener.bind("inproc://" + str(id(self)))
readable = []
while listener not in readable:
readable, _, _ = zmq.select([sub_socket, listener], [], [])
if sub_socket in readable:
published = json.loads(sub_socket.recv())
if published.get("uuid") == self.uuid:
if not self.failed and published.get("status_name") == "failed":
self.failed = True
self.status_handler(self.alive, self.failed)
elif not self.alive and published.get("peers") and "mx" in published['peers']:
self.alive = True
self.status_handler(self.alive, self.failed)
listener.recv()
listener.close()
sub_socket.close()
def _runloop(self):
"""A single run-through of all sockets handled by this backend."""
inputs = [self.pull_socket, sys.stdin]
outputs = [] if self.pub_queue.empty() else [self.pub_socket]
exceptionals = inputs + [self.pub_socket]
read, write, error = zmq.select(inputs, outputs, exceptionals)
for fileno in read:
if fileno == self.pull_socket:
self.route_message(self.pull_socket.recv_json())
elif fileno == sys.stdin.fileno():
for _ in sys.stdin:
pass
sys.exit(0)
for fileno in write:
if fileno == self.pub_socket:
try:
self.pub_socket.send_json(self.pub_queue.get_nowait())
except Queue.Empty:
pass
for fileno in error:
if fileno == self.pub_socket:
raise Exception('PUB socket in exceptional state')
elif fileno == self.pull_socket:
raise Exception('PULL socket in exceptional state')
elif fileno == sys.stdin.fileno():
raise Exception('stdin in exceptional state')
def run_loop():
while True:
http_loop()
rlist, _, _ = zmq.select([httpd, zmq_socket], [], [])
if zmq_socket in rlist:
new_subscriber()
def trySend(sock, message):
reply = None
for _ in range(3):
sock.send_json(message)
ready = zmq.select([sock],[],[],10.0)
if ready:
return sock.recv_json()
print "Awaiting response from server"
return reply
def _connect(self, sshserver, ssh_kwargs, timeout):
"""setup all our socket connections to the cluster. This is called from
__init__."""
# Maybe allow reconnecting?
if self._connected:
return
self._connected=True
def connect_socket(s, url):
url = util.disambiguate_url(url, self._config['location'])
if self._ssh:
return tunnel.tunnel_connection(s, url, sshserver, **ssh_kwargs)
else:
return s.connect(url)
self.session.send(self._query_socket, 'connection_request')
r,w,x = zmq.select([self._query_socket],[],[], timeout)
if not r:
raise error.TimeoutError("Hub connection request timed out")
idents,msg = self.session.recv(self._query_socket,mode=0)
if self.debug:
pprint(msg)
msg = ss.Message(msg)
content = msg.content
self._config['registration'] = dict(content)
if content.status == 'ok':
if content.mux:
self._mux_socket = self._context.socket(zmq.XREQ)
self._mux_socket.setsockopt(zmq.IDENTITY, self.session.session)
connect_socket(self._mux_socket, content.mux)
if content.task:
self._task_scheme, task_addr = content.task
self._task_socket = self._context.socket(zmq.XREQ)
self._task_socket.setsockopt(zmq.IDENTITY, self.session.session)
connect_socket(self._task_socket, task_addr)
if content.notification:
self._notification_socket = self._context.socket(zmq.SUB)
connect_socket(self._notification_socket, content.notification)
self._notification_socket.setsockopt(zmq.SUBSCRIBE, b'')
# if content.query:
# self._query_socket = self._context.socket(zmq.XREQ)
# self._query_socket.setsockopt(zmq.IDENTITY, self.session.session)
# connect_socket(self._query_socket, content.query)
if content.control:
self._control_socket = self._context.socket(zmq.XREQ)
self._control_socket.setsockopt(zmq.IDENTITY, self.session.session)
connect_socket(self._control_socket, content.control)
if content.iopub:
self._iopub_socket = self._context.socket(zmq.SUB)
self._iopub_socket.setsockopt(zmq.SUBSCRIBE, b'')
self._iopub_socket.setsockopt(zmq.IDENTITY, self.session.session)
connect_socket(self._iopub_socket, content.iopub)
self._update_engines(dict(content.engines))
else:
self._connected = False
raise Exception("Failed to connect!")
def handle_tty(cmd,cid,socket):
print 'handling tty docker session'
import pty
import shlex
master, slave = pty.openpty()
p = subprocess.Popen(shlex.split(cmd), stdin = slave, stdout = slave, stderr = slave)
print 'started container with pid: {}'.format(p.pid)
term_size = socket.recv_json()['ctrl']['term_size']
set_winsize(master,term_size['rows'],term_size['cols'],p.pid)
while True:
#print "polling"
r, w, x = select.select([master],[master],[master], 0.0)
#print "master poll: r: {} w: {} x: {}".format(r,w,x)
zr,zw,zx = zmq.select([socket], [socket],[socket], timeout = 0.0)
#print "ZMQ poll: r: {} w: {} x: {}".format(zr,zw,zx)
procpoll = p.poll()
#print 'process: {}'.format(procpoll)
if (procpoll is not None) and (socket in zw):
print "ending session because process ended"
socket.send_json({'ctrl':'terminated'})
print 'wait for ack from client'
ack = socket.recv_json()
print "return"
return
if (master in r) and (socket in zw):
#print "reading!"
fromprocess = os.read(master,1024)
#print 'sending {}'.format(fromprocess)
socket.send_json({'p':fromprocess})
if (master in w) and (socket in zr):
# Wait for next request from client
#print "recv"
message = socket.recv_json()
#print("Received request: {} length: {}".format(message,len(message)))
try:
os.write(master,message['p'])
except KeyError:
if 'ctrl' in message:
ctrlmsg = message['ctrl']
if 'term_size' in ctrlmsg:
set_winsize(master,ctrlmsg['term_size']['rows'],ctrlmsg['term_size']['cols'],p.pid)
if 'signal' in ctrlmsg:
print 'got signal: {}'.format(ctrlmsg['signal'])
os.kill(p.pid,ctrlmsg['signal'])
if ctrlmsg['signal'] in [signal.SIGHUP,signal.SIGTERM,signal.SIGKILL]:
stop_container(get_container_id(cid))
return
def recv(self, timeout=0.0):
r, w, x = zmq.select([self.s], [], [], timeout)
if r:
try:
self._last = decode_utf8_json(self.s.recv())
return self._last
except ValueError:
raise NoMessagesException
else:
raise NoMessagesException
def control(self, *args):
sock = self.zmq.socket(zmq.REQ)
try:
sock.connect(CONTROL_ADDR)
sock.send_multipart([a.encode('utf-8') for a in args])
self.assertEqual(([sock], [], []),
zmq.select([sock], [], [], timeout=self.timeout))
return sock.recv_multipart()
finally:
sock.close()
def select(rlist, wlist, xlist, timeout):
i, rlist = _mkindex(rlist)
wi, wlist = _mkindex(wlist)
xi, xlist = _mkindex(xlist)
i.update(wi)
i.update(xi)
r, w, x = zmq.select(rlist, wlist, xlist, timeout)
return _useindex(r, i), _useindex(w, i), _useindex(x, i)
def test_pair(self):
s1, s2 = self.create_bound_pair(zmq.PAIR, zmq.PAIR)
# Sleep to allow sockets to connect.
wait()
rlist, wlist, xlist = zmq.select([s1, s2], [s1, s2], [s1, s2])
self.assert_(s1 in wlist)
self.assert_(s2 in wlist)
self.assert_(s1 not in rlist)
self.assert_(s2 not in rlist)
def backend_recv(self, backend=None):
if backend is None:
sock = self.chatfw
else:
raise NotImplementedError(backend)
if ([sock], [], []) != zmq.select([sock], [], [], timeout=self.timeout):
raise TimeoutError()
val = sock.recv_multipart()
if val[1] == b"heartbeat":
return self.backend_recv(backend=backend)
return val
def zmq_monitor(sio, socketname):
context = zmq.Context()
socket = context.socket(zmq.PULL)
socket.bind(socketname)
while True:
zr, zw, zx = zmq.select([socket], [socket], [socket], timeout=0.0)
if socket in zr:
jsondata = socket.recv_json()
senddata = json.dumps(jsondata)
sio.emit(jsondata["to"], senddata, room=jsondata["compid"])
time.sleep(0.01)
def backend_recv(self, backend=None):
if backend is None:
sock = self.chatfw
else:
sock = self.minigame
if (([sock], [], []) !=
zmq.select([sock], [], [], timeout=self.timeout)):
raise TimeoutError()
val = sock.recv_multipart()
if val[1] == b'heartbeat':
return self.backend_recv(backend=backend)
return val
def read_write_nontty(socket,writer,outfile):
try:
r, w, x = select.select([], [outfile], [], 0.0)
except select.error:
pass
zr,zw,zx = zmq.select([socket],[socket],[], timeout = 0.0)
if (socket in zr) and (outfile in w):
x = socket.recv_json()
result = handle_message(x,socket,writer,outfile)
if result == 1: return 1
return 0
def trySend(sock, message):
reply = None
for _ in range(4):
try:
sock.send_json(message)
except zmq.ZMQError:
# try resetting the socket
sock.close()
sock.connect("tcp://brazil.vampire:5556")
ready,_,_ = zmq.select([sock],[],[],10.0)
if ready:
return sock.recv_json()
print "Awaiting response from server"
return reply
def _select_recv(self, multipart, socket, **kwargs):
"""call recv[_multipart] in a way that raises if there is nothing to receive"""
if zmq.zmq_version_info() >= (3,1,0):
# zmq 3.1 has a bug, where poll can return false positives,
# so we wait a little bit just in case
# See LIBZMQ-280 on JIRA
time.sleep(0.1)
r,w,x = zmq.select([socket], [], [], timeout=5)
assert len(r) > 0, "Should have received a message"
kwargs['flags'] = zmq.DONTWAIT | kwargs.get('flags', 0)
recv = socket.recv_multipart if multipart else socket.recv
return recv(**kwargs)
def recv(self):
# check to see if there is read, write, or erros
r, w, e = zmq.select([self.socket], [], [], self.poll_time)
topic = ''
msg = {}
# should this be a for loop? I don't think so???
if len(r) > 0:
topic, jmsg = r[0].recv_multipart()
msg = json.loads(jmsg)
# topic, jmsg = self.socket.recv_multipart()
# msg = json.loads(jmsg)
return topic, msg
def rawrequest(self, service, msg, timeout=None):
if not timeout or timeout < 0.0:
timeout = None
if type(msg) in (bytes, str):
msg = [msg]
to_send = [self._proto_version, service]
to_send.extend(msg)
self.socket.send_multipart(to_send)
ret = None
rlist, _, _ = select([self.socket], [], [], timeout)
if rlist and rlist[0] == self.socket:
ret = self.socket.recv_multipart()
ret.pop(0) # remove service from reply
ret.pop(0)
return ret
def start_docker(cmd, cid, socket, logfile, msgstub):
cmd = cmd + "| cat" # make sure there is not interactive updating
writelog = open(logfile, "w")
readlog = open(logfile, "r")
p = subprocess.Popen(cmd, shell=True, stdout=writelog, stderr=writelog)
while True:
r, w, x = select.select([readlog], [], [], 0.0)
zr, zw, zx = zmq.select([socket], [socket], [socket], timeout=0.0)
procpoll = p.poll()
if procpoll is not None:
print "ending session because process ended"
break
if (readlog in r) and (socket in zw):
fromprocess = os.read(readlog.fileno(), 1024)
if fromprocess:
socket.send_json(dict(p=fromprocess, **msgstub))
def eventcollector(elastic_ip):
context = zmq.Context()
pull_socket = context.socket(zmq.PULL)
pull_socket.bind("tcp://*:3141")
nevents = 0
put_event = es_put.get_putter(elastic_ip)
click.echo('start collection of events with forwarding to {}'.format(elastic_ip))
while True:
zr,zw,zx = zmq.select([pull_socket], [],[], timeout = 0.0)
if pull_socket in zr:
message = pull_socket.recv_json()
nevents = nevents+1
print '{} events received'.format(nevents)
put_event(message)
time.sleep(0.001)
def handle_nontty(cmd,cid,socket):
print 'handling non tty docker session'
import shlex
p = subprocess.Popen(shlex.split(cmd), stdin = subprocess.PIPE, stdout = subprocess.PIPE, stderr = subprocess.STDOUT)
print 'started container with pid: {}'.format(p.pid)
while True:
if p.poll() is not None:
print "ending session because process ended"
socket.send_json({'ctrl':'terminated'})
print 'wait for ack from client'
ack = socket.recv_json()
print "return"
return
writefds = []
if not p.stdin.closed:
writefds.append(p.stdin)
r,w,x = select.select([p.stdout],writefds,[],0.0)
zr,zw,zx = zmq.select([socket], [socket],[socket], timeout = 0.0)
if (socket in zr) and (p.stdin in w):
message = socket.recv_json()
try:
if message['p'] == '':
print 'EOF of input'
p.stdin.close()
else:
p.stdin.write(message['p'])
except KeyError:
if 'ctrl' in message:
ctrlmsg = message['ctrl']
if 'signal' in ctrlmsg:
print 'got signal: {}'.format(ctrlmsg['signal'])
os.kill(p.pid,ctrlmsg['signal'])
if ctrlmsg['signal'] in [signal.SIGHUP,signal.SIGTERM,signal.SIGKILL]:
stop_container(get_container_id())
return
if (p.stdout in r) and (socket in zw):
x = os.read(p.stdout.fileno(),1024)
socket.send_json({'p':x})
print 'read reamining stdout buffer'
for x in p.stdout.readlines():
socket.send_json({'p':x})
return
if __name__ == '__main__':
zmqctx = zmq.Context()
sock_txt = zmqctx.socket(zmq.SUB)
sock_txt.connect("tcp://localhost:13373")
sock_txt.setsockopt(zmq.SUBSCRIBE, "")
sock_rych = zmqctx.socket(zmq.PUB)
sock_rych.setsockopt(zmq.HWM, 100)
sock_rych.bind('tcp://*:13374')
try:
while True:
while not zmq.select([sock_txt], [], [], 3)[0]:
time.sleep(.1)
article = sock_txt.recv_pyobj()
if is_bloomberg_scorable(article):
print 'processing %r' % (article['id'], )
add_bloomberg_score(article)
else:
print '(%s %s)' % (article['id'], article['lang'])
sock_rych.send_pyobj(article)
except:
traceback.print_exc()
finally:
sock_txt.close()
sock_rych.close()
def recorder():
global telemFile, videoFile, videoQFile, doRec
cnt = 0
imgCnt = 0
imgDilution = 4 #save full frame each
hasHighQuality = False
while True:
socks = zmq.select(subs_socks, [], [], 0.001)[0]
ts = time.time()
cnt += 1
if cnt % 200 == 0:
total, used, free = shutil.disk_usage("/")
if free // 2**30 < 5:
doRec = False
print("***Low disk space - record Stopped! ******" * 5)
for sock in socks:
ret = sock.recv_multipart()
#topic,data=ret[0],pickle.loads(ret[1])
topic = ret[0]
if topic in mpsDict.keys():
mpsDict[topic].calcMPS()
if topic == zmq_topics.topic_system_state:
newDoRec = pickle.loads(ret[1])['record']
if (newDoRec) and (not doRec):
recPath = initRec()
print('record started, %s' % recPath)
doRec = True
elif (not newDoRec) and doRec:
print('Stop recording...')
doRec = False
if doRec:
if topic == zmq_topics.topic_stereo_camera:
imgCnt += 1
imMetaData = pickle.loads(ret[-3])
hasHighQuality = imMetaData[-1]
if hasHighQuality:
with open(videoFile, 'ab') as fid:
# write image raw data
fid.write(ret[-1])
'''
imShape = pickle.loads(ret[1])[1]
imRaw = np.frombuffer(ret[-1], dtype='uint8').reshape(imShape)
low = cv2.resize(imRaw, (imShape[1]//2, imShape[0]//2))
'''
low = ret[-2]
with open(videoQFile, 'ab') as fid:
# write image raw data
fid.write(low) #.tobytes())
with open(telemFile, 'ab') as fid:
# write image metadata
recImMetadata = ret[:-2]
recImMetadata.append(hasHighQuality)
pickle.dump([ts, recImMetadata], fid)
else:
with open(telemFile, 'ab') as fid:
pickle.dump([ts, ret], fid)
def listener():
rospy.init_node('ue4_bridge', anonymous=True, log_level=rospy.DEBUG)
rate=rospy.Rate(1000)
rospy.loginfo('starting...')
publishers={}
start=rospy.get_time()
while not rospy.is_shutdown():
while len(zmq.select([zmq_sub],[],[],0)[0])>0:
topic, shape, data = zmq_sub.recv_multipart()
#rospy.loginfo(rospy.get_caller_id() + ' got topic %s', topic)
topic=topic.decode()
#rospy.loginfo('topic is..%s',topic)
if topic not in publishers:
if 'depth' not in topic:
publishers[topic]=rospy.Publisher(topic,Image,queue_size=2)
else:
publishers[topic+'_rgb8']=rospy.Publisher(topic+'_rgb8',Image,queue_size=2)
publishers[topic+'_range_mm']=rospy.Publisher(topic+'_range_mm',Image,queue_size=2)
publishers[topic+'_cam_info']=rospy.Publisher(topic+'_cam_info',CameraInfo,queue_size=2)
#import pdb;pdb.set_trace()
shape=struct.unpack('lll',shape)
if 'depth' not in topic:
img=np.fromstring(data,'uint8').reshape(shape)
publishers[topic].publish(cvbridge.cv2_to_imgmsg(img,'bgr8'))
else:
#img is in the format of RGBA A store depth in cm and all in float16 format
tstamp=rospy.get_rostime()
img=np.fromstring(data,'float16').reshape(shape)
img_max_val = 5.0 #assuming I right
depth_camera_img_rgb = (img[:,:,[2,1,0]].clip(0,img_max_val)/img_max_val*255).astype('uint8')
img_rgb=cvbridge.cv2_to_imgmsg(depth_camera_img_rgb,'bgr8')
img_rgb.header.stamp=tstamp
publishers[topic+'_rgb8'].publish(img_rgb)
#conversions to be compatible with intel realsense camera
depth_err_val=65504
depth_img_f16=img[:,:,3]
#max_distance=64#m
depth_img_f16[depth_img_f16==depth_err_val]=0
depth_final_mm=(depth_img_f16*10).astype('uint16') #cm to mm
img_depth=cvbridge.cv2_to_imgmsg(depth_final_mm,'mono16')
img_depth.header.stamp=tstamp
publishers[topic+'_range_mm'].publish(img_depth)
cam_info = CameraInfo()
cam_info.header.stamp=tstamp
cam_info.distortion_model='plumb_bob'
cam_info.D=[0.0]*6
cam_info.K=[ 160.0, 0.0, 160.0,
0.0, 160.0, 120.0,
0.0, 0.0, 1.0]
cam_info.R=[1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
cam_info.P=[ 160.0, 0.0, 160.0, 0,
0.0, 160.0, 120.0, 0,
0.0, 0.0, 1.0 , 0]
cam_info.header.frame_id='camera_depth_optical_frame'
cam_info.height = 240
cam_info.width = 320
cam_info.roi.height = 240
cam_info.roi.width = 320
cam_info.roi.do_rectify = False
publishers[topic+'_cam_info'].publish(cam_info)
if cvshow:
if 'depth' in topic:
clip_dist_cm=30*100
aaa=img.max()
inds2=img==aaa
img=img.clip(0,clip_dist_cm)
#img=np.fromstring(data,'uint8').reshape(shape)
rospy.loginfo('maxmin %f %f'%(img.max(),img.min()))
img=img.astype('float')/clip_dist_cm*255
img[inds2]=255
#import pdb;pdb.set_trace()
cv2.imshow(topic,img.astype('uint8'))
else:
cv2.imshow(topic,cv2.resize(cv2.resize(img,(1024,1024)),(512,512)))
cv2.waitKey(1)
rate.sleep()
def main_loop(gworld):
print('-- actors --')
for p in ph.GetActorsNames(gworld):
print(p)
print('-- textures --')
drone_textures=[]
for tn in drone_texture_names:
drone_textures.append(ph.GetTextureByName(tn))
drone_textures_down=[]
for tn in drone_textures_down_names:
drone_textures_down.append(ph.GetTextureByName(tn))
drone_textures_depth=[]
for tn in drone_textures_depth_names:
drone_textures_depth.append(ph.GetTextureByName(tn))
if not all(drone_textures):
print("Error, Could not find all textures")
while 1:
yield
drone_actors=[]
for drn in drone_actors_names:
drone_actors.append(ph.FindActorByName(gworld,drn))
if not all(drone_actors):
print("Error, Could not find all drone actors")
while 1:
yield
for _ in range(10): #need to send it a few time don't know why.
print('sending state main loop')
socket_pub.send_multipart([config.topic_unreal_state,b'main_loop'])
yield
drone_start_positions=[np.array(ph.GetActorLocation(drone_actor)) for drone_actor in drone_actors]
positions=[None for _ in range(config.n_drones)]
while 1:
for drone_index in range(config.n_drones):
socket_sub=drone_subs[drone_index]
drone_actor=drone_actors[drone_index]
while len(zmq.select([socket_sub],[],[],0)[0])>0:
topic, msg = socket_sub.recv_multipart()
positions[drone_index]=pickle.loads(msg)
#print('-----',positions[drone_index])
position=positions[drone_index]
if position is not None:
new_pos=drone_start_positions[drone_index]+np.array([position['posx'],position['posy'],position['posz']])*100 #turn to cm
#print('-----',drone_index,new_pos)
ph.SetActorLocation(drone_actor,new_pos)
ph.SetActorRotation(drone_actor,(position['roll'],position['pitch'],position['yaw']))
positions[drone_index]=None
yield
for drone_index in range(config.n_drones):
#img=cv2.resize(ph.GetTextureData(drone_textures[drone_index]),(1024,1024),cv2.INTER_LINEAR)
topics=[]
imgs=[]
img=ph.GetTextureData(drone_textures[drone_index])
topics.append(config.topic_unreal_drone_rgb_camera%drone_index)
imgs.append(img)
if drone_index<len(drone_textures_down):
img_down=ph.GetTextureData(drone_textures_down[drone_index])
topics.append(config.topic_unreal_drone_rgb_camera%drone_index+b'down')
imgs.append(img_down)
if drone_index<len(drone_textures_depth):
img_depth=ph.GetTextureData16f(drone_textures_depth[drone_index],channels=[0,1,2,3]) #depth data will be in A componnent
#img_depth=ph.GetTextureData(drone_textures_depth[drone_index],channels=[2]) #depth data will be in red componnent
topics.append(config.topic_unreal_drone_rgb_camera%drone_index+b'depth')
imgs.append(img_depth)
#topics=[config.topic_unreal_drone_rgb_camera%drone_index,
# config.topic_unreal_drone_rgb_camera%drone_index+b'down',
# config.topic_unreal_drone_rgb_camera%drone_index+b'depth']
#imgs=[ ph.GetTextureData(drone_textures[drone_index]),
# ph.GetTextureData(drone_textures_down[drone_index]),
# ph.GetTextureData(drone_textures_depth[drone_index],channels=[2])]
if pub_cv:
for topic,img in zip(topics,imgs):
#socket_pub.send_multipart([topic,pickle.dumps(img,2)])
#print('--->',img.shape)
socket_pub.send_multipart([topic,struct.pack('lll',*img.shape),img.tostring()])
#socket_pub.send_multipart([topic,pickle.dumps(img,-1)])
if show_cv:
cv2.imshow('drone camera %d'%drone_index,img)
cv2.waitKey(1)
def recv_multipart(self, socket, *args, **kwargs):
"""call recv_multipart in a way that raises if there is nothing to receive"""
r,w,x = zmq.select([socket], [], [], timeout=5)
assert len(r) > 0, "Should have received a message"
return socket.recv_multipart(*args, **kwargs)
def backend_send(self, *args):
self.assertEqual(([], [self.chatout], []),
zmq.select([], [self.chatout], [],
timeout=self.timeout))
self.chatout.send_multipart(
[a if isinstance(a, bytes) else a.encode('utf-8') for a in args])
socket.connect("tcp://%s" % ip)
socket.setsockopt(zmq.SUBSCRIBE, '')
sockets.append(socket)
time.sleep(1)
count = 1
f = open("results.dat", "w+")
f.write("time \t pi1 \t pi2 \t pi3 \t pi4 \t hive \n")
final = ""
try:
while True:
amps = {}
slab = {}
readable, writable, exceptional = zmq.select(sockets, [], [])
for socket in readable:
topic = socket.recv()
data = socket.recv()
print "-----"
print get_amplitude(data)
amps[socket] = 20 * math.log10(get_amplitude(data) / float(2**15))
slab[socket] = data
print "============="
print ">>>>>" + str(count)
print amps.values()
maxamp = max(amps.values())
print maxamp
def main():
flock = FlockManager(
match_found,
config["BLEET_TIMEOUT"],
config["SERVICE_TIMEOUT"]
)
logger.info("Shepherd starting.")
while True:
# Wait until either the public or sheep socket has messages waiting
zmq.select([public, sheep], [], [], timeout = 5)
# Will grab all of the outstanding messages from the outside and place them
# in the request queue
while public.getsockopt(zmq.EVENTS) & zmq.POLLIN != 0:
request = public.recv_json()
logger.debug("Raw test request: %s", str(request))
request = TestRequest.from_dict(request)
try:
submission = \
Submission.objects.get(id = ObjectId(request.submission_id))
except Submission.DoesNotExist as e:
logger.warning(
"Received test request for non-existant submission [%s].",
str(request.submission_id)
)
continue
except bson.errors.InvalidId as e:
logger.warning("Received malformed test request. %s", str(e))
continue
try:
assignment = Assignment.objects.get(id = submission.assignment)
except Assignment.DoesNotExist as e:
logger.error(
"Received test request for a submission [%s] referencing "
"an invalid assignment [%s].",
str(submission.id),
str(submission.assignment)
)
continue
if not assignment.test_harness:
logger.warning(
"Received test request for a submission [%s] referencing "
"an assignment [%s] that does not have a test harness "
"associated with it.",
str(submission.id),
str(submission.assignment)
)
continue
try:
test_harness = \
TestHarness.objects.get(id = assignment.test_harness)
except TestHarness.DoesNotExit as e:
logger.error(
"Received test request for a submission [%s] referencing "
"an assignment [%s] that references a non-existant test "
"harness [%s].",
str(submission.id),
str(submission.assignment),
str(assignment.test_harness)
)
continue
# Gather all the necessary information from the test request
# received from the outside.
processed_request = InternalTestRequest(
submission.id,
test_harness.config.get("galah/timeout",
config["BLEET_TIMEOUT"].seconds),
test_harness.config.get("galah/environment", {})
)
logger.info("Received test request.")
flock.received_request(processed_request)
# Will grab all of the outstanding messages from the sheep and process them
while sheep.getsockopt(zmq.EVENTS) & zmq.POLLIN != 0:
try:
sheep_identity, sheep_message = router_recv_json(sheep)
sheep_message = FlockMessage.from_dict(sheep_message)
logger.debug(
"Received message from sheep: %s",
str(sheep_message)
)
except ValueError as e:
logger.error("Could not decode sheep's message: %s", str(e))
logger.debug(
"Exception thrown while decoding sheep's message...",
exc_info = sys.exc_info()
)
continue
if sheep_message.type == "distress":
logger.warn("Received distress message. Sending bloot.")
router_send_json(
sheep, sheep_identity, FlockMessage("bloot", "").to_dict()
)
elif sheep_message.type == "bleet":
logger.debug(
"Sheep [%s] bleeted. Sending bloot.",
repr(sheep_identity)
)
result = flock.sheep_bleeted(sheep_identity)
# Under certain circumstances we want to completely ignore a
# bleet (see FlockManager.sheep_bleeted() for more details)
if result is FlockManager.IGNORE:
logger.debug("Ignoring bleet.")
continue
if not result:
router_send_json(
sheep,
sheep_identity,
FlockMessage("identify", "").to_dict()
)
logger.info(
"Unrecognized sheep [%s] connected, identify sent.",
repr(sheep_identity)
)
continue
router_send_json(
sheep,
sheep_identity,
FlockMessage("bloot", "").to_dict()
)
elif sheep_message.type == "environment":
if not flock.manage_sheep(sheep_identity, sheep_message.body):
logger.warn(
"Received environment from an already-recognized sheep."
)
elif sheep_message.type == "result":
logger.info("Received test result from sheep.")
logger.debug(
"Received test result from sheep: %s",
str(sheep_message.body)
)
try:
submission_id = ObjectId(sheep_message.body["id"])
submission = Submission.objects.get(id = submission_id)
test_result = TestResult.from_dict(sheep_message.body)
try:
test_result.save()
except InvalidDocument:
logger.warn(
"Test result is too large for the database.",
exc_info = True
)
test_result = TestResult(failed = True)
test_result.save()
submission.test_results = test_result.id
submission.save()
except (InvalidId, Submission.DoesNotExist) as e:
logger.warn(
"Could not retrieve submission [%s] for test result "
"received from sheep [%s].",
str(submission_id),
repr(sheep_identity)
)
continue
router_send_json(
sheep,
sheep_identity,
FlockMessage(
"bloot", sheep_message.body["id"]
).to_dict()
)
if not flock.sheep_finished(sheep_identity):
logger.info(
"Got result from sheep [%s] who was not processing "
"a test request.",
repr(sheep_identity)
)
# Let the flock manager get rid of any dead or killed sheep.
lost_sheep, killed_sheep = flock.cleanup()
if lost_sheep:
logger.warn(
"%d sheep lost due to bleet timeout: %s",
len(lost_sheep),
str([repr(i) for i in lost_sheep])
)
if killed_sheep:
logger.warn(
"%d sheep lost due to request timeout: %s",
len(killed_sheep),
str([repr(i) for i in killed_sheep])
)
def update_graph(axes):
global hdl_pos, hdl_arrow, ch, sh
tic = time.time()
new_data = False
while 1:
socks = zmq.select(subs_socks, [], [], 0.001)[0]
if time.time() - tic >= 0.09:
print('too much time break', time.time() - tic())
break
if len(socks) == 0:
break
else:
for sock in socks:
ret = sock.recv_multipart()
topic, data = ret
#print('--- topic ---',topic)
data = pickle.loads(ret[1])
if topic == zmq_topics.topic_tracker:
#new_data=True
new_ref = False
keys = ['valid', 'pt_l', 'pt_r', 'range', 'ref_cnt']
#print([(i,data[i]) for i in keys])
new_ref = data['ref_cnt'] != tin_data.get('ref_cnt', -1)
for k in keys:
tin_data[k] = data[k]
pickle.dump(tin_data, fd)
#print(tin_data)
new_data = True
ypr = (tin_data['yaw'], tin_data['pitch'],
tin_data['roll'])
xy = tin_data['pt_l']
ret = tracer.feed(tin_data['sonar'][0], new_ref, ypr,
xy[0], xy[1])
ret = (ret[1], -ret[0])
gdata.pos_hist.add(ret)
gdata.trace_hist.add(ret)
gdata.curr_pos = ret
gdata.heading_rot = tin_data['yaw']
print('---', ret)
if topic == zmq_topics.topic_imu:
for k in ['yaw', 'pitch', 'roll']:
tin_data[k] = data[k]
if topic == zmq_topics.topic_sonar:
tin_data['sonar'] = (data[0] / 100.0, data[1] / 100.0)
gdata.range_arr.add(tin_data['sonar'][0])
#toprint=['valid','pt_l','pt_r','range']
#print('--imu--',data)
if not pause_satus and new_data:
xs = np.arange(len(gdata.trace_hist))
pos_arr = gdata.pos_hist()
hdl_pos[0].set_ydata(pos_arr[:, 1])
hdl_pos[0].set_xdata(pos_arr[:, 0])
#hdl_last_pos
for i in [0, 1]:
hdl_trace[i][0].set_xdata(xs)
hdl_trace[i][0].set_ydata(gdata.trace_hist()[:, i])
ax2.set_xlim(len(gdata.trace_hist) - 100, len(gdata.trace_hist))
ax2.set_ylim(-0.2 * 4, 0.2 * 4)
hdl_arrow.remove()
hdl_arrow = ax1.arrow(gdata.curr_pos[0],
gdata.curr_pos[1],
-ch * 0.1,
-sh * 0.1,
width=0.3)
cx, cy = gdata.map_center[:2]
ax1.set_xlim(-rad + cx, rad + cx)
ax1.set_ylim(-rad + cy, rad + cy)
xs = np.arange(len(gdata.range_arr))
hdl_range[0][0].set_xdata(xs)
#print(pos_arr[:,2][-3:])
hdl_range[0][0].set_ydata(gdata.range_arr.buf)
ax3.set_xlim(len(xs) - 100, len(xs))
ax3.set_ylim(0, 2)
axes.figure.canvas.draw()
control_socket = context.socket(zmq.PUB)
control_socket.connect("tcp://192.168.8.106:5556")
print('connected to landrov server')
############ main loop ##################
start_time = time.time()
while 1:
k = cv2.waitKey(20)
#if sensor_socket.poll(0.001):
if k != -1:
if k == 27 or k == ord('q'):
break
if len(zmq.select([sensor_socket], [], [], 0)[0]):
topic, buf = sensor_socket.recv_multipart()
#print('got topic',topic)
if topic == b'rgbimage':
img = cv2.imdecode(np.fromstring(buf, dtype=np.uint8),
cv2.IMREAD_COLOR)
cv2.imshow('img', img)
if topic == b'depthimage':
depth = cv2.imdecode(np.fromstring(buf, dtype=np.uint8),
cv2.IMREAD_GRAYSCALE)
color_depth = cv2.applyColorMap(depth, cv2.COLORMAP_JET)
cv2.imshow('depth', color_depth)
tdiff = time.time() - start_time
fact = 0.6
def tick(self):
for sock in zmq.select([self.result_pull, self.router], [], [])[0]:
if sock is self.router:
self.recv_request()
elif sock is self.result_pull:
self.recv_chunk_result()
def run_cell(self, cell, cell_index=0, store_history=False):
parent_msg_id = self.kc.execute(cell.source,
store_history=store_history,
stop_on_error=not self.allow_errors)
self.log.debug("Executing cell:\n%s", cell.source)
exec_timeout = self._get_timeout(cell)
deadline = None
if exec_timeout is not None:
deadline = monotonic() + exec_timeout
cell.outputs = []
self.clear_before_next_output = False
# we need to have a reply, and return to idle before we can consider the cell executed
idle = False
execute_reply = None
deadline_passed = 0
deadline_passed_max = 5
while not idle or execute_reply is None:
# we want to timeout regularly, to see if the kernel is still alive
# this is tested in preprocessors/test/test_execute.py#test_kernel_death
# this actually fakes the kernel death, and we might be able to use the xlist
# to detect a disconnected kernel
timeout = min(1, deadline - monotonic())
# if we interrupt on timeout, we allow 1 seconds to pass till deadline
# to make sure we get the interrupt message
if timeout >= 0.0:
# we include 0, which simply is a poll to see if we have messages left
rlist = zmq.select([
self.kc.iopub_channel.socket, self.kc.shell_channel.socket
], [], [], timeout)[0]
if not rlist:
self._check_alive()
if monotonic() > deadline:
self._handle_timeout(exec_timeout)
deadline_passed += 1
assert self.interrupt_on_timeout
if deadline_passed <= deadline_passed_max:
# when we interrupt, we want to do this multiple times, so we give some
# extra time to handle the interrupt message
deadline += self.iopub_timeout
if self.kc.shell_channel.socket in rlist:
msg = self.kc.shell_channel.get_msg(block=False)
if msg['parent_header'].get('msg_id') == parent_msg_id:
execute_reply = msg
if self.kc.iopub_channel.socket in rlist:
msg = self.kc.iopub_channel.get_msg(block=False)
if msg['parent_header'].get('msg_id') == parent_msg_id:
if msg['msg_type'] == 'status' and msg['content'][
'execution_state'] == 'idle':
idle = True
else:
self.process_message(msg, cell, cell_index)
else:
self.log.debug(
"Received message for which we were not the parent: %s",
msg)
else:
self._handle_timeout(exec_timeout)
break
return execute_reply, cell.outputs
async def recv_and_process():
keep_running = True
pitch, roll = 0, 0
target_range = 0
pid = None
ab = None
rate = 0
system_state = {'mode': []}
jm = Joy_map()
while keep_running:
socks = zmq.select(subs_socks, [], [], 0.005)[0]
for sock in socks:
ret = sock.recv_multipart()
topic, data = ret[0], pickle.loads(ret[1])
if topic == zmq_topics.topic_sonar:
new_sonar_ts, range = data[
'ts'], data['sonar'][0] / 1000 # Convert to m
if ab is None:
ab = ab_filt([range, 0])
else:
depth, rate = ab(range, new_sonar_ts - sonar_ts)
sonar_ts = new_sonar_ts
if 'SONAR_HOLD' in system_state['mode']:
if pid is None:
pid = PID(**sonar_pid)
else:
ud_command = -pid(range, target_range, rate, 0)
debug_pid = {
'P': pid.p,
'I': pid.i,
'D': pid.d,
'C': ud_command,
'T': target_range,
'N': range,
'TS': new_sonar_ts
}
pub_sock.send_multipart([
zmq_topics.topic_sonar_hold_pid,
pickle.dumps(debug_pid, -1)
])
thruster_cmd = mixer.mix(ud_command, 0, 0, 0, 0, 0,
pitch, roll)
thrusters_source.send_pyobj(
['sonar', time.time(), thruster_cmd])
else:
if pid is not None:
pid.reset()
thrusters_source.send_pyobj(
['sonar', time.time(),
mixer.zero_cmd()])
target_range = range
if topic == zmq_topics.topic_axes:
jm.update_axis(data)
target_range += -jm.joy_mix()['ud'] / 25.0
if topic == zmq_topics.topic_imu:
pitch, roll = data['pitch'], data['roll']
if topic == zmq_topics.topic_system_state:
_, system_state = data
await asyncio.sleep(0.001)
def main():
############ connecting to landrov ##################
context = zmq.Context()
# sensor_socket = context.socket(zmq.SUB)
# sensor_socket.setsockopt(zmq.CONFLATE, 1)
# sensor_socket.connect("tcp://192.168.8.106:5557")
# sensor_socket.setsockopt(zmq.SUBSCRIBE,b'pointcloud')
# sensor_socket.setsockopt(zmq.SUBSCRIBE,b'rgbimage')
control_socket = context.socket(zmq.PUB)
control_socket.connect("tcp://192.168.8.106:5556")
print('connected to landrov server')
############ main loop ##################
#initialize Kernal
kernel = np.ones((5, 5), np.uint8)
# Initialize the PCL visualizer.
# visual_2 = pcl.pcl_visualization.CloudViewing()
visual = pcl.pcl_visualization.PCLVisualizering(
'3D Viewer') # Initialize the visualizer.
depth_scale = 0.0010000000474974513 + 0.03
# depth_scale = 0.03
#Create PointCloud
pc = rs.pointcloud()
clipping_distance_in_meters = 2
clipping_distance = clipping_distance_in_meters / depth_scale
found_cloud = False
updated_cloud = False
cnt = 0
left_turns = 0
right_turns = 0
while 1:
# k=cv2.waitKey(20)
# #if sensor_socket.poll(0.001):
#
# if k!=-1:
# if k == 27 or k == ord('q'):
# break
# # import pdb; pdb.set_trace()
# # cv2.imshow('img', color)
if not found_cloud:
time.sleep(0.5)
sensor_socket = context.socket(zmq.SUB)
# sensor_socket.setsockopt(zmq.CONFLATE, 1)
sensor_socket.connect("tcp://192.168.8.106:5557")
sensor_socket.setsockopt(zmq.SUBSCRIBE, b'pointcloud')
while not found_cloud:
if len(zmq.select([sensor_socket], [], [], 0)[0]):
topic, buf = sensor_socket.recv_multipart()
# print('got topic',topic)
if topic == b'pointcloud':
cloud_vtx = np.fromstring(buf,
dtype=[('f0', '<f4'),
('f1', '<f4'),
('f2', '<f4')])
found_cloud = True
# print("Recieved PC")
sensor_socket.close()
else:
time_start = time.time()
print("Original Point Cloud Size:", cloud.size, "points")
# import pdb; pdb.set_trace()
cloud = pcl.PointCloud()
cloud.from_list(cloud_vtx)
# Removes points outside of the range 0.1 to 1.5 in the Z axis.
cloud_filtered = cloud_filter(cloud, "z", 0.1, 1.2)
#Applies a voxel grid to the cloud filter
voxel_cloud = voxel_filter(cloud_filtered, 0.04, 0.04, 0.04)
print("Voxel filter Cloud Size:", voxel_cloud.size, "points")
# Remove the ground plane inrfont of the robot
ground_removed = remove_ground(voxel_cloud)
# objects = cluster_extraction(ground_removed, 0.05, 20, 1000)
#
# closest_object_index = find_closest(objects)
#
# closest_object = objects[closest_object_index]
# Create a passthrough filter shows points infront of the robot
path_objects = cloud_filter(ground_removed, "x", -0.3, 0.3)
# Create a passthrough filter shows points left of the robot
left_objects = cloud_filter(ground_removed, "x", -2, -0.3)
# Create a passthrough filter shows points right of the robot
right_objects = cloud_filter(ground_removed, "x", 0.3, 2)
if path_objects.size > 10:
if left_objects.size > right_objects.size:
command = "right"
else:
command = "left"
else:
command = "forward"
visual.RemoveAllPointClouds(0)
template = "Points left {},Points infront {}, points right {}"
print(
template.format(left_objects.size, path_objects.size,
right_objects.size))
path_color = pcl.pcl_visualization.PointCloudColorHandleringCustom(
path_objects, 0, 0, 255)
left_color = pcl.pcl_visualization.PointCloudColorHandleringCustom(
left_objects, 255, 0, 0)
right_color = pcl.pcl_visualization.PointCloudColorHandleringCustom(
right_objects, 0, 255, 0)
visual.AddPointCloud_ColorHandler(left_objects, left_color,
b'outliers')
visual.AddPointCloud_ColorHandler(right_objects, right_color,
b'inliers')
visual.AddPointCloud_ColorHandler(path_objects, path_color,
b'original')
print(command)
# # Provide a colour for the point cloud.
# cloud_color = pcl.pcl_visualization.PointCloudColorHandleringCustom(closest_object, 255, 255, 255)
# # Display the point cloud.
# visual.AddPointCloud_ColorHandler(closest_object, cloud_color, b'ground_removed')
print("Time taken", time.time() - time_start)
cnt += 1
updated_cloud = True
# import pdb; pdb.set_trace()
# import pdb; pdb.set_trace()
# Navigate Robot
if updated_cloud:
fact = 0.6
if command == "forward":
cmd = (fact, fact) # straight
travel_time = 3.0
print("Going Forward")
elif command == "right":
cmd = (fact, -fact) # turn right
travel_time = 0.5
print("Turn Right")
elif command == "left":
cmd = (-fact, fact) # turn left
travel_time = 0.5
print("Turn Left")
else:
print("No Command")
send_command(cmd, travel_time)
found_cloud = False
updated_cloud = False
def listener():
cnt = 0
rgb = None
while 1:
while len(zmq.select([zmq_sub], [], [], 0.001)[0]) > 0:
data = zmq_sub.recv_multipart()
topic = data[0]
frame_cnt, shape = pickle.loads(data[1])
if topic == topic_stereo:
imgl = np.frombuffer(data[2], 'uint8').reshape(shape)
imgr = np.frombuffer(data[3], 'uint8').reshape(shape)
ch, cw, cl = shape
ch = ch // 2
cw = cw // 2
if config.rov_type == 2: #resizing for prev cameras
nh, nw = config.cam_res_rgby, config.cam_res_rgbx
imgl = imgl[ch - nh // 2:ch + nh // 2,
cw - nw // 2:cw + nw // 2, :]
imgr = imgr[ch - nh // 2:ch + nh // 2,
cw - nw // 2:cw + nw // 2, :]
shape = imgl.shape
if 0 and cvshow:
#if 'depth' in topic:
# cv2.imshow(topic,img)
#else:
#cv2.imshow(topic,cv2.resize(cv2.resize(img,(1920/2,1080/2)),(512,512)))
imgls = imgl[::2, ::2]
imgrs = imgr[::2, ::2]
cv2.imshow(topic.decode() + 'l', imgls)
cv2.imshow(topic.decode() + 'r', imgrs)
cv2.waitKey(1)
zmq_pub.send_multipart([
zmq_topics.topic_stereo_camera,
pickle.dumps([frame_cnt, shape]),
imgl.tostring(),
imgr.tostring()
])
time.sleep(0.001)
zmq_pub.send_multipart([
zmq_topics.topic_stereo_camera_ts,
pickle.dumps((frame_cnt, time.time()))
]) #for sync
if topic == topic_depth:
img = np.frombuffer(data[2], 'float16').reshape(shape)
min_range = img.min()
#import pdb;pdb.set_trace()
img = np.squeeze(img).copy()
#img.byteswap()
img_show = (img / 10.0).clip(0, 255).astype('uint8')
img[img > 5000] = np.nan
max_range = np.nanmax(img)
print('sonar::', min_range, max_range)
pub_sonar.send_multipart([
zmq_topics.topic_sonar,
pickle.dumps([min_range, max_range])
])
if cvshow:
cv2.imshow(topic.decode(), img_show)
cv2.waitKey(1)
### test
time.sleep(0.010)
if cnt % 20 == 0 and rgb is not None:
print('send...', cnt, rgb.shape)
cnt += 1
async def recv_and_process():
keep_running = True
tracker = sTracker.tracker()
tracker.init()
imBuffer = {}
maxImBuffer = 20
system_state = {'mode': []}
trackerInitiated = False
curYaw, curPitch, curRoll = 0.0, 0.0, 0.0
rates = [0] * 3
## IDS
fovX = 40.58 # deg.
fovY = 30.23 # deg.
pidY = None
pidP = None
pidR = None
pidX = None
tPitch = 0.0
tRoll = 0.0
tYaw = 0.0
tX = 0.0
maxAllowedPitch = 15 # deg
while keep_running:
socks = zmq.select(subs_socks, [], [], 0.005)[0]
for sock in socks:
ret = sock.recv_multipart()
topic, data = ret[0], pickle.loads(ret[1])
if topic == zmq_topics.topic_stereo_camera:
frameCnt, shape, ts, curExp, hasHighRes = pickle.loads(ret[1])
frame = np.frombuffer(ret[-2], 'uint8').reshape(
(shape[0] // 2, shape[1] // 2, 3)).copy()
if 'IM_TRACKER_MODE' in system_state[
'mode'] and trackerInitiated:
trackRes = tracker.track(frame)
#print('--->', trackRes)
if tracker.trackerInit:
if trackRes is not None:
msg = [
zmq_topics.topic_tracker_result,
pickle.dumps([frameCnt, trackRes])
]
sock_pub.send_multipart(msg)
centerX = frame.shape[1] // 2
centerY = frame.shape[0] // 2
iFovX = fovX / frame.shape[1]
iFovY = fovY / frame.shape[0]
#import ipdb; ipdb.set_trace()
trckResCenteredX = (trackRes[0] - centerX) * iFovX
trckResCenteredY = -(trackRes[1] - centerY) * iFovY
#print('--> dx=%f, dy=%f'%(trckResCenteredX-curYaw, trckResCenteredY+curPitch) )
if pidX == None:
tYaw = curYaw
pidX = PID(**pos_pid_x)
pidY = PID(**yaw_pid)
pidP = PID(**pitch_im_pid)
pidR = PID(**roll_pid)
tPitch = curPitch + trckResCenteredY
print('-1-> tP: %.2f' % tPitch)
tPitch = min(max(tPitch, -maxAllowedPitch),
maxAllowedPitch)
print('-2-> tP: %.2f' % tPitch)
tX = centerX - trackRes[0]
#print('--> dx=%f, dp=%f'%(trackRes[0]-centerX, tPitch-curPitch) )
pitchCmd = pidP(curPitch, tPitch, 0, 0)
rollCmd = 0 #pidR(curRoll, tRoll, 0, 0)
yawCmd = pidY(curYaw, tYaw, 0, 0)
ts = time.time()
debug_pid = {
'P': pidR.p,
'I': pidR.i,
'D': pidR.d,
'C': rollCmd,
'T': tRoll,
'N': curRoll,
'R': rates[0],
'TS': ts
}
pub_sock.send_multipart([
zmq_topics.topic_att_hold_roll_pid,
pickle.dumps(debug_pid, -1)
])
debug_pid = {
'P': pidP.p,
'I': pidP.i,
'D': pidP.d,
'C': pitchCmd,
'T': tPitch,
'N': curPitch,
'R': rates[1],
'TS': ts
}
pub_sock.send_multipart([
zmq_topics.topic_att_hold_pitch_pid,
pickle.dumps(debug_pid, -1)
])
debug_pid = {
'P': pidY.p,
'I': pidY.i,
'D': pidY.d,
'C': yawCmd,
'T': tYaw,
'N': curYaw,
'R': rates[2],
'TS': ts
}
pub_sock.send_multipart([
zmq_topics.topic_att_hold_yaw_pid,
pickle.dumps(debug_pid, -1)
])
xCmd = pidX(tX, 0)
#print('xCmd: %f tx: %f'%(xCmd, tX), pidX.p, pidX.i, pidX.d)
thrusterCmd = np.array(
mixer.mix(0, xCmd, 0, rollCmd, pitchCmd,
yawCmd, curPitch, curRoll))
#print( {'P':pidX.p,'I':pidX.i,'D':pidX.d,'C':xCmd,'T':tX,'N':0, 'TS':ts} )
thrusterCmd += mixer.mix(0, 0, 0, 0,
-rates[1] * pidP.D,
-rates[2] * pidY.D, 0, 0)
thrusters_source.send_pyobj(
['trck', time.time(), thrusterCmd])
else:
print('oitracker break...')
trackerInitiated = False
sock_pub.send_multipart([
zmq_topics.topic_tracker_result,
pickle.dumps([frameCnt, (-1, -1)])
])
if pidX is not None:
print('kill controllers...')
pidY.reset(), pidP.reset(), pidX.reset()
pidY = None
pidP = None
pidX = None
thrusters_source.send_pyobj(
['trck', time.time(),
mixer.zero_cmd()])
if topic == zmq_topics.topic_imu:
curYaw, curPitch, curRoll = data['yaw'], data['pitch'], data[
'roll']
rates = [x / np.pi * 180 for x in data['rates']]
elif topic == zmq_topics.topic_tracker_cmd:
#print('-->', data)
if data['frameId'] == -1:
tracker.stopTracker()
trackerInitiated = False
sock_pub.send_multipart([
zmq_topics.topic_tracker_result,
pickle.dumps([-1, [-1, -1]])
])
if pidX is not None:
pidY.reset(), pidP.reset(), pidX.reset()
pidY = None
pidP = None
pidX = None
thrusters_source.send_pyobj(
['trck', time.time(),
mixer.zero_cmd()])
print('got kill tracker...')
else:
trackerInitiated = tracker.initTracker(data['trackPnt'])
trackRes = tracker.track(frame)
print('--->', trackerInitiated, data['trackPnt'])
if topic == zmq_topics.topic_system_state:
system_state = data
await asyncio.sleep(0.001)
def prompt_restart_rerun_buttons(pkgs):
"""
Jupyter-specific implementation of `prompt_restart_rerun_buttons`.
Displays a warning that the notebook kernel must be restarted in
order to use the just-smuggled version of one or more previously
imported `pkgs`, and displays a pair of buttons (via
`JS_FUNCTIONS.jupyter.displayButtonPrompt`) that prompt the user to
either (a) restart the kernel and rerun all cells up to the current
point, or (b) ignore the warning and continue running. Then, polls
the kernel's stdin socket until it receives a reply from the
notebook frontend, or the kernel is restarted.
Parameters
----------
pkgs : list of str
Packages that could not be reloaded without restarting the
kernel.
Returns
-------
None
If the user clicks the "Continue Running" button, returns
`None`. Otherwise, restarts the kernel and therefore never
returns.
See Also
--------
JS_FUNCTIONS.jupyter.displayButtonPrompt :
JavaScript function for prompting user input with buttons.
ipykernel.kernelbase.Kernel._input_request :
Kernel method that replaces the built-in `input` in notebooks.
Notes
-----
1. This method of blocking and waiting for user input is based on
`ipykernel`'s replacement for the built-in `input` function used
in notebook environments.
"""
# UI: could remove warning message when "continue" button is clicked
msg = (
"WARNING: The following packages were previously imported by the "
"interpreter and could not be reloaded because their compiled modules "
f"have changed:\n\t[{', '.join(pkgs)}]\nRestart the kernel to use "
"the newly installed version.")
# noinspection JSUnusedLocalSymbols,JSUnresolvedFunction
button_args = dedent("""
const buttonArgs = [
{
text: 'Restart Kernel and Rerun Cells',
onClick: () => {restartRunCellsAbove();},
},
{
text: 'Continue Running',
result: null,
},
]
""")
display_button_prompt_full = dedent(f"""
{JS_FUNCTIONS.jupyter.restartRunCellsAbove};
console.log('restartRunCellsAbove defined');
{JS_FUNCTIONS.jupyter.displayButtonPrompt};
console.log('displayButtonPrompt defined');
{button_args};
console.warn(`{msg}`);
displayButtonPrompt(buttonArgs, true);
""")
# get_ipython() exists globally when imported into IPython context
kernel = get_ipython().kernel
stdin_sock = kernel.stdin_socket
print(f"\033[0;31;1m{msg}\033[0m")
# flush output before creating button display
sys.stdout.flush()
sys.stderr.flush()
# flush ipykernel stdin socket to purge stale replies
while True:
try:
# noinspection PyUnresolvedReferences
# (dynamically imported names not included in stub files)
stdin_sock.recv_multipart(zmq.NOBLOCK)
except zmq.ZMQError as e:
# noinspection PyUnresolvedReferences
# (dynamically imported names not included in stub files)
if e.errno == zmq.EAGAIN:
break
raise
# noinspection PyTypeChecker
display(Javascript(display_button_prompt_full))
while True:
try:
# zmq.select (zmq.sugar.poll.select) args:
# - list of sockets/FDs to be polled for read events
# - list of sockets/FDs to be polled for write events
# - list of sockets/FDs to be polled for error events
# - timeout (in seconds; None implies no timeout)
rlist, _, xlist = zmq.select([stdin_sock], [], [stdin_sock], 0.01)
if rlist or xlist:
ident, reply = kernel.session.recv(stdin_sock)
if ident is not None or reply is not None:
break
except Exception as e: # pylint: disable=broad-except
if isinstance(e, KeyboardInterrupt):
# re-raise KeyboardInterrupt with simplified traceback
# (excludes some convoluted calls to internal
# IPython/zmq machinery)
raise KeyboardInterrupt("Interrupted by user") from None
kernel.log.warning("Invalid Message:", exc_info=True)
# noinspection PyBroadException
try:
value = reply['content']['value']
except Exception: # pylint: disable=broad-except
if ipykernel.version_info[0] >= 6:
_parent_header = kernel._parent_ident['shell']
else:
_parent_header = kernel._parent_ident
kernel.log.error(f"Bad input_reply: {_parent_header}")
value = ''
if value == '\x04':
# end of transmission
raise EOFError
return value
def send(self, msg, timeout=10.0):
r, w, x = zmq.select([], [self.s], [], timeout)
if w:
self.s.send(codecs.encode(json.dumps(msg), 'utf8'))
def main_loop(gworld):
frame_cnt = 0
print('-- actors list --', gworld)
i = 0
for p in ph.GetActorsNames(gworld, 1024 * 1000):
p_wc = p[1::2]
a = ph.FindActorByName(gworld, p_wc)
print(i, p,
ph.GetActorLocation(a)
if a else a) #,'pos',ph.GetActorLocation(a))
if 0:
import pdb
pdb.set_trace()
i += 1
#print(p,'pos',ph.GetActorLocation(a))
print('-- textures --')
print('-- starting openrov simulation --')
drone_textures = []
for tn in drone_texture_names:
drone_textures.append(ph.GetTextureByName(tn))
drone_textures_depth = []
for tn in drone_textures_depth_names:
drone_textures_depth.append(ph.GetTextureByName(tn))
if not all(drone_textures):
print("Error, Could not find all textures")
while 1:
yield
drone_actors = []
for drn in drone_actors_names:
drone_actors.append(ph.FindActorByName(gworld, drn))
if not all(drone_actors):
print("Error, Could not find all drone actors")
while 1:
yield
#change cameras angle
for cam_name in ['SceneCaptureBROV1left', 'SceneCaptureBROV1right']:
#ca=ph.FindActorByName(gworld,'SceneCaptureBROV1left')
ca = ph.FindActorByName(gworld, cam_name)
#ph.SetActorRotation(ca,(1,-1,1)) #left
#ph.SetActorRotation(ca,(1,-1,89)) #forward
#ph.SetActorRotation(ca,(1,-89,89)) #down (pitch -90)
#pitch range >=1 <=89
#pitch=45
#pitch=1
ph.SetActorRotation(ca, (1, -pitch, 89))
yield
#ph.SetActorRotation(caml,(1,1,89)) #facing down
print('camera ' + cam_name + ' rotation ---', ph.GetActorRotation(ca))
#print('--- ',caml)
for _ in range(10): #need to send it a few time don't know why.
print('sending state main loop')
socket_pub.send_multipart([config.topic_unreal_state, b'main_loop'])
yield
print('initial_pos is ', initial_pos)
drone_start_positions = [
np.array(
ph.GetActorLocation(drone_actor
) if initial_pos is None else initial_pos)
for drone_actor in drone_actors
]
positions = [None for _ in range(config.n_drones)]
while 1:
for drone_index in range(config.n_drones):
socket_sub = drone_subs[drone_index]
drone_actor = drone_actors[drone_index]
while len(zmq.select([socket_sub], [], [], 0)[0]) > 0:
topic, msg = socket_sub.recv_multipart()
positions[drone_index] = pickle.loads(msg)
#print('-----',positions[drone_index])
position = positions[drone_index]
if position is not None:
new_pos = drone_start_positions[drone_index] + np.array([
position['posx'], position['posy'], position['posz']
]) * 100 #turn to cm
#print('-----',drone_index,position)
ph.SetActorLocation(drone_actor, new_pos)
ph.SetActorRotation(
drone_actor,
(position['roll'], position['pitch'], position['yaw']))
positions[drone_index] = None
yield
#for drone_index in range(config.n_drones):
#img=cv2.resize(ph.GetTextureData(drone_textures[drone_index]),(1024,1024),cv2.INTER_LINEAR)
topics = []
imgs = []
img = ph.GetTextureData(drone_textures[0])
imgr = ph.GetTextureData(drone_textures[1])
imgl = ph.GetTextureData(drone_textures[2])
topics.append(config.topic_unreal_drone_rgb_camera % 0)
topics.append(config.topic_unreal_drone_rgb_camera % 0 + b'l')
topics.append(config.topic_unreal_drone_rgb_camera % 0 + b'r')
imgs = [img, imgl, imgr]
#if drone_index<len(drone_textures_depth):
# img_depth=ph.GetTextureData16f(drone_textures_depth[drone_index],channels=[0,1,2,3]) #depth data will be in A componnent
#img_depth=ph.GetTextureData(drone_textures_depth[drone_index],channels=[2]) #depth data will be in red componnent
# topics.append(config.topic_unreal_drone_rgb_camera%drone_index+b'depth')
# imgs.append(img_depth)
if pub_cv:
for topic, img in zip(topics, imgs):
#socket_pub.send_multipart([topic,pickle.dumps(img,2)])
#print('--->',img[:].max(),img[:].min())
#rgb=img[...,::-1]
socket_pub.send_multipart([
topic,
struct.pack('llll', *img.shape, frame_cnt),
img.tostring()
])
#socket_pub.send_multipart([topic,pickle.dumps(img,-1)])
if show_cv:
cv2.imshow('drone camera %d' % drone_index, img)
cv2.waitKey(1)
frame_cnt += 1
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
thrustersRate = 50.0
thrustersTimerHz = time.time() + 1 / thrustersRate
timer0_1hz = time.time() + 10
initPwmFocus = 1400
preFocusFileName = '../hw/focusState.bin'
if os.path.exists(preFocusFileName):
with open(preFocusFileName, 'r') as fid:
initPwmFocus = int(fid.read(4))
print('reset focus to:', initPwmFocus)
gainCtl = -1
expCtl = -1
camStateFile = '../hw/camSate.pkl'
if os.path.exists(camStateFile):
try:
with open(camStateFile, 'rb') as fid:
camState = pickle.load(fid)
if 'aGain' in camState.keys():
gainCtl = camState['aGain']
if 'aExp' in camState.keys():
expCtl = camState['aExp']
except:
print('no cam state...')
diskUsage = int(
os.popen('df -h / | tail -n 1').readline().strip().split()[-2][:-1])
system_state = {
'ts': time.time(),
'arm': False,
'mode': [],
'lights': 0,
'focus': initPwmFocus,
'record': False,
'diskUsage': diskUsage,
'autoGain': gainCtl,
'autoExp': expCtl
} #lights 0-5
thrusters_dict = {}
jm = Joy_map()
def togle_mode(m):
s = system_state
if m in s['mode']:
s['mode'].remove(m)
else:
s['mode'].append(m)
def test_togle(b):
return new_joy_buttons[b] == 1 and joy_buttons[b] == 0
while keep_running:
socks = zmq.select(subs_socks, [], [], 0.002)[0]
for sock in socks:
if sock == thruster_sink:
source, _, thruster_src_cmd = sock.recv_pyobj()
thrusters_dict[source] = thruster_src_cmd
else:
ret = sock.recv_multipart()
topic, data = ret[0], pickle.loads(ret[1])
if topic == zmq_topics.topic_button or topic == zmq_topics.topic_gui_diveModes:
jm.update_buttons(data)
if jm.depth_hold_event():
print('Toggle depth hold...')
if ('IM_TRACKER_MODE' in system_state['mode']) and (
'DEPTH_HOLD' in system_state['mode']):
print(
'failed to toggle to DEPTH_HOLD (needs to stay on DEPTH_HOLD) while IM_TRACKER_MODE'
)
else:
togle_mode('DEPTH_HOLD')
if jm.att_hold_event():
print('Toggle attitude hold...')
#if ('IM_TRACKER_MODE' in system_state['mode']) and ('ATT_HOLD' in system_state['mode']):
# print('failed to toggle to ATT_HOLD (needs to stay on ATT_HOLD) while IM_TRACKER_MODE')
#else:
# togle_mode('ATT_HOLD')
togle_mode('ATT_HOLD')
if jm.Rx_hold_event():
togle_mode('RX_HOLD')
if jm.Ry_hold_event():
togle_mode('RY_HOLD')
if jm.Rz_hold_event():
togle_mode('RZ_HOLD')
if jm.record_event():
system_state['record'] = not system_state['record']
print('record state', system_state['record'])
if jm.arm_event():
system_state['arm'] = not system_state['arm']
print('state arm:', system_state['arm'])
if not system_state['arm']:
system_state['mode'] = []
elif topic == zmq_topics.topic_axes or topic == zmq_topics.topic_gui_controller:
jm.update_axis(data)
if jm.inc_lights_event():
system_state['lights'] = min(
5, system_state['lights'] + 1)
pub_sock.send_multipart([
zmq_topics.topic_lights,
pickle.dumps(system_state['lights'])
])
print('lights set to ', system_state['lights'])
if jm.dec_lights_event():
system_state['lights'] = max(
0, system_state['lights'] - 1)
pub_sock.send_multipart([
zmq_topics.topic_lights,
pickle.dumps(system_state['lights'])
])
print('lights set to ', system_state['lights'])
if jm.inc_focus_event():
system_state['focus'] = min(
2250, system_state['focus'] + focusResolution)
pub_sock.send_multipart([
zmq_topics.topic_focus,
pickle.dumps(system_state['focus'])
])
print('focus set to ', system_state['focus'])
if jm.dec_focus_event():
system_state['focus'] = max(
850, system_state['focus'] - focusResolution)
pub_sock.send_multipart([
zmq_topics.topic_focus,
pickle.dumps(system_state['focus'])
])
print('focus set to ', system_state['focus'])
elif topic == zmq_topics.topic_gui_focus_controller or topic == zmq_topics.topic_autoFocus:
pwm = data
system_state['focus'] = pwm
pub_sock.send_multipart([
zmq_topics.topic_focus,
pickle.dumps(system_state['focus'])
])
print('focus set to value ', system_state['focus'])
elif topic == zmq_topics.topic_gui_autoFocus:
os.system('../scripts/runAutofocus.sh')
elif topic == zmq_topics.topic_gui_start_stop_track:
print('start/stop tracker... ', data)
'''
if 'ATT_HOLD' not in system_state['mode']:
togle_mode('ATT_HOLD')
'''
if 'DEPTH_HOLD' not in system_state['mode']:
togle_mode('DEPTH_HOLD')
togle_mode('IM_TRACKER_MODE')
if 'IM_TRACKER_MODE' in system_state['mode']:
pub_sock.send_multipart(
[zmq_topics.topic_tracker_cmd,
pickle.dumps(data)])
else:
pub_sock.send_multipart([
zmq_topics.topic_tracker_cmd,
pickle.dumps({
'frameId': -1,
'trackPnt': (-1, -1)
})
])
elif topic == zmq_topics.topic_tracker_result:
#print('--->', data)
if data[1][0] < 0:
if 'IM_TRACKER_MODE' in system_state['mode']:
print('Tracker ended...')
togle_mode('IM_TRACKER_MODE')
elif topic == zmq_topics.topic_gui_toggle_auto_exp:
print('got auto exposure command')
if expCtl == 0:
expCtl = 1
else:
expCtl = 0
system_state['autoExp'] = expCtl
camPubSock.send_multipart([
zmq_topics.topic_cam_toggle_auto_exp,
pickle.dumps(expCtl)
])
elif topic == zmq_topics.topic_gui_toggle_auto_gain:
print('got auto gain command')
if gainCtl == 0:
gainCtl = 1
else:
gainCtl = 0
system_state['autoGain'] = gainCtl
camPubSock.send_multipart([
zmq_topics.topic_cam_toggle_auto_gain,
pickle.dumps(gainCtl)
])
elif topic == zmq_topics.topic_gui_inc_exp:
print('got camera inc exp.')
camPubSock.send_multipart(
[zmq_topics.topic_cam_inc_exp,
pickle.dumps([0])])
elif topic == zmq_topics.topic_gui_dec_exp:
print('got camera dec exp.')
camPubSock.send_multipart(
[zmq_topics.topic_cam_dec_exp,
pickle.dumps([0])])
elif topic == zmq_topics.topic_gui_exposureVal:
print('got camera exp. value:', data)
camPubSock.send_multipart(
[zmq_topics.topic_cam_exp_val,
pickle.dumps(data)])
tic = time.time()
if tic - timer10hz > 0:
timer10hz = tic + 1 / 10.0
system_state['ts'] = tic
pub_sock.send_multipart(
[zmq_topics.topic_system_state,
pickle.dumps(system_state)])
if tic - thrustersTimerHz > 0:
thrustersTimerHz = tic + 1 / thrustersRate
if not system_state['arm']:
thruster_cmd = np.zeros(8)
else:
for k in thrusters_dict:
thruster_cmd += thrusters_dict[k]
pub_sock.send_multipart([
zmq_topics.topic_thrusters_comand,
pickle.dumps((tic, list(thruster_cmd)))
])
thruster_cmd = np.zeros(8)
if tic - timer0_1hz > 0:
timer0_1hz = tic + 10
system_state['diskUsage'] = int(
os.popen('df -h / | tail -n 1').readline().strip().split()[-2]
[:-1])
#print('botton',ret)
await asyncio.sleep(0.001)
async def recv_and_process():
keep_running=True
target_att=np.zeros(3)
pid_y,pid_p,pid_r=[None]*3
system_state={'mode':[]}
jm=Joy_map()
joy=None
while keep_running:
socks=zmq.select(subs_socks,[],[],0.005)[0]
for sock in socks:
ret=sock.recv_multipart()
topic,data=ret[0],pickle.loads(ret[1])
if topic==zmq_topics.topic_imu:
yaw,pitch,roll=data['yaw'],data['pitch'],data['roll']
if 0 and 'yawr' in data:
ans = (data['yawr'],data['pitchr'],data['rollr'])
yawr,pitchr,rollr=ans
else:
#ans=#mixer.from_ang_rates_to_euler_rates(yaw,pitch,roll,data['rates'])
#if ans is not None:
# yawr,pitchr,rollr=mixer.from_ang_rates_to_euler_rates(yaw,pitch,roll,data['rates'])
rates = [x/np.pi*180 for x in data['rates']]
joy = jm.joy_mix()
if 'ATT_HOLD' in system_state['mode']:# and ans is not None:
if pid_y is None:
pid_y=PID(**yaw_pid)
pid_p=PID(**pitch_pid)
pid_r=PID(**roll_pid)
else:
#if joy and joy['inertial'] and abs(joy['yaw'])<0.05:
if joy and abs(joy['yaw'])<0.05:
yaw_cmd = pid_y(yaw,target_att[0],0,0)
else:
target_att[0]=yaw
yaw_cmd=0
#print('R{:06.3f} Y{:06.3f} YT{:06.3f} C{:06.3f}'.format(yawr,yaw,target_att[0],yaw_cmd))
if joy and abs(joy['pitch'])<0.1:
pitch_cmd = pid_p(pitch,target_att[1],0,0)
else:
target_att[1]=pitch
pitch_cmd=0
#print('R{:06.3f} P{:06.3f} PT{:06.3f} C{:06.3f}'.format(pitchr,pitch,target_att[1],pitch_cmd))
roll_cmd = pid_r(roll,0 if roll_target_0 else target_att[2],0,0)
#print('RR{:06.3f} R{:06.3f} RT{:06.3f} C{:06.3f}'.format(rollr,roll,target_att[2],roll_cmd))
ts=time.time()
debug_pid = {'P':pid_r.p,'I':pid_r.i,'D':pid_r.d,'C':roll_cmd,'T':0,'N':roll, 'R':rates[0], 'TS':ts}
pub_sock.send_multipart([zmq_topics.topic_att_hold_roll_pid, pickle.dumps(debug_pid,-1)])
debug_pid = {'P':pid_p.p,'I':pid_p.i,'D':pid_p.d,'C':pitch_cmd,'T':target_att[1],'N':pitch, 'R':rates[1],'TS':ts}
pub_sock.send_multipart([zmq_topics.topic_att_hold_pitch_pid, pickle.dumps(debug_pid,-1)])
debug_pid = {'P':pid_y.p,'I':pid_y.i,'D':pid_y.d,'C':yaw_cmd,'T':target_att[0],'N':yaw, 'R':rates[2], 'TS':ts}
pub_sock.send_multipart([zmq_topics.topic_att_hold_yaw_pid, pickle.dumps(debug_pid,-1)])
thruster_cmd = np.array(mixer.mix(0,0,0,roll_cmd,pitch_cmd,yaw_cmd,pitch,roll))
thruster_cmd += mixer.mix(0,0,0,-rates[0]*pid_r.D,-rates[1]*pid_p.D,-rates[2]*pid_y.D,0,0)
thrusters_source.send_pyobj(['att',time.time(),thruster_cmd])
else:
if pid_y is not None:
pid_y.reset(),pid_r.reset(),pid_y.reset()
target_att=[yaw,0,0]
thrusters_source.send_pyobj(['att',time.time(),mixer.zero_cmd()])
if topic==zmq_topics.topic_axes:
jm.update_axis(data)
if topic==zmq_topics.topic_button:
jm.update_buttons(data)
#target_depth+=data[jm.ud]
if topic==zmq_topics.topic_system_state:
_,system_state=data
await asyncio.sleep(0.001)
# ph.GetTextureData(drone_textures_down[drone_index]),
# ph.GetTextureData(drone_textures_depth[drone_index],channels=[2])]
if pub_cv:
for topic,img in zip(topics,imgs):
#socket_pub.send_multipart([topic,pickle.dumps(img,2)])
#print('--->',img.shape)
socket_pub.send_multipart([topic,struct.pack('lll',*img.shape),img.tostring()])
#socket_pub.send_multipart([topic,pickle.dumps(img,-1)])
if show_cv:
cv2.imshow('drone camera %d'%drone_index,img)
cv2.waitKey(1)
def kill():
print('done!')
socket_pub.send_multipart([config.topic_unreal_state,b'kill'])
if show_cv:
cv2.destroyAllWindows()
for _ in range(10):
cv2.waitKey(10)
if __name__=="__main__":
while 1:
for drone_index in range(config.n_drones):
socket_sub=drone_subs[drone_index]
while len(zmq.select([socket_sub],[],[],0)[0])>0:
topic, msg = socket_sub.recv_multipart()
print("got ",topic)
def result_status(self, msg_ids, status_only=True):
"""Check on the status of the result(s) of the apply request with `msg_ids`.
If status_only is False, then the actual results will be retrieved, else
only the status of the results will be checked.
Parameters
----------
msg_ids : list of msg_ids
if int:
Passed as index to self.history for convenience.
status_only : bool (default: True)
if False:
Retrieve the actual results of completed tasks.
Returns
-------
results : dict
There will always be the keys 'pending' and 'completed', which will
be lists of msg_ids that are incomplete or complete. If `status_only`
is False, then completed results will be keyed by their `msg_id`.
"""
if not isinstance(msg_ids, (list, tuple)):
msg_ids = [msg_ids]
theids = []
for msg_id in msg_ids:
if isinstance(msg_id, int):
msg_id = self.history[msg_id]
if not isinstance(msg_id, basestring):
raise TypeError("msg_ids must be str, not %r" % msg_id)
theids.append(msg_id)
completed = []
local_results = {}
# comment this block out to temporarily disable local shortcut:
for msg_id in theids:
if msg_id in self.results:
completed.append(msg_id)
local_results[msg_id] = self.results[msg_id]
theids.remove(msg_id)
if theids: # some not locally cached
content = dict(msg_ids=theids, status_only=status_only)
msg = self.session.send(self._query_socket,
"result_request",
content=content)
zmq.select([self._query_socket], [], [])
idents, msg = self.session.recv(self._query_socket, zmq.NOBLOCK)
if self.debug:
pprint(msg)
content = msg['content']
if content['status'] != 'ok':
raise self._unwrap_exception(content)
buffers = msg['buffers']
else:
content = dict(completed=[], pending=[])
content['completed'].extend(completed)
if status_only:
return content
failures = []
# load cached results into result:
content.update(local_results)
# update cache with results:
for msg_id in sorted(theids):
if msg_id in content['completed']:
rec = content[msg_id]
parent = rec['header']
header = rec['result_header']
rcontent = rec['result_content']
iodict = rec['io']
if isinstance(rcontent, str):
rcontent = self.session.unpack(rcontent)
md = self.metadata[msg_id]
md.update(self._extract_metadata(header, parent, rcontent))
md.update(iodict)
if rcontent['status'] == 'ok':
res, buffers = util.unserialize_object(buffers)
else:
print rcontent
res = self._unwrap_exception(rcontent)
failures.append(res)
self.results[msg_id] = res
content[msg_id] = res
if len(theids) == 1 and failures:
raise failures[0]
error.collect_exceptions(failures, "result_status")
return content
def main_loop(gworld):
global file_dump
if dump_name:
print('openning dump file dump name is ', dump_name)
file_dump = open(dump_name, 'wb')
os.chmod(dump_name, 0o777)
print('-- actors 1--')
for p in ph.GetActorsNames(gworld, 100 * 1024):
print(p)
print('-- textures --')
drone_textures = []
for tn in drone_texture_names:
drone_textures.append(ph.GetTextureByName(tn))
if not all(drone_textures):
print("Error, Could not find all textures")
while 1:
yield
drone_actors = []
for drn in drone_actors_names:
drone_actors.append(ph.FindActorByName(gworld, drn))
reference_actor = ph.FindActorByName(gworld, reference_actor_name)
if not all(drone_actors):
print("Error, Could not find all drone actors")
while 1:
yield
for _ in range(10): #need to send it a few time don't know why.
print('sending state main loop')
socket_pub.send_multipart([config.topic_unreal_state, b'main_loop'])
yield
drone_start_positions = [
np.array(ph.GetActorLocation(drone_actor))
for drone_actor in drone_actors
]
positions = [None for _ in range(config.n_drones)]
reference_start = np.array(ph.GetActorLocation(reference_actor))
reference_rotation = np.array(ph.GetActorRotation(reference_actor))
while 1:
for drone_index in range(config.n_drones):
socket_sub = drone_subs[drone_index]
drone_actor = drone_actors[drone_index]
while len(zmq.select([socket_sub], [], [], 0)[0]) > 0:
topic, msg = socket_sub.recv_multipart()
positions[drone_index] = pickle.loads(msg)
#print('-----',positions[drone_index])
#position=positions[drone_index]
#if position is not None:
# new_pos=drone_start_positions[drone_index]+np.array([position['posx'],position['posy'],position['posz']])*100 #turn to cm
# #print('-----',drone_index,new_pos)
# ph.SetActorLocation(drone_actor,new_pos)
# ph.SetActorRotation(drone_actor,(position['roll'],position['pitch'],position['yaw']))
# positions[drone_index]=None
#new_pos=reference_start-ph.GetActorLocation(reference_actor)+drone_start_positions[drone_index]
#reference_rotation = reference_rotation*0.9+np.array(ph.GetActorRotation(reference_actor))*0.1
#ph.SetActorRotation(drone_actor,reference_rotation)
# ph.SetActorLocation(drone_actor,new_pos)
reference_rotation = ph.GetActorRotation(reference_actor)
reference_location = ph.GetActorLocation(reference_actor)
# print('-----',reference_location,reference_rotation)
yield
for drone_index in range(config.n_drones):
#img=cv2.resize(ph.GetTextureData(drone_textures[drone_index]),(1024,1024),cv2.INTER_LINEAR)
topics = []
imgs = []
img = ph.GetTextureData(drone_textures[drone_index])
#print('--==--',img.shape)
topics.append(config.topic_unreal_drone_rgb_camera % drone_index)
imgs.append(img)
#topics=[config.topic_unreal_drone_rgb_camera%drone_index,
# config.topic_unreal_drone_rgb_camera%drone_index+b'down',
# config.topic_unreal_drone_rgb_camera%drone_index+b'depth']
#imgs=[ ph.GetTextureData(drone_textures[drone_index]),
# ph.GetTextureData(drone_textures_down[drone_index]),
# ph.GetTextureData(drone_textures_depth[drone_index],channels=[2])]
if pub_cv:
for topic, img in zip(topics, imgs):
#socket_pub.send_multipart([topic,pickle.dumps(img,2)])
#print('--->',img.shape)
socket_pub.send_multipart([
topic,
struct.pack('lll', *img.shape),
img.tostring()
])
#socket_pub.send_multipart([topic,pickle.dumps(img,-1)])
if show_cv:
#img=cv2.blur(cv2.resize(img,(512,512)),(3,3))
cv2.imshow('drone camera %d' % drone_index, img)
cv2.waitKey(1)
if file_dump is not None:
pickle.dump((img, reference_rotation, reference_location),
file_dump, -1)
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()
topic,data=ret[0],pickle.loads(ret[1])
if topic == zmq_topics.topic_record_state and data != prev_rec_state:
print('Record start/stop sync light blink')
ser.write(b'%c' % (0+2))
time.sleep(0.5)
ser.write(b'%c' % (LIGHTS_MAX+2))
time.sleep(0.5)
ser.write(b'%c' % (cur_lights_cmd+2))
prev_rec_state = data
if topic==zmq_topics.topic_lights:
print('got lights command',data)
ser.write(b'%c'%(data+2))
def _connect(self, sshserver, ssh_kwargs, timeout):
"""setup all our socket connections to the cluster. This is called from
__init__."""
# Maybe allow reconnecting?
if self._connected:
return
self._connected = True
def connect_socket(s, url):
url = util.disambiguate_url(url, self._config['location'])
if self._ssh:
return tunnel.tunnel_connection(s, url, sshserver,
**ssh_kwargs)
else:
return s.connect(url)
self.session.send(self._query_socket, 'connection_request')
r, w, x = zmq.select([self._query_socket], [], [], timeout)
if not r:
raise error.TimeoutError("Hub connection request timed out")
idents, msg = self.session.recv(self._query_socket, mode=0)
if self.debug:
pprint(msg)
msg = ss.Message(msg)
content = msg.content
self._config['registration'] = dict(content)
if content.status == 'ok':
if content.mux:
self._mux_socket = self._context.socket(zmq.XREQ)
self._mux_socket.setsockopt(zmq.IDENTITY, self.session.session)
connect_socket(self._mux_socket, content.mux)
if content.task:
self._task_scheme, task_addr = content.task
self._task_socket = self._context.socket(zmq.XREQ)
self._task_socket.setsockopt(zmq.IDENTITY,
self.session.session)
connect_socket(self._task_socket, task_addr)
if content.notification:
self._notification_socket = self._context.socket(zmq.SUB)
connect_socket(self._notification_socket, content.notification)
self._notification_socket.setsockopt(zmq.SUBSCRIBE, b'')
# if content.query:
# self._query_socket = self._context.socket(zmq.XREQ)
# self._query_socket.setsockopt(zmq.IDENTITY, self.session.session)
# connect_socket(self._query_socket, content.query)
if content.control:
self._control_socket = self._context.socket(zmq.XREQ)
self._control_socket.setsockopt(zmq.IDENTITY,
self.session.session)
connect_socket(self._control_socket, content.control)
if content.iopub:
self._iopub_socket = self._context.socket(zmq.SUB)
self._iopub_socket.setsockopt(zmq.SUBSCRIBE, b'')
self._iopub_socket.setsockopt(zmq.IDENTITY,
self.session.session)
connect_socket(self._iopub_socket, content.iopub)
self._update_engines(dict(content.engines))
else:
self._connected = False
raise Exception("Failed to connect!")
omnirobot_manager.robot.resetOdom(0, 0, 0)
omnirobot_manager.robot.reset()
omnirobot_manager.robot.pubPosCmd()
r = rospy.Rate(RL_CONTROL_FREQ)
while not rospy.is_shutdown():
print("wait for new command")
msg = socket.recv_json()
print("msg: {}".format(msg))
omnirobot_manager.processMsg(msg)
# wait for robot to finish the action, timeout 30s
timeout = 30 # second
for i in range(timeout):
readable_list, _, _ = zmq.select([socket], [], [], 0)
if len(readable_list) > 0:
print("New command incomes, ignore the current command")
continue
if omnirobot_manager.robot.move_finished:
print("action done")
break
elif i == timeout - 1:
print("Error: timeout for action finished signal")
exit()
time.sleep(1)
if IMAGE_TOPIC is not None:
# Retrieve last image from image topic
def listener():
fmt_cnt_l = -1
fmt_cnt_r = -2
if doplot:
plot = ploter()
plot.__next__()
keep_running = True
while keep_running:
while len(zmq.select([zmq_sub], [], [], 0.001)[0]) > 0:
topic, info, data = zmq_sub.recv_multipart()
#topic=topic.decode()
info = struct.unpack('llll', info)
shape = info[:3]
img = np.fromstring(data, 'uint8').reshape(shape)
if topic == topicl:
fmt_cnt_l = info[3]
imgl = img
if topic == topicr:
fmt_cnt_r = info[3]
imgr = img
if cvshow:
#if 'depth' in topic:
# cv2.imshow(topic,img)
#else:
#cv2.imshow(topic,cv2.resize(cv2.resize(img,(1920/2,1080/2)),(512,512)))
img_shrk = img[::2, ::2]
centx = None
centy = None
wx = 100
wy = 100
if fmt_cnt_r == fmt_cnt_l:
disparity = stereo.compute(preisterproc(imgl),
preisterproc(imgr))
disparityu8 = np.clip(disparity, 0, 255).astype('uint8')
centx = disparityu8.shape[1] // 2
centy = disparityu8.shape[0] // 2
#print('--->',disparity.max(),disparity.min(),type(disparity),imgr.shape)
#disparityu8[centy-wy//2:centy+wy//2,centx-wx//2:centx+wx//2]=255
cv2.rectangle(disparityu8,
(centx - wx // 2, centy - wy // 2),
(centx + wx // 2, centy + wy // 2), 255)
cv2.imshow('disparity', disparityu8)
avg_disp, min_d, max_d, cnt_d = avg_disp_win(disparity,
centx,
centy,
wx,
wy,
tresh=10)
print('D {:05.2f}, {:05.2f} ,{:05.2f} ,{:05d} R {:5.2f}'.
format(avg_disp, min_d, max_d, cnt_d,
disp2range(avg_disp)))
if doplot:
plot.send({
'tstemp': time.time() - start,
'disp': avg_disp
})
centx_full = img.shape[1] // 2
centy_full = img.shape[0] // 2
cv2.rectangle(img,
(centx_full - wx // 2, centy_full - wy // 2),
(centx_full + wx // 2, centy_full + wy // 2),
(0, 0, 255))
cv2.imshow(topic.decode(), img)
#import pdb;pdb.set_trace()
k = cv2.waitKey(1)
if k == ord('q'):
keep_running = False
plot.send('stop')
break
### test
time.sleep(0.010)
async def recv_and_process():
keep_running = True
pitch, roll = 0, 0
target_depth = 0
pid = None
ab = None
rate = 0
system_state = {'mode': []}
jm = Joy_map()
while keep_running:
socks = zmq.select(subs_socks, [], [], 0.005)[0]
for sock in socks:
ret = sock.recv_multipart()
topic, data = ret[0], pickle.loads(ret[1])
if topic == zmq_topics.topic_depth:
new_depth_ts, depth = data['ts'], data['depth']
if ab is None:
ab = ab_filt([depth, 0])
else:
depth, rate = ab(depth, new_depth_ts - depth_ts)
depth_ts = new_depth_ts
if 'DEPTH_HOLD' in system_state['mode']:
if pid is None:
pid = PID(**depth_pid)
else:
ud_command = pid(depth, target_depth, rate, 0)
debug_pid = {
'P': pid.p,
'I': pid.i,
'D': pid.d,
'C': ud_command,
'T': target_depth,
'N': depth,
'TS': new_depth_ts
}
pub_sock.send_multipart([
zmq_topics.topic_depth_hold_pid,
pickle.dumps(debug_pid, -1)
])
thruster_cmd = mixer.mix(ud_command, 0, 0, 0, 0, 0,
pitch, roll)
thrusters_source.send_pyobj(
['depth', time.time(), thruster_cmd])
else:
if pid is not None:
pid.reset()
thrusters_source.send_pyobj(
['depth', time.time(),
mixer.zero_cmd()])
target_depth = depth
if topic == zmq_topics.topic_axes:
jm.update_axis(data)
target_depth += jm.joy_mix()['ud'] / 250.0
if topic == zmq_topics.topic_gui_depthAtt:
if data['dDepth'] is not None:
target_depth = data['dDepth']
print('set new depth: %.2f' % target_depth)
if topic == zmq_topics.topic_imu:
pitch, roll = data['pitch'], data['roll']
if topic == zmq_topics.topic_system_state:
system_state = data
await asyncio.sleep(0.001)
async def recv_and_process():
keep_running = True
target_pos = np.zeros(3)
pids = [None] * 3
system_state = {'mode': []}
jm = Joy_map()
joy = None
while keep_running:
socks = zmq.select(subs_socks, [], [], 0.005)[0]
for sock in socks:
ret = sock.recv_multipart()
topic, data = ret[0], pickle.loads(ret[1])
if topic == zmq_topics.topic_imu:
yaw, pitch, roll = data['yaw'], data['pitch'], data['roll']
if topic == zmq_topics.topic_tracker:
td = data
cmds = [0] * 3
for ind in range(len(pids)):
#mapping from track image coords to world coords
imap = ['dx_f', 'dy_f', 'dz_f'][ind]
pid_states = ['RX_HOLD', 'RY_HOLD', 'RZ_HOLD']
if pid_states[ind] not in system_state['mode'] \
or pids[ind] is None:
pids[ind] = PID(**pos_pids[ind])
if td['valid'] and imap in td:
x, _ = td[imap]
#we want it to return to 0 for the y lock
target_pos[ind] = 0 if ind == 1 else x
else:
if td['valid'] and imap in td:
x, v = td[imap]
cmds[ind] = -pids[ind](x, target_pos[ind], v)
#print('pid=',ind,x,v,str(pids[ind]))
ts = time.time()
debug_pid = {
'P': pids[ind].p,
'I': pids[ind].i,
'D': pids[ind].d,
'C': cmds[ind],
'T': target_pos[ind],
'N': x,
'R': v,
'TS': ts
}
pub_sock.send_multipart([
zmq_topics.topic_pos_hold_pid_fmt % ind,
pickle.dumps(debug_pid, -1)
])
thruster_cmd = mixer.mix(cmds[2], cmds[1], cmds[0], 0, 0, 0, 0,
0)
thrusters_source.send_pyobj(['pos', time.time(), thruster_cmd])
if topic == zmq_topics.topic_axes:
jm.update_axis(data)
if topic == zmq_topics.topic_button:
jm.update_buttons(data)
#target_depth+=data[jm.ud]
if topic == zmq_topics.topic_system_state:
_, system_state = data
await asyncio.sleep(0.001)
