def main():
# Get video memmap absolute path
video_path = os.getenv(key="MEMMAP_PATH", default="/tmp")
# Get axis for concatenating all thread images: Axis=1 showed almost 10x
# better performance than axis=2, and slightly better than axis=0
axis_concat = int(os.getenv(key="MMAP_AXIS_CONCAT", default=1))
rospy.init_node('video_mapping_node', anonymous=True) # Node initialization
rospy.set_param('/video_mapping/debug', 0) # Set node debugger
setProcessName("video_mapping_node") # Set video mapping process name
r = rospy.Rate(hz=30) # Set node rate
# Initiate CameraSupervisor Class that handles the threads that reads the
# cameras. Dont take last label, since it doesn't correspond to a physical
# device
cameras_labels = ["L", "C", "R"] # define camera labels order for memory mapping
cameras_supervisor = CamerasSupervisor( #mtx, dist
None, None, cameras_labels=cameras_labels)
# video mapping for raw images
video_map = MultiImagesMemmap(
memmap_path=video_path, labels=cameras_labels, name="main_stream",
axis=axis_concat, mode="w")
# Setting debuggers
main_debugger = Debugger() # Debugger that logs properly in stdout
# Used to update logging level. DEPRECATED since reading rosparam in main loop is SLOW
list_debuggers = [cameras_supervisor, main_debugger]+cameras_supervisor.camera_handlers
main_debugger.debugger(DEBUG_LEVEL_0, "Initiating main loop")
while not rospy.is_shutdown():
# Read into a list all images read from the threads
images = list(map(lambda o: o.image, cameras_supervisor.camera_handlers))
# Show video streamins
# for idx, cam_label in enumerate(cameras_labels):
# cv2.imshow("CAM{}".format(cam_label), images[idx]); cv2.waitKey(10)
# Concatenate all list images in one big 3D matrix
data = np.concatenate(images, axis=axis_concat)
video_map.write(data) # Write into memory
# Suspend execution of R expressions for a specified time interval.
r.sleep()
def detection(self):
self.graph, self.score, self.expand = self.load_frozenmodel(
self.model_path)
print("Building Graph")
detection_graph = self.graph
score = self.score
expand = self.expand
# Session Config: allow seperate GPU/CPU adressing and limit memory allocation
config = tf.ConfigProto(
allow_soft_placement=True,
log_device_placement=False) # Log to true to debug
config.gpu_options.allow_growth = True
config.gpu_options.per_process_gpu_memory_fraction = 0.4
cur_frames = 0
with detection_graph.as_default():
with tf.Session(graph=detection_graph, config=config) as sess:
# Define Input and Ouput tensors
image_tensor = detection_graph.get_tensor_by_name(
'image_tensor:0')
detection_boxes = detection_graph.get_tensor_by_name(
'detection_boxes:0')
detection_scores = detection_graph.get_tensor_by_name(
'detection_scores:0')
detection_classes = detection_graph.get_tensor_by_name(
'detection_classes:0')
num_detections = detection_graph.get_tensor_by_name(
'num_detections:0')
score_out = detection_graph.get_tensor_by_name(
'Postprocessor/convert_scores:0')
expand_out = detection_graph.get_tensor_by_name(
'Postprocessor/ExpandDims_1:0')
score_in = detection_graph.get_tensor_by_name(
'Postprocessor/convert_scores_1:0')
expand_in = detection_graph.get_tensor_by_name(
'Postprocessor/ExpandDims_1_1:0')
# Threading
gpu_worker = SessionWorker("GPU", detection_graph, config)
cpu_worker = SessionWorker("CPU", detection_graph, config)
gpu_opts = [score_out, expand_out]
cpu_opts = [
detection_boxes, detection_scores, detection_classes,
num_detections
]
gpu_counter = 0
cpu_counter = 0
video_stream = MultiImagesMemmap(mode="r",
name="main_stream",
memmap_path=os.getenv(
"MEMMAP_PATH", "/tmp"))
video_stream.wait_until_available(
) #initialize and find video data
tick = time.time()
print('Starting Detection')
init_time = time.time()
fps = FPS2(5).start()
while True: # Always True
# if time.time() - init_time > 10:
# self.call_model = not self.call_model
# init_time = time.time()
if self.call_model:
gpu_worker.call_model = True
cpu_worker.call_model = True
if gpu_worker.is_sess_empty():
# read video frame, expand dimensions and convert to rgb
image = video_stream.read("C")
image_expanded = np.expand_dims(cv2.cvtColor(
image, cv2.COLOR_BGR2RGB),
axis=0)
# put new queue
gpu_feeds = {image_tensor: image_expanded}
gpu_worker.put_sess_queue(gpu_opts, gpu_feeds, "C")
g = gpu_worker.get_result_queue()
if g is None:
# gpu thread has no output queue. ok skip, let's check cpu thread.
gpu_counter += 1
else:
# gpu thread has output queue.
gpu_counter = 0
score, expand, extras = g["results"][0], g[
"results"][1], g["extras"]
if cpu_worker.is_sess_empty():
# When cpu thread has no next queue, put new queue.
# else, drop gpu queue.
cpu_feeds = {
score_in: score,
expand_in: expand
}
cpu_worker.put_sess_queue(
cpu_opts, cpu_feeds, "C")
c = cpu_worker.get_result_queue()
if c is None:
# cpu thread has no output queue. ok, nothing to do. continue
cpu_counter += 1
time.sleep(0.005)
continue # If CPU RESULT has not been set yet, no fps update
else:
cpu_counter = 0
boxes, scores, classes, num, extras = c["results"][
0], c["results"][1], c["results"][2], c[
"results"][3], c["extras"]
# print("time: {}".format(time.time() - tick))
tick = time.time()
# fps.update()
self.predictions = parser(num,
boxes,
scores,
classes,
image_shape=image.shape)
# print(self.predictions)
fps.update()
else:
gpu_worker.call_model = False
cpu_worker.call_model = False
time.sleep(0.1)
# End everything
gpu_worker.stop()
cpu_worker.stop()
def main():
# create and set debugger: 0-DEBUG, 1-INFO, 2-WARNING, 3-ERROR, 4-FATAL_ERROR
main_debugger = Debugger()
# Initialize data capture ros node
rospy.init_node('data_capture_node', anonymous=True)
rospy.set_param('/data_capture/debug', 0)
setProcessName("data_capture_node")
rate = 15 #args.rate
r = rospy.Rate(hz=rate) # Set ros node rate
# create publishers
capture_publisher = rospy.Publisher('data_capture/status',
Status,
queue_size=2)
date = get_current_date_str("%m-%d-%y") # Get current date
bot_id = int(os.getenv(key='ROVER_ID', default=0)) # Get bot's ID
# Get base path
base_path = None
device = None
sub_folder_path = "data"
if not int(os.getenv(key='IS_SIM',
default=0)): # If environment is a simulation
usb_devices = get_mount_points() # Get connected usb devices
if len(usb_devices):
base_path = usb_devices[-1][
-1] # Get absolute path to root in usb device
device = usb_devices[-1][0] # Get usb device
base_path = os.path.join(base_path,
"bot-{}-{}".format(bot_id, date))
main_debugger.debugger(DEBUG_LEVEL_0,
"USB Detected: {}".format(device),
log_type='info')
main_debugger.debugger(DEBUG_LEVEL_0,
"Destination folder: {}".format(base_path),
log_type='info')
else: # If environment is real world
main_debugger.debugger(DEBUG_LEVEL_0,
"Runing in simulated environment",
log_type='warn')
base_path = os.getenv(key='USB_MOUNT', default="~/local_sim_data/")
main_debugger.debugger(DEBUG_LEVEL_0,
"Destination folder: {}".format(base_path),
log_type='info')
# If path already exits then rename destination folder with new index
if base_path is not None:
if os.path.isdir(base_path):
i = 1
axu_dest = base_path + "({})".format(i)
while (os.path.isdir(axu_dest)):
i += 1
axu_dest = base_path + "({})".format(i)
base_path = axu_dest
# Create subfolder to save images
sub_folder_path = os.path.join(base_path, sub_folder_path)
if not os.path.isdir(sub_folder_path):
os.makedirs(str(sub_folder_path))
img_quality = 80 # Quality of images to write
try: # Create data.csv headers if it not exists
if base_path is not None:
csv_file, log_msg = create_folder_csv_4data_capture(base_path)
else:
csv_file, log_msg = None, "USB device no found or it may have been disconnected"
main_debugger.debugger(DEBUG_LEVEL_0, log_msg, log_type='warn')
except Exception as err:
csv_file, log_msg = None, "Error creating csv file"
main_debugger.debugger(DEBUG_LEVEL_0, err, log_type='err')
# Create data capture object to catch important variables and enable/disable data capturing
bot_data_capture = DataCapture(bot_id=bot_id,
csv_file=csv_file,
dest_folder=base_path,
jpg_quality=img_quality,
data_capture=int(
os.getenv('DATA_CAPTURE', 0)))
# Show messages in debuggers
list_debuggers = [bot_data_capture, main_debugger]
if not bot_data_capture.is_sim: # If environment is a simulation
# Initialize memmap variable and wait for data
video_map = MultiImagesMemmap(mode="r",
name="main_stream",
memmap_path=os.getenv(
"MEMMAP_PATH", "/tmp"))
video_map.wait_until_available() #initialize and find video data
main_debugger.debugger(DEBUG_LEVEL_0,
"Memmap video data ready",
log_type='info')
msg = create_status_msg(size=video_map.read().shape[:2],
quality=img_quality,
device=device,
images_folder=base_path)
else:
msg = create_status_msg(quality=img_quality,
device=device,
images_folder=base_path)
# Create timer for checking usb space and images each minute
check_usb_timer = RoscppTimer(60, check_usb, msg, bot_data_capture,
main_debugger, base_path, device)
check_usb_timer.start()
# Init ros node cycle
main_debugger.debugger(DEBUG_LEVEL_0,
"data capture node ready!",
log_type='info')
c_image_old = None
first_time = True
while not rospy.is_shutdown():
# makes recording slow, without this mean record fps 15 -> with this 10
node_debug_level = int(rospy.get_param('/data_capture/debug'))
update_debuggers(list_debuggers, node_debug_level) # Update debuggers
msg.fps = bot_data_capture.fps # Update current frame rate
if bot_data_capture.recording and base_path is not None: # If data capture then do
if first_time:
start_time = time.time() # Get time if first iteration
start_time_local = time.time() # Get local start time
i = 0
is_same_image = True
while is_same_image:
if i >= 1000: #more than one sec and same image,
main_debugger.debugger(
DEBUG_LEVEL_1,
"Could not grab a valid frame in more than 1 sec",
log_type='warn')
break
images = []
if bot_data_capture.is_sim: # If environment is simulated
if (bot_data_capture.multiple_cameras):
images.append(bot_data_capture.left_image)
images.append(bot_data_capture.center_image)
images.append(bot_data_capture.right_image)
else:
images.append([bot_data_capture.center_image])
images = np.array(images, dtype=np.uint8)
else:
images = capture_images_memory(video_map)
# Check if current capture change
is_same_image = np.array_equal(c_image_old, images[0]) if (
bot_data_capture.is_sim and
not bot_data_capture.multiple_cameras) else np.array_equal(
c_image_old, images[1])
if not is_same_image:
c_image_old = np.array(
images[0], dtype=np.uint8).copy() if (
bot_data_capture.is_sim
and not bot_data_capture.multiple_cameras
) else images[1].copy()
time.sleep(0.001)
i += 1 # Wait and increment capture time index
if not is_same_image and bot_data_capture.throttle > 0.0:
# if not is_same_image:
# Show captured images to record
# for idx, img in enumerate(images):
# cv2.imshow("test_data_capture_{}".format(idx), img); cv2.waitKey(10)
# Write images in destination
bot_data_capture.images = images
timestamp, name_l, name_c, name_r = write_images(
images=bot_data_capture.images,
dest=sub_folder_path,
quality=bot_data_capture.quality,
multiple_images=bot_data_capture.multiple_cameras)
names = [name_l, name_c, name_r]
# Structure: 'capture', 'timestamp','camera','filename', 'steering
rows = [[
bot_data_capture.capture_id, timestamp, cam_label,
file_name, bot_data_capture.steering_angle
] for file_name, cam_label in zip([name_l, name_c, name_r],
["L", "C", "R"])]
# Write data and variables in csv file
with open(bot_data_capture.csv_file, 'a') as fd:
writer = csv.writer(fd)
writer.writerows(rows)
first_time = False
msg.recording = True
# Update frame rate
bot_data_capture.fps = 1.0 / (time.time() - start_time)
fps = 1.0 / (time.time() - start_time_local)
# If time left for next capture then wait
start_time = time.time()
time2sleep = 1.0 / bot_data_capture.desired_fps - 1.0 / fps - 1.0 / rate
main_debugger.debugger(
DEBUG_LEVEL_3,
"Capture FPS: {}, time2sleep: {}".format(
bot_data_capture.fps, time2sleep),
log_type='info')
if time2sleep >= 0:
time.sleep(time2sleep)
else:
msg.recording = False
first_time = True
bot_data_capture.fps = 0
time.sleep(0.1)
capture_publisher.publish(msg)
r.sleep()
import cv2
import numpy as np
from easy_memmap import MultiImagesMemmap
v0 = cv2.VideoCapture(0)
v1 = cv2.VideoCapture(4)
labels = ["C", "B", "F"]
m = MultiImagesMemmap(mode="w", name="mycamera3", labels=labels)
print(m.get_labels())
while True:
_, image0 = v0.read()
_, image1 = v1.read()
data = np.concatenate([image0, image1], axis=2)
m.write(data)
cv2.imshow("Writer0", image0)
cv2.imshow("Writer1", image1)
cv2.waitKey(1)
def main():
# create and set debugger: 0-DEBUG, 1-INFO, 2-WARNING, 3-ERROR, 4-FATAL_ERROR
main_debugger = Debugger()
# Initialize data capture ros node
rospy.init_node('data_capture_node', anonymous=True)
rospy.set_param('/data_capture/debug', 0)
setProcessName("data_capture_node")
rate = 30 #args.rate
r = rospy.Rate(hz=rate) # Set ros node rate
# Get current date
date = datetime.date.today().strftime("%m-%d-%y")
# Get base path
base_path = None
device = None
sub_folder_path = "data"
usb_devices = get_mount_points() # Get connected usb devices
if len(usb_devices):
base_path = usb_devices[-1][
-1] # Get absolute path to root in usb device
device = usb_devices[-1][0] # Get usb device
base_path = os.path.join(base_path, "data_capture-{}".format(date))
main_debugger.debugger(DEBUG_LEVEL_0,
"USB Detected: {}".format(device),
log_type='info')
# If path already exits then rename destination folder with new index
if base_path is not None:
if os.path.isdir(base_path):
i = 1
axu_dest = base_path + "({})".format(i)
while (os.path.isdir(axu_dest)):
i += 1
axu_dest = base_path + "({})".format(i)
base_path = axu_dest
# Create subfolder to save images
sub_folder_path = os.path.join(base_path, sub_folder_path)
if not os.path.isdir(sub_folder_path):
os.makedirs(str(sub_folder_path))
try: # Create data.csv headers if it not exists
if base_path is not None:
main_debugger.debugger(DEBUG_LEVEL_0,
"Destination folder: {}".format(base_path),
log_type='info')
csv_file, log_msg = create_folder_csv_4data_capture(
dest_folder=base_path, )
else:
csv_file, log_msg = None, "USB device no found or it may have been disconnected"
main_debugger.debugger(DEBUG_LEVEL_0, log_msg, log_type='warn')
except Exception as err:
csv_file, log_msg = None, "Error creating csv file"
main_debugger.debugger(DEBUG_LEVEL_0, err, log_type='err')
# Initialize memmap variable and wait for data
video_map = MultiImagesMemmap(mode="r",
name="main_stream",
memmap_path=os.getenv("MEMMAP_PATH", "/tmp"))
video_map.wait_until_available() #initialize and find video data
main_debugger.debugger(DEBUG_LEVEL_0,
"Memmap video data ready!",
log_type='info')
# Get cameras label and ports
cam_labels = read_cam_ports(os.environ.get("CAM_PORTS_PATH"))
try: # Wait for cameras status response
main_debugger.debugger(DEBUG_LEVEL_0,
"Waiting for cameras status response",
log_type='info')
rospy.wait_for_service('video_mapping/cameras_status',
3.0 * len(cam_labels))
except (rospy.ServiceException, rospy.ROSException), e:
main_debugger.debugger(DEBUG_LEVEL_0,
"Did not get cameras response \
status",
log_type='err')
return 1
def main():
# For LOCAL_RUN==2, get the data folder
folder_path = str(os.getenv(key="LOCAL_DATA_PATH", default=""))
# Enable(1)/Disable(0) local run
LOCAL_RUN = int(os.getenv(key="LOCAL_RUN", default=0))
# Start rosnode
rospy.init_node('video_mapping_node',
anonymous=True) # Node initialization
rospy.set_param('/video_mapping/debug', 0) # Set node debugger
setProcessName("video_mapping_node") # Set video mapping process name
r = rospy.Rate(hz=30) # Set node rate
# Initiate CameraSupervisor Class that handles the threads that reads the cameras
cameras_supervisor = CamerasSupervisor()
cam_labels = cameras_supervisor.cameras_labels.keys()
# video mapping for raw images
video_map = MultiImagesMemmap(memmap_path=os.getenv(key="MEMMAP_PATH",
default="/tmp"),
labels=cam_labels,
name="main_stream",
axis=1,
mode="w")
# Setting debuggers
main_debugger = Debugger() # Debugger that logs properly in stdout
main_debugger.debugger(DEBUG_LEVEL_0, "Initiating main loop")
# Calibration variables
intrinsic_calibration = load_intrinsic_calibration(
abs_path=os.path.dirname(os.getenv(key="CAM_PORTS_PATH")),
file_name="cam_intrinsic_{}X{}.yaml".format(
int(os.environ.get("VIDEO_WIDTH", 640)),
int(os.environ.get("VIDEO_HEIGHT", 360))))
extrinsic_calibrations = {
key: load_extrinsic_calibration(abs_path=os.path.join(
os.path.dirname(os.getenv(
key="CAM_PORTS_PATH")), "{}_extrinsic.yaml".format(key)))
for key in cam_labels
}
# Local launch variables
LOCAL_WIN_NAME = "Local_visualizer"
local_pause = False
local_cam_idx = 1
# Camera index
local_intrinsic = True # Enable/Disable intrinsic calibration
# Stitcher variables
local_stitcher = False # Enable/Disable local stitcher
stitcher_conf_path = save_path = os.path.join(
os.path.dirname(os.getenv(key="CAM_PORTS_PATH")),
'Stitcher_config.pkl')
while not rospy.is_shutdown():
if LOCAL_RUN != 2 and not local_pause: # Read into a list all images read from the threads
images = list(
map(lambda o: o.image, cameras_supervisor.camera_handlers))
# Create dictionary of images with keys as cameras labels
images_dic = dict([(label, img)
for img, label in zip(images, cam_labels)])
elif not local_pause: # Read from folder data
if not 'DataReader' in locals():
DataReader = data_reader()
DataReader.load_data(path=folder_path)
main_debugger.debugger(
DEBUG_LEVEL_0,
"Data loaded from folder:{}".format(folder_path))
print("\n")
print(DataReader)
print("\n")
idx_capture = 0
idx_time = 0
images = list([
cv2.imread(
os.path.join(
folder_path, "data",
DataReader.get_image(timestamp_idx=idx_time,
camera_idx=idx_cam,
capture_idx=idx_capture)))
for idx_cam in range(0, len(DataReader.camera_labels))
])
# Creates dictionary of images with keys as cameras labels
images_dic = dict([
(label, img)
for img, label in zip(images, DataReader.camera_labels.keys())
])
# Start from begining again assigning capture index and timestamp index
if idx_time < len(DataReader.timestamps[idx_capture]) - 1:
idx_time += 1
else:
idx_capture = idx_capture + 1 if idx_capture < len(
DataReader.images) - 1 else 0
idx_time = 0
DataReader_porc = round(
idx_time / float(len(DataReader.timestamps[idx_capture])) *
100., 2)
data_status = True
for img in images:
if img is None:
main_debugger.debugger(DEBUG_LEVEL_1,
"Image with None type data",
log_type="err")
data_status = False
break
if not data_status: continue
# Concatenate all list images in one big 3D matrix and write them into memory
video_map.write(np.concatenate(images, axis=1))
# Create stitcher object if it doesnt exist
if not 'CamsSticher' in locals():
CamsSticher = Stitcher(images_dic=images_dic, super_mode=False)
CamsSticher = CamsSticher.load_stitcher(
load_path=stitcher_conf_path)
# ---------------------------------------------------------------------
# Visual debugging - Visual debugging - Visual debugging - Visual debug
if LOCAL_RUN:
cam_key = "CAM{}".format(local_cam_idx)
img = images_dic[cam_key]
# If intrinsic calibration available
if intrinsic_calibration["mtx"] is not None and local_intrinsic:
# Undistord the image
img = cv2.undistort(src=img,
cameraMatrix=intrinsic_calibration["mtx"],
distCoeffs=intrinsic_calibration["dist"])
# If extrinsic calibration available
if extrinsic_calibrations[cam_key]["M"] is not None:
draw_extrinsic(
img_src=img,
src_pts=extrinsic_calibrations[cam_key]["src_pts"])
# Print some info in image
srt2print = ["{}".format(cam_key)]
if LOCAL_RUN == 2:
time = float(
DataReader.timestamps[idx_capture][idx_time]) / 1000.
srt2print += [
"Capture: {}/{}".format(idx_capture + 1,
len(DataReader.images)),
"{}".format(datetime.fromtimestamp(time)),
"%{}".format(DataReader_porc)
]
print_list_text(img,
str_list=srt2print,
origin=(10, 20),
color=(0, 0, 255),
line_break=22,
thickness=1,
fontScale=0.5)
if local_stitcher: # Show stitcher
Stitcher_img = CamsSticher.stitch(images_dic=images_dic)
cv2.imshow(LOCAL_WIN_NAME + "_stitcher", Stitcher_img)
cv2.imshow(LOCAL_WIN_NAME, img)
# Show image
key = cv2.waitKey(30) # Capture user key
if key == 173 or key == 98: # (-) If pressed go to previous camera
if local_cam_idx != 1: local_cam_idx -= 1
elif key == 171 or key == 110: # (+) If pressed go to next camera
if local_cam_idx < len(images): local_cam_idx += 1
elif key == 116: # (T) If pressed calibrate stitcher
CamsSticher.calibrate_stitcher(images_dic=images_dic,
save_path=stitcher_conf_path)
# cv2.imshow("Stitcher_result", CamsSticher.stitch(images_dic=images_dic))
elif key == 115: # (S) If pressed save image current capture
re_path = os.getenv(key="CALIBRATION_PATH")
pic_idx = 0
if not os.path.isdir(re_path): os.mkdir(re_path)
abs_path = "{}/picture_{}({}).jpg".format(
re_path, cam_key, pic_idx)
while os.path.isfile(abs_path):
pic_idx += 1
abs_path = "{}/picture_{}({}).jpg".format(
re_path, cam_key, pic_idx)
try:
cv2.imwrite(filename=abs_path, img=images[local_cam_idx])
main_debugger.debugger(
DEBUG_LEVEL_0,
"Image saved at {}".format(abs_path),
log_type="info")
except:
main_debugger.debugger(
DEBUG_LEVEL_0,
"Saving image ar {}".format(abs_path),
log_type="err")
elif key == 113: # (Q) If pressed then quit/restrat node
exit()
elif key == 105: # (I) If pressed perform intrinsic camera calibration
# Perform intrinsic calibration from image gallery
intrinsic_calibration = perform_intrinsic_calibration(
abs_path=os.getenv(key="CALIBRATION_PATH"), n_x=6, n_y=4)
# Saves intrinsic calibration from image gallery
file_name = save_intrinsic_calibration(
dest_path=os.path.dirname(os.getenv(key="CAM_PORTS_PATH")),
intrinsic_calibration=intrinsic_calibration)
intrinsic_calibration = load_intrinsic_calibration(
abs_path=os.path.dirname(os.getenv(key="CAM_PORTS_PATH")),
file_name=file_name)
# Validate calibration
validate_intrinsic_calibration(
abs_path=os.getenv(key="CALIBRATION_PATH"),
intrinsic_calibration=intrinsic_calibration)
elif key == 101: # (E) If pressed perform Extrinsic camera calibration
cam_extrinsic = perform_extrinsic_calibration(
img_src=img, WIN_NAME=LOCAL_WIN_NAME)
save_extrinsic_calibration(
file_path=os.path.dirname(os.getenv(key="CAM_PORTS_PATH")),
file_name="{}_extrinsic.yaml".format(cam_key),
extrinsic_calibration=cam_extrinsic)
extrinsic_calibrations[cam_key] = cam_extrinsic
elif key == 100: # (D) If pressed start/Stop data capture
local_data_capture_pub = rospy.Publisher(
"MotionTestTrack/data_capture/capture", Bool, queue_size=2)
local_data_capture_pub.publish(True)
elif key == 185 and LOCAL_RUN == 2: # (9) If pressed Increment capture
idx_capture = idx_capture + 1 if idx_capture < len(
DataReader.images) - 1 else 0
idx_time = 0
elif key == 182 and LOCAL_RUN == 2: # (6) If pressed Increase capture
idx_capture = idx_capture - 1 if idx_capture >= 0 else idx_capture
idx_time = 0
elif key == 32: # (Space bar) If pressed stop capture
local_pause = not local_pause
elif key != -1: # No key command
print("Command or key action no found: {}".format(key))
#6 ---------------------------------------------------------------------
# Suspend execution of R expressions for a specified time interval.
r.sleep()
def detection(model, config):
print("> Building Graph")
# tf Session Config
tf_config = tf.ConfigProto(allow_soft_placement=True)
tf_config.gpu_options.allow_growth = True
tf_config.gpu_options.per_process_gpu_memory_fraction = 0.1
detection_graph = model.detection_graph
category_index = model.category_index
with detection_graph.as_default():
with tf.Session(graph=detection_graph, config=tf_config) as sess:
# start Videostream
# vs = WebcamVideoStream(config.VIDEO_INPUT,config.WIDTH,config.HEIGHT).start()
vs = MultiImagesMemmap(mode="r",
name="main_stream",
memmap_path=os.getenv(
"MEMMAP_PATH", "/tmp"))
vs.wait_until_available() #initialize and find video data
# Define Input and Ouput tensors
tensor_dict = model.get_tensordict([
'num_detections', 'detection_boxes', 'detection_scores',
'detection_classes', 'detection_masks'
])
image_tensor = detection_graph.get_tensor_by_name('image_tensor:0')
score_out = detection_graph.get_tensor_by_name(
'Postprocessor/convert_scores:0')
expand_out = detection_graph.get_tensor_by_name(
'Postprocessor/ExpandDims_1:0')
score_in = detection_graph.get_tensor_by_name(
'Postprocessor/convert_scores_1:0')
expand_in = detection_graph.get_tensor_by_name(
'Postprocessor/ExpandDims_1_1:0')
# Threading
score = model.score
expand = model.expand
gpu_worker = SessionWorker("GPU", detection_graph, tf_config)
cpu_worker = SessionWorker("CPU", detection_graph, tf_config)
gpu_opts = [score_out, expand_out]
cpu_opts = [
tensor_dict['detection_boxes'],
tensor_dict['detection_scores'],
tensor_dict['detection_classes'], tensor_dict['num_detections']
]
gpu_counter = 0
cpu_counter = 0
fps = FPS2(config.FPS_INTERVAL).start()
print('> Starting Detection')
frame = vs.read("C")
# frame = vs.read()
h, w, _ = frame.shape
vs.real_width, vs.real_height = w, h
while True:
# Detection
# split model in seperate gpu and cpu session threads
masks = None # No Mask Detection possible yet
if gpu_worker.is_sess_empty():
# read video frame, expand dimensions and convert to rgb
frame = vs.read("C")
# frame = vs.read()
# put new queue
image_expanded = np.expand_dims(cv2.cvtColor(
frame, cv2.COLOR_BGR2RGB),
axis=0)
gpu_feeds = {image_tensor: image_expanded}
if config.VISUALIZE:
gpu_extras = frame # for visualization frame
else:
gpu_extras = None
gpu_worker.put_sess_queue(gpu_opts, gpu_feeds, gpu_extras)
g = gpu_worker.get_result_queue()
if g is None:
# gpu thread has no output queue. ok skip, let's check cpu thread.
gpu_counter += 1
else:
# gpu thread has output queue.
gpu_counter = 0
score, expand, frame = g["results"][0], g["results"][1], g[
"extras"]
if cpu_worker.is_sess_empty():
# When cpu thread has no next queue, put new queue.
# else, drop gpu queue.
cpu_feeds = {score_in: score, expand_in: expand}
cpu_extras = frame
cpu_worker.put_sess_queue(cpu_opts, cpu_feeds,
cpu_extras)
c = cpu_worker.get_result_queue()
if c is None:
# cpu thread has no output queue. ok, nothing to do. continue
cpu_counter += 1
continue # If CPU RESULT has not been set yet, no fps update
else:
cpu_counter = 0
boxes, scores, classes, num, frame = c["results"][0], c[
"results"][1], c["results"][2], c["results"][3], c[
"extras"]
# reformat detection
num = int(num)
boxes = np.squeeze(boxes)
classes = np.squeeze(classes).astype(np.uint8)
scores = np.squeeze(scores)
# Visualization
# print frame.shape
if frame is not None:
vis = vis_detection(frame.copy(), boxes, classes,
scores, masks, category_index,
fps.fps_local(), config.VISUALIZE,
config.DET_INTERVAL, config.DET_TH,
config.MAX_FRAMES,
fps._glob_numFrames,
config.OD_MODEL_NAME)
if not vis:
break
fps.update()
# End everything
# vs.stop()
fps.stop()
if config.SPLIT_MODEL:
gpu_worker.stop()
cpu_worker.stop()
import cv2
from easy_memmap import EasyMemmap, MultiImagesMemmap
import time
m = MultiImagesMemmap(mode="r")
print("Available streams: {}".format(EasyMemmap.get_memmap_files()))
m.wait_until_available("mycamera3")
print("Labels: {}".format(m.get_labels()))
while True:
image0 = m.read("C")
image1 = m.read("B")
cv2.imshow("Reader0", image0)
cv2.imshow("Reader1", image1)
cv2.waitKey(1)