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()
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()
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
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)