def main_loop(gworld):
drone_actor = ph.FindActorByName(gworld, 'Parrot_Drone_6', 1)
#drone_camera_actor=ph.FindActorByName(gworld,'SceneCapture2Ddrone',1)
if drone_actor is None: # or drone_camera_actor is None:
print('ERROR: could not find drone_actor')
while 1:
yield
for _ in range(10): #need to send it a few time don't know why.
socket_pub.send_multipart([config.topic_unreal_state, b'main_loop'])
yield
drone_start_pos = np.array(ph.GetActorLocation(drone_actor))
position = None
while 1:
while len(zmq.select([socket_sub], [], [], 0)[0]) > 0:
topic, msg = socket_sub.recv_multipart()
position = pickle.loads(msg)
if position is not None:
new_pos = drone_start_pos + np.array([
position['posx'], position['posy'], position['posz']
]) * 100 #turn to cm
ph.SetActorLocation(drone_actor, new_pos)
ph.SetActorRotation(
drone_actor,
(position['roll'], position['pitch'], position['yaw']))
#incase of gimabl
#ph.SetActorRotation(drone_camera_actor,(-position['roll'],-position['pitch'],-position['yaw']))
position = None
yield
img1 = cv2.resize(ph.GetTextureImg(), (512, 512), cv2.INTER_LINEAR)
cv2.imshow('camera 1', img1)
cv2.waitKey(1)
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 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
def main_loop(gworld):
global cv_loop_itr,proc
cv_loop_itr=cv_loop(imgq)
next(cv_loop_itr)
if save_path is not None:
if os.path.isdir(save_path):
shutil.rmtree(save_path)
os.makedirs(save_path)
ex_camera_actor=ph.FindActorByName(gworld,'CameraActor_2',1)
camera_actor=ph.FindActorByName(gworld,'Parrot_Drone_0',1)
PlayerStart=ph.FindActorByName(gworld,'PlayerStart_1',1)
loc=ph.GetActorLocation(PlayerStart) #PlayerController_0 PlayerState_0 PlayerStart_1
ph.SetActorLocation(ph.FindActorByName(gworld,'PlayerController_0',0),(loc[0],loc[1],loc[2]+1000))
if camera_actor is None:
print('could not find Parrot_Drone_0 yeilding forever')
while 1:
yield
camera_texture=ph.GetTextureByName('/Game/TextureRenderTarget2D_0')
#tick_actor=ph.FindActorByName(gworld,'PyServerTickActor_0')
#ph.MoveToCameraActor(tick_actor,camera_actor)
wind_actor=ph.FindActorByName(gworld,'WindDirectionalSource1')
camera_initial_location=ph.GetActorLocation(camera_actor)
camera_initial_rot=ph.GetActorRotation(camera_actor)
for case_params in case_params_list:
if save_path is not None:
case_path=save_path+'/'+case_params['name']
os.mkdir(case_path)
for interation_num in range(case_params['iterations']):
#camera_initial_location=(-370-1000,-2920,case_params['camera_height']*100) #centimeters
ph.SetActorLocation(camera_actor,camera_initial_location)
ph.SetActorRotation(camera_actor,camera_initial_rot)
ph.SetWindParams(wind_actor,case_params['wind_speed'])
#move wind actor otherwize change wind speed doesn't work (https://answers.unrealengine.com/questions/35478/possible-to-change-wind-strength-in-level-blueprin.html)
loc=ph.GetActorLocation(wind_actor)
ph.SetActorLocation(wind_actor,(loc[0],loc[1],loc[2]+1000))
tic=time.time()
while time.time()-tic>10:
yield
if save_path is not None:
iter_path=case_path+'/%d'%interation_num
os.mkdir(iter_path)
imgq.put('init,'+iter_path)
else:
imgq.put('reset')
for _ in range(300): #adjustment frames...
yield
for cnt in range(case_params['iteration_frame_cnt']):
tic=time.time()
speed=case_params['camera_speed']
cycle=case_params['frames_in_cycle']
#img=cv2.resize(img,(640,480))
dirx=np.cos(np.radians(camera_initial_rot[2]))
diry=np.sin(np.radians(camera_initial_rot[2]))
if cnt>=cycle:
direction=0
elif (cnt%cycle) >= cycle/2:
direction=1
else:
direction=-1
loc=ph.GetActorLocation(camera_actor)
ph.SetActorLocation(camera_actor,(direction*dirx*speed+loc[0],direction*diry*speed+loc[1],camera_initial_location[2]+case_params['camera_height']*100))
yield
img=cv2.resize(ph.GetTextureData(camera_texture),(512,512),cv2.INTER_LINEAR)
#img=ph.TakeScreenshot()
if img is None:
print('got None im')
else:
imgq.put(img)
next(cv_loop_itr)
print('case:',case_params['name'],'iter=',interation_num,r'cnt=',cnt,direction,img.shape,imgq.qsize(),(time.time()-tic)*100,'ms')
if save_path is not None:
print('---saving last----')
imgq.put('save_last')
next(cv_loop_itr)
print('Done experiment!')
while 1:
yield
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 rovvision 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", drone_textures)
while 1:
yield
drone_actors = []
print('---looking for actors---')
for drn in drone_actors_names:
drone_actors.append(ph.FindActorByName(gworld, drn))
print('---looking for actors 2---', drone_actors)
if not all(drone_actors):
print("Error, Could not find all drone actors")
while 1:
yield
print('---debug 1---')
#change cameras angle
for cam_name in ['SceneCaptureBROV1left', 'SceneCaptureBROV1right']:
#ca=ph.FindActorByName(gworld,'SceneCaptureBROV1left')
print('---debug 1.1---')
ca = ph.FindActorByName(gworld, cam_name)
print('---debug 1.2---', ca)
ph.SetActorRotation(ca, (1, -pitch, 89))
#ph.SetActorRotation(ca,(0,-pitch,0))
print('---debug 1.3---')
yield
#ph.SetActorRotation(caml,(1,1,89)) #facing down
print('camera ' + cam_name + ' rotation ---', ph.GetActorRotation(ca))
print('---debug 1.4---')
#print('--- ',caml)
print('---debug 2---')
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)]
tic = time.time()
while 1:
while time.time() - tic < 1.0 / fps:
time.sleep(0.001)
yield
tic = time.time()
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['posy'], position['posx'], -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
topics = []
imgs = []
#img=ph.GetTextureData(drone_textures[0])
imgr = ph.GetTextureData(drone_textures[0])
imgl = ph.GetTextureData(drone_textures[1])
#topics.append(config.topic_unreal_drone_rgb_camera%0)
img_depth = ph.GetTextureData32f(
drone_textures_depth[0], channels=[3],
verbose=1) #depth data will be in red componnent
#img_depth=ph.GetTextureData(drone_textures_depth[0],channels=[2]) #depth data will be in red componnent
socket_pub.send_multipart([
config.topic_unreal_stereo_camera % 0,
pickle.dumps((frame_cnt, imgl.shape)),
imgl.tostring(),
imgr.tostring()
])
socket_pub.send_multipart([
config.topic_unreal_depth % 0,
pickle.dumps((frame_cnt, img_depth.shape)),
img_depth.tostring()
])
if show_cv:
cv2.imshow('drone camera %d' % drone_index, imgl)
cv2.waitKey(1)
frame_cnt += 1
