def main():
video = VideoCapture(video_sources.video_2)
frame = video.read()
backSubtractor = BackgroundSubtractorAVG(0.2, denoise(frame))
for frame in video.frames():
with utils.timeit_context():
frame = denoise(frame)
foreGround = backSubtractor.getForeground(frame)
# Apply thresholding on the background and display the resulting mask
ret, mask = cv2.threshold(foreGround, 15, 255, cv2.THRESH_BINARY)
cv2.imshow('input', frame)
cv2.imshow('foreground', foreGround)
# Note: The mask is displayed as a RGB image, you can
# display a grayscale image by converting 'foreGround' to
# a grayscale before applying the threshold.
cv2.imshow('mask', mask)
if cv2.waitKey(10) & 0xFF == 27:
break
video.release()
cv2.destroyAllWindows()
def main():
# dir_name = '/lfs/1/ddkang/blazeit/data/svideo/jackson-town-square/2017-12-14'
# index_fname = '/lfs/1/ddkang/blazeit/data/svideo/jackson-town-square/2017-12-14.json'
dir_name = '/lfs/1/ddkang/blazeit/data/svideo/jackson-town-square/short'
index_fname = dir_name + '.json'
cap = VideoCapture(dir_name, index_fname)
'''cap.set(cv2.CAP_PROP_POS_FRAMES, 300)
ret, frame = cap.read()
cap2 = cv2.VideoCapture(os.path.join(dir_name, '3.mp4'))
_, f2 = cap2.read()'''
i = 0
ret, frame = cap.read()
while ret:
i += 1
ret, frame = cap.read()
print(i)
print(np.array_equal(frame, f2))
def main():
video = VideoCapture(video_sources.video_6)
diff_wnd = CvNamedWindow('diff')
mask_wnd = CvNamedWindow('mask')
input_wnd = CvNamedWindow('input')
# prev_frame = denoise(video.read())
prev_frame = cv2.cvtColor(denoise(video.read()), cv2.COLOR_BGR2GRAY)
vm = VideoController(10)
diff = None
for frame in video.frames():
# with utils.timeit_context('frame processing'):
# frame = denoise(frame)
# diff = cv2.absdiff(prev_frame, frame, dst=diff)
# ret, mask = cv2.threshold(diff, 45, 255, cv2.THRESH_BINARY)
# binary_mask = cv2.cvtColor(mask, cv2.COLOR_BGR2GRAY)
# # cn = cv2.countNonZero(binary_mask)
# nonzero = cv2.findNonZero(binary_mask)
with utils.timeit_context('frame processing'):
frame = denoise(frame)
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
diff = cv2.absdiff(prev_frame, gray, dst=diff)
ret, mask = cv2.threshold(diff, 45, 255, cv2.THRESH_BINARY)
# cn = cv2.countNonZero(binary_mask)
nonzero = cv2.findNonZero(mask)
diff_wnd.imshow(diff)
mask_wnd.imshow(mask)
input_wnd.imshow(utils.put_frame_pos(frame.copy(), video.frame_pos()))
prev_frame = gray
if not vm.wait_key():
break
video.release()
cv2.destroyAllWindows()
def main():
parser = argparse.ArgumentParser()
parser.add_argument('--video_dir', required=True)
parser.add_argument('--index_fname', required=True)
parser.add_argument('--npy_fname', required=True)
parser.add_argument('--resol', type=int, required=True)
parser.add_argument('--base_name', default=None)
args = parser.parse_args()
vid_dir = args.video_dir
index_fname = args.index_fname
npy_fname = args.npy_fname
resol = args.resol
if args.base_name is None:
crop_and_exclude = False
else:
crop_and_exclude = True
from blazeit.data.video_data import get_video_data
video_data = get_video_data(args.base_name)
cap = VideoCapture(vid_dir, index_fname)
data = np.zeros((cap.cum_frames[-1], resol, resol, 3), dtype=np.float32)
for i in range(len(data)):
if i % 1000 == 0:
print('Processing frame', i)
ret, frame = cap.read()
if not ret:
print('Something really bad happened')
sys.exit(1)
if crop_and_exclude:
frame = video_data.process_frame(frame)
data[i] = cv2.resize(frame, (resol, resol))
data /= 255.
data[..., :] -= [0.485, 0.456, 0.406]
data[..., :] /= [0.229, 0.224, 0.225]
np.save(npy_fname, data)
class Controller(object):
def __init__(self, env, video_device):
try:
self.API_ENDPOINT = env['ApiEndpoint']
self.FACE_AREA_THRESHOLD = env['FaceAreaThreshold']
self.NAME_TTL_SEC = env['NameTtlSec']
self.FACE_SIMILARITY_THRESHOLD = env['FaceSimilarityThreshold']
self.COGNITO_USERPOOL_ID = env['CognitoUserPoolId']
self.COGNITO_USERPOOL_CLIENT_ID = env['CognitoUserPoolClientId']
self.REGION = env['Region']
except KeyError:
print('Invalid config file')
raise
self.recent_name_list = []
self.registered_name_set = set()
self.video_capture = VideoCapture(env, video_device)
self.detector = Detector(env)
self.viewer = Viewer(env)
def _update_name_list(self):
limit_time = datetime.datetime.now() - datetime.timedelta(seconds=self.NAME_TTL_SEC)
for d in self.recent_name_list[:]:
if d.get('timestamp') < limit_time:
self.recent_name_list.remove(d)
def _sign(self, key, msg):
return hmac.new(key, msg.encode("utf-8"), hashlib.sha256).digest()
def _get_signature_key(self, key, date_stamp, region_name, service_name):
date = self._sign(('AWS4' + key).encode('utf-8'), date_stamp)
region = self._sign(date, region_name)
service = self._sign(region, service_name)
signing = self._sign(service, 'aws4_request')
return signing
def _get_id_token_by_cognito(self, username, password):
client = boto3.client('cognito-idp', self.REGION)
response = client.initiate_auth(
ClientId=self.COGNITO_USERPOOL_CLIENT_ID,
AuthFlow='USER_PASSWORD_AUTH',
AuthParameters={
'USERNAME': username,
'PASSWORD': password
}
)
return response['AuthenticationResult']['IdToken']
def run(self):
# input username and password
username = input("Enter username: "******"key 'q' is pressed")
class VideoStream:
def __init__(self, uri, transform=None, queue_size=128, gpu_id=None, quiet=True, read_type='numpy'):
# initialize the video capture along with the boolean
# used to indicate if the thread should be stopped or not
self.cap = VideoCapture(uri, gpu_id=gpu_id, quiet=quiet)
self.stopped = False
self.transform = transform
self.read_type = read_type
# initialize queue and thread
self.Q = Queue(maxsize=queue_size)
self.thread = Thread(target=self.update, args=())
self.thread.daemon = True
def start(self):
# start a thread to read frames from the file video stream
self.thread.start()
return self
def update(self):
# keep looping infinitely
while True:
# if the thread indicator variable is set, stop the thread
if self.stopped: break
# otherwise, ensure the queue has room in it
if not self.Q.full():
# read the next frame from the file
frame = self.cap.read(type=self.read_type)
# if the `grabbed` boolean is `False`, then we have
# reached the end of the video file
if frame is None:
self.stopped = True
# if there are transforms to be done, might as well
# do them on producer thread before handing back to
# consumer thread. ie. Usually the producer is so far
# ahead of consumer that we have time to spare.
#
# Python is not parallel but the transform operations
# are usually OpenCV native so release the GIL.
#
# Really just trying to avoid spinning up additional
# native threads and overheads of additional
# producer/consumer queues since this one was generally
# idle grabbing frames.
if self.transform:
frame = self.transform(frame)
# add the frame to the queue
self.Q.put(frame)
else:
time.sleep(0.1) # Rest for 10ms, we have a full queue
self.cap.release()
def read(self):
# return next frame in the queue
return self.Q.get()
# Insufficient to have consumer use while(more()) which does
# not take into account if the producer has reached end of stream.
def running(self):
return self.more() or not self.stopped
def more(self):
# return True if there are still frames in the queue. If stream is not stopped, try to wait a moment
tries = 0
while self.Q.qsize() == 0 and not self.stopped and tries < 5:
time.sleep(0.1)
tries += 1
return self.Q.qsize() > 0
def stop(self):
# wait until stream resources are released (producer thread might be still grabbing frame)
self.thread.join()
# indicate that the thread should be stopped
self.stopped = True
class RealSawyerLift(object):
def __init__(self, control_freq=20, horizon=1000, camera_height=256,grip_thresh=.25,
camera_width=256, use_image=False, use_proprio=False,
port1=None, joint_proprio=True, do_sleep=True):
self.joint_proprio = joint_proprio
self.ctrl_freq = control_freq
self.t = horizon
self.use_image = use_image
self.use_proprio = use_proprio
self.do_sleep = do_sleep
assert self.use_image or self.use_proprio, 'One of use_image and use_proprio must be set'
# self.controller = controller
self.cam_dim = (camera_height, camera_width)
self.control_timestep = 1. / self.ctrl_freq
self.is_py2 = sys.version_info[0] < 3
self.gripper_closed = 0 # 0 = open 1 = closed
self.timestep = 0
self.done = False
self.has_object_obs = False
self.data_size = 96000
self.grip_thresh=grip_thresh
self.sawyer_interface = None
self.camera = None
#np.random.seed(int(time.time()))
self.port1 = port1 #np.random.choice(range(PORT_LOW, PORT_HIGH))
self.port2 = self.port1 + 1
self.debounce = 0 # make sure the gripper doesnt toggle too quickly
self.config = make_config('RealSawyerDemoServerConfig')
self.config.infer_settings()
self.controller = self.config.controller.mode
if self.is_py2:
self.bridge = PythonBridge(self.port1, self.port2, self.data_size)
self._setup_robot()
self._recv_loop()
else:
self.bridge = PythonBridge(self.port2, self.port1, self.data_size)
self._setup_camera()
def signal_handler(self, signal, frame):
"""
Exits on ctrl+C
"""
if self.is_py2:
self.sawyer_interface.open_gripper()
if self.controller=='opspace':
self.osc_controller.reset_flag.publish(True)
def send(self, data):
#self.close()
#exit()
self.bridge.send(data)
def recv(self):
data = self.bridge.recieve()
action = array('f')
action.fromstring(data)
data = np.array(action)
# print('Recieved', data)
if data[0] == RESET and data[3] == RESET and data[-1] == RESET:
self.reset()
elif data[0] == SUCCESS and data[3] == SUCCESS and data[-1] == SUCCESS:
return True
elif data[0] == GET_POSE and data[3] == GET_POSE and data[-1] == GET_POSE:
pose = self.sawyer_interface.ee_pose
pose = self.sawyer_interface.pose_endpoint_transform(pose, des_endpoint='right_hand',
curr_endpoint='right_gripper_tip')
pose.append(self.sawyer_interface._gripper.get_position())
self.send(array('f', pose))
elif data[0] == GET_PROPRIO and data[3] == GET_PROPRIO and data[-1] == GET_PROPRIO:
joint_pos = np.array(self.sawyer_interface.q)
joint_vel = np.array(self.sawyer_interface.dq)
gripper_pos = np.array(self.sawyer_interface._gripper.get_position())
ee_pose = self.sawyer_interface.ee_pose
ee_pose = np.array(self.sawyer_interface.pose_endpoint_transform(ee_pose, des_endpoint='right_hand',
curr_endpoint='right_gripper_tip'))
print(joint_pos.shape, joint_vel.shape, gripper_pos.shape, ee_pose.shape)
pose = np.concatenate([
np.sin(joint_pos), np.cos(joint_pos), joint_vel, [gripper_pos], [-gripper_pos], ee_pose
])
print(pose)
self.send(array('f', pose))
elif data[0] == CLOSE and data[3] == CLOSE and data[-1] == CLOSE:
self.close()
exit()
else:
self._apply_action(data)
def _recv_loop(self):
while True:
self.recv()
def _setup_camera(self):
self.camera = VideoCapture(0, self.cam_dim)
def _setup_robot(self):
self.osc_robot = make_robot(self.config.robot.type, config=self.config)
self.osc_controller = make_controller(self.config.controller.type, robot=self.osc_robot, config=self.config)
self.osc_controller.reset()
self.osc_controller.sync_state()
self.sawyer_interface = self.osc_robot.robot_arm
signal.signal(signal.SIGINT, self.signal_handler) # Handles ctrl+C
def reset(self):
self.timestep = 0
if self.is_py2:
self.sawyer_interface.open_gripper()
self.sawyer_interface.reset()
time.sleep(1)
# Successful Reset
self.send(array('f', np.array([SUCCESS] * self.dof).tolist()))
return
else:
# Request a reset
self.send(array('f', np.array([RESET] * self.dof).tolist()))
_ = self.recv()
return self._get_observation()
#def _process_image(self, img):
# h, w, _ = img.shape
# new_w = int( float(w) / float(h) * self.cam_dim[0])
# new_shape = (new_w, self.cam_dim[0])
#
# resized = cv2.resize(img, new_shape, cv2.INTER_AREA)
#
# center = new_w // 2
# left = center - self.cam_dim[1] // 2 # assume for now that this is multiple of 2
# right = center + self.cam_dim[1] // 2
#
# img = resized[:,left:right,:]
# img = np.array([img])
# img = np.transpose(img, (0, 3, 1, 2))
# return img
def _get_image(self):
ret, frame = self.camera.read()
if not ret:
raise RuntimeError('camera read failed')
return frame
def _get_proprio(self):
if self.joint_proprio:
self.send(array('f', np.array([GET_PROPRIO]*self.dof).tolist()))
else:
self.send(array('f', np.array([GET_POSE]*self.dof).tolist()))
data = self.bridge.recieve()
proprio = array('f')
proprio.fromstring(data)
return np.array(proprio)
def _get_observation(self):
start = time.time()
di = {}
if self.use_image:
img = self._get_image()
di['image'] = img
if self.use_proprio:
di['proprio'] = self._get_proprio()
# print('Observation retrieval time: {}'.format(time.time()-start))
return di
def _pre_action(self, action):
if self.controller == 'velocity':
# TODO: there is linear interpolation being done between the velocity limits
# in sim, do we have something similar to that on the real robot?
action = np.clip(action, -0.3, 0.3)
return action
def _apply_action(self, action):
if self.is_py2:
if self.controller == 'velocity':
self.sawyer_interface.dq = action[:-1]
elif self.controller == 'opspace':
self.osc_controller.send_action(action[:-1])
if self.debounce:
self.debounce-=1
else:
if self.gripper_closed:
if abs(action[-1]) < 1-self.grip_thresh:
self.sawyer_interface.open_gripper()
self.gripper_closed=0
self.debounce=5
print('open gripper')
else:
if abs(action[-1])>self.grip_thresh:
self.sawyer_interface.close_gripper()
self.gripper_closed=1
self.debounce=5
print('close gripper')
else:
# send to py2
self.send(bytes(array('f', action.tolist())))
@property
def action_spec(self):
low = np.ones(self.dof) * -1.
high = np.ones(self.dof) * 1.
return low, high
@property
def dof(self):
return 8
def step(self, action):
if self.done:
raise RuntimeError('Env is terminated')
start = time.time()
self.timestep += 1
action = self._pre_action(action)
self._apply_action(action)
if self.do_sleep:
end_time = start + self.control_timestep
while time.time() < end_time:
pass
return self._get_observation(), 0., False, {}
def render(self):
cv2.imshow('Observation', self._get_image())
cv2.waitKey(1)
def observation_spec(self):
observation = self._get_observation()
return observation
def close(self):
if self.is_py2:
self.sawyer_interface.open_gripper()
self.sawyer_interface.reset()
else:
self.camera.release()
self.send(array('f', np.array([CLOSE] * self.dof).tolist()))